1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
| """
Multistatic UWB 性能分析
对比单基站 vs 多基站的检测能力
核心物理优势：
1. 空间分集：多个视角观察同一目标
2. 频率分集：不同路径的衰落不相关
3. 角度分辨率：通过三角测量提高定位精度
4. 冗余性：一个节点被遮挡时，其他节点仍可工作
"""
import numpy as np
from typing import List, Tuple
class UWBNode:
"""UWB 节点定义"""
def __init__(self, node_id: str, position: Tuple[float, float],
is_tx: bool = True, is_rx: bool = True):
self.node_id = node_id
self.position = position
self.is_tx = is_tx
self.is_rx = is_rx
class MultistaticUWBSystem:
"""多基站 UWB 系统"""
def __init__(self, nodes: List[UWBNode]):
self.nodes = nodes
self.links = self._compute_links()
def _compute_links(self):
"""计算所有可行的 Tx-Rx 链路"""
links = []
tx_nodes = [n for n in self.nodes if n.is_tx]
rx_nodes = [n for n in self.nodes if n.is_rx]
for tx in tx_nodes:
for rx in rx_nodes:
if tx.node_id != rx.node_id:
distance = np.linalg.norm(
np.array(tx.position) - np.array(rx.position)
)
links.append({
'tx': tx,
'rx': rx,
'distance': distance
})
return links
def locate_target(self, target_pos: Tuple[float, float],
noise_std: float = 0.05) -> Tuple[float, float]:
"""
使用多基站链路定位目标
通过多个 Tx-Rx-Target 的双程距离测量
使用最小二乘法估计目标位置
Args:
target_pos: 目标真实位置 (x, y)
noise_std: 测距噪声标准差 (m)
Returns:
estimated_pos: 估计位置 (x, y)
"""
target = np.array(target_pos)
measurements = []
for link in self.links:
d_tx_target = np.linalg.norm(
np.array(link['tx'].position) - target
)
d_target_rx = np.linalg.norm(
target - np.array(link['rx'].position)
)
bistatic_range = d_tx_target + d_target_rx
noisy_range = bistatic_range + np.random.normal(
0, noise_std
)
measurements.append({
'tx_pos': np.array(link['tx'].position),
'rx_pos': np.array(link['rx'].position),
'bistatic_range': noisy_range
})
est_pos = self._least_squares_locate(
measurements, initial_guess=target_pos
)
return tuple(est_pos)
def _least_squares_locate(self, measurements,
initial_guess, max_iter=20):
"""
非线性最小二乘法定位
使用 Gauss-Newton 迭代
"""
pos = np.array(initial_guess, dtype=float)
for _ in range(max_iter):
H = np.zeros((len(measurements), 2))
residual = np.zeros(len(measurements))
for i, m in enumerate(measurements):
d_tx = np.linalg.norm(m['tx_pos'] - pos)
d_rx = np.linalg.norm(pos - m['rx_pos'])
d_total = d_tx + d_rx
if d_total < 1e-10:
continue
grad_tx = (pos - m['tx_pos']) / (d_tx + 1e-10)
grad_rx = (m['rx_pos'] - pos) / (d_rx + 1e-10)
grad = grad_tx + grad_rx
H[i, :] = grad
residual[i] = m['bistatic_range'] - d_total
try:
delta = np.linalg.lstsq(H, residual, rcond=None)[0]
pos += delta
if np.linalg.norm(delta) < 1e-6:
break
except np.linalg.LinAlgError:
break
return pos
def assess_coverage(self, cabin_width=1.5, cabin_length=2.5,
grid_res=0.05):
"""
评估座舱覆盖度
检查每个位置至少被多少条链路覆盖
"""
x = np.arange(0, cabin_length, grid_res)
y = np.arange(0, cabin_width, grid_res)
coverage_map = np.zeros((len(y), len(x)))
for i, yi in enumerate(y):
for j, xj in enumerate(x):
count = 0
for link in self.links:
d_tx = np.linalg.norm(
np.array(link['tx'].position) - np.array([xj, yi])
)
d_rx = np.linalg.norm(
np.array([xj, yi]) - np.array(link['rx'].position)
)
if d_tx + d_rx < link['distance'] + 3.0:
count += 1
coverage_map[i, j] = count
return coverage_map
if __name__ == "__main__":
nodes = [
UWBNode("front_left", (0.0, 0.0)),
UWBNode("front_right", (0.0, 1.5)),
UWBNode("rear_left", (2.5, 0.0)),
UWBNode("rear_right", (2.5, 1.5)),
UWBNode("overhead", (1.25, 0.75)),
]
system = MultistaticUWBSystem(nodes)
print(f"=== 多基站 UWB 系统配置 ===")
print(f" 节点数: {len(nodes)}")
print(f" 链路数: {len(system.links)}")
print()
np.random.seed(42)
test_targets = [
("后座中间（儿童位置）", (1.8, 0.75)),
("驾驶座", (0.3, 0.3)),
("副驾驶座", (0.3, 1.2)),
("后座左侧", (1.8, 0.3)),
("后备箱", (2.3, 0.75)),
]
print("=== 定位精度测试 ===")
for name, true_pos in test_targets:
est_pos = system.locate_target(true_pos, noise_std=0.03)
error = np.linalg.norm(
np.array(true_pos) - np.array(est_pos)
)
print(f" {name}:")
print(f" 真实: ({true_pos[0]:.2f}, {true_pos[1]:.2f})")
print(f" 估计: ({est_pos[0]:.2f}, {est_pos[1]:.2f})")
print(f" 误差: {error*100:.1f} cm")
print()
coverage = system.assess_coverage()
print(f"=== 覆盖度统计 ===")
print(f" 最小覆盖链路数: {coverage.min():.0f}")
print(f" 最大覆盖链路数: {coverage.max():.0f}")
print(f" 平均覆盖链路数: {coverage.mean():.1f}")
print(f" 零覆盖区域比例: "
f"{np.mean(coverage == 0)*100:.1f}%")
|
