静磁场仿真-主题059_磁导航与定位-磁导航与定位
磁导航与定位技术利用环境中分布的磁场特征或人工布置的磁标，通过测量磁场强度和方向来确定目标的位置和姿态。该技术广泛应用于室内定位、机器人导航、医疗设备追踪、地下管线探测等领域。本教程将系统讲解磁场指纹地图构建、磁定位算法、多传感器融合、精度分析和干扰抑制等核心技术。永磁体产生的磁场可用磁偶极子模型描述：B⃗(r⃗)=μ04πr3[3(m⃗⋅r^)r^−m⃗]\vec{B}(\vec{r}) = \
·
主题059：磁导航与定位
场景描述
磁导航与定位技术利用环境中分布的磁场特征或人工布置的磁标，通过测量磁场强度和方向来确定目标的位置和姿态。该技术广泛应用于室内定位、机器人导航、医疗设备追踪、地下管线探测等领域。本教程将系统讲解磁场指纹地图构建、磁定位算法、多传感器融合、精度分析和干扰抑制等核心技术。
数学/物理模型
1. 磁偶极子磁场模型
永磁体产生的磁场可用磁偶极子模型描述：
B⃗(r⃗)=μ04πr3[3(m⃗⋅r^)r^−m⃗]\vec{B}(\vec{r}) = \frac{\mu_0}{4\pi r^3} [3(\vec{m} \cdot \hat{r})\hat{r} - \vec{m}]B(r)=4πr3μ0[3(m⋅r^)r^−m]
其中：
- B⃗\vec{B}B：磁感应强度 (T)
- μ0=4π×10−7\mu_0 = 4\pi \times 10^{-7}μ0=4π×10−7 H/m：真空磁导率
- m⃗\vec{m}m：磁矩 (A·m²)
- r⃗\vec{r}r：距离矢量
- rrr：距离
简化标量形式：
B=μ0m4πr3B = \frac{\mu_0 m}{4\pi r^3}B=4πr3μ0m
2. 磁场指纹定位原理
磁场指纹定位基于空间位置的磁场特征唯一性：
p^=argminp∈P∥Bmeas−Bfp(p)∥\hat{p} = \arg\min_{p \in P} \| B_{meas} - B_{fp}(p) \|p^=argp∈Pmin∥Bmeas−Bfp(p)∥
其中：
- p^\hat{p}p^：估计位置
- PPP：指纹地图位置集合
- BmeasB_{meas}Bmeas：实测磁场向量
- Bfp(p)B_{fp}(p)Bfp(p)：位置ppp处的指纹磁场
3. 卡尔曼滤波器
多传感器融合使用卡尔曼滤波进行状态估计：
预测步：
x^k∣k−1=Fkx^k−1∣k−1\hat{x}_{k|k-1} = F_k \hat{x}_{k-1|k-1}x^k∣k−1=Fkx^k−1∣k−1
Pk∣k−1=FkPk−1∣k−1FkT+QkP_{k|k-1} = F_k P_{k-1|k-1} F_k^T + Q_kPk∣k−1=FkPk−1∣k−1FkT+Qk
更新步：
Kk=Pk∣k−1HkT(HkPk∣k−1HkT+Rk)−1K_k = P_{k|k-1} H_k^T (H_k P_{k|k-1} H_k^T + R_k)^{-1}Kk=Pk∣k−1HkT(HkPk∣k−1HkT+Rk)−1
x^k∣k=x^k∣k−1+Kk(zk−Hkx^k∣k−1)\hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k (z_k - H_k \hat{x}_{k|k-1})x^k∣k=x^k∣k−1+Kk(zk−Hkx^k∣k−1)
Pk∣k=(I−KkHk)Pk∣k−1P_{k|k} = (I - K_k H_k) P_{k|k-1}Pk∣k=(I−KkHk)Pk∣k−1
4. 定位误差分析
定位误差与多个因素相关：
σp=∑i=1N(∂p∂Bi)2σBi2\sigma_p = \sqrt{\sum_{i=1}^{N} \left(\frac{\partial p}{\partial B_i}\right)^2 \sigma_{B_i}^2}σp=i=1∑N(∂Bi∂p)2σBi2
其中：
- σp\sigma_pσp：位置估计标准差
- σBi\sigma_{B_i}σBi：第iii个磁标的测量噪声
- NNN：磁标数量
5. RANSAC鲁棒估计
随机采样一致性算法用于干扰抑制：
- 随机选择最小样本集
- 计算模型参数
- 统计内点数量
- 迭代至找到最优模型
环境准备
依赖库安装
pip install numpy matplotlib scipy pillow
导入库
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Circle, FancyArrowPatch, Rectangle
from matplotlib.collections import LineCollection
import os
# 强制使用Agg后端
plt.switch_backend('Agg')
# 物理常数
MU_0 = 4 * np.pi * 1e-7 # 真空磁导率 (H/m)
完整代码实现
案例1：磁场指纹地图构建
def magnetic_fingerprint_map():
"""
磁场指纹地图构建
模拟室内环境中永磁体产生的磁场分布
"""
# 房间尺寸 (m)
room_width = 10
room_height = 8
# 创建网格
x = np.linspace(0, room_width, 100)
y = np.linspace(0, room_height, 80)
X, Y = np.meshgrid(x, y)
# 永磁体位置
magnets = [
{'pos': (1, 1), 'm': 0.5, 'name': '磁标A'},
{'pos': (9, 1), 'm': 0.6, 'name': '磁标B'},
{'pos': (1, 7), 'm': 0.4, 'name': '磁标C'},
{'pos': (9, 7), 'm': 0.55, 'name': '磁标D'},
{'pos': (5, 4), 'm': 0.8, 'name': '中心标'},
]
# 计算磁场
def dipole_field(X, Y, x0, y0, m):
r_x = X - x0
r_y = Y - y0
r = np.sqrt(r_x**2 + r_y**2)
r = np.maximum(r, 0.1)
B = MU_0 * m / (4 * np.pi * r**3) * 1e6
return B
# 计算总磁场
B_total = np.zeros_like(X)
for magnet in magnets:
B_magnet = dipole_field(X, Y, magnet['pos'][0], magnet['pos'][1], magnet['m'])
B_total += B_magnet
# 添加环境噪声
np.random.seed(42)
noise = np.random.normal(0, 0.5, B_total.shape)
B_measured = B_total + noise
# 采样点
sample_x = np.linspace(1, 9, 20)
sample_y = np.linspace(1, 7, 16)
sample_X, sample_Y = np.meshgrid(sample_x, sample_y)
# 插值获取采样点磁场值
from scipy.interpolate import griddata
points = np.column_stack((X.ravel(), Y.ravel()))
values = B_total.ravel()
sample_points = np.column_stack((sample_X.ravel(), sample_Y.ravel()))
B_samples = griddata(points, values, sample_points, method='linear')
# 可视化...
案例2：磁定位算法实现
def magnetic_localization_algorithm():
"""
磁定位算法实现
基于磁场指纹的最近邻定位算法
"""
# 房间尺寸
room_width = 10
room_height = 8
# 构建指纹数据库
x_grid = np.linspace(0, room_width, 50)
y_grid = np.linspace(0, room_height, 40)
# 磁标位置
beacons = [
{'pos': (2, 2), 'm': 1.0},
{'pos': (8, 2), 'm': 1.0},
{'pos': (2, 6), 'm': 1.0},
{'pos': (8, 6), 'm': 1.0},
]
# 生成指纹地图
fingerprint_db = []
positions_db = []
for x in x_grid:
for y in y_grid:
distances = [np.sqrt((x-b['pos'][0])**2 + (y-b['pos'][1])**2) for b in beacons]
B_measured = [MU_0 * b['m'] / (4 * np.pi * (d**3 + 0.01)) * 1e6
for b, d in zip(beacons, distances)]
B_measured = [b + np.random.normal(0, 0.1) for b in B_measured]
fingerprint_db.append(B_measured)
positions_db.append((x, y))
fingerprint_db = np.array(fingerprint_db)
positions_db = np.array(positions_db)
# 测试定位
t = np.linspace(0, 4*np.pi, 100)
true_x = 5 + 2 * np.cos(t)
true_y = 4 + 1.5 * np.sin(2*t)
# 最近邻定位
estimated_positions_knn = []
for tx, ty in zip(true_x, true_y):
distances = [np.sqrt((tx-b['pos'][0])**2 + (ty-b['pos'][1])**2) for b in beacons]
B_test = [MU_0 * b['m'] / (4 * np.pi * (d**3 + 0.01)) * 1e6 for b, d in zip(beacons, distances)]
B_test = [b + np.random.normal(0, 0.2) for b in B_test]
# 最近邻搜索
distances_to_db = np.linalg.norm(fingerprint_db - B_test, axis=1)
nearest_idx = np.argmin(distances_to_db)
estimated_positions_knn.append(positions_db[nearest_idx])
estimated_positions_knn = np.array(estimated_positions_knn)
# 计算定位误差
errors_knn = np.sqrt((estimated_positions_knn[:, 0] - true_x)**2 +
(estimated_positions_knn[:, 1] - true_y)**2)
# 可视化...
案例3：多传感器融合定位
def multisensor_fusion():
"""
多传感器融合定位
融合磁场、IMU和轮速计数据
"""
# 仿真参数
dt = 0.1 # 时间步长 (s)
T = 30 # 总时间 (s)
time = np.arange(0, T, dt)
n_steps = len(time)
# 真实轨迹
true_x = np.zeros(n_steps)
true_y = np.zeros(n_steps)
true_vx = np.zeros(n_steps)
true_vy = np.zeros(n_steps)
# 生成轨迹
for i in range(1, n_steps):
if i < n_steps // 3:
true_vx[i] = 0.5
true_vy[i] = 0
elif i < 2 * n_steps // 3:
true_vx[i] = 0.5 * np.cos((i - n_steps//3) * 0.05)
true_vy[i] = 0.5 * np.sin((i - n_steps//3) * 0.05)
else:
true_vx[i] = 0
true_vy[i] = 0.5
true_x[i] = true_x[i-1] + true_vx[i] * dt
true_y[i] = true_y[i-1] + true_vy[i] * dt
# 传感器噪声
imu_noise = 0.05
wheel_noise = 0.1
mag_noise = 0.5
# 磁标位置
beacons = [(2, 2), (8, 2), (2, 6), (8, 6)]
# IMU测量
ax_meas = np.gradient(true_vx, dt) + np.random.normal(0, imu_noise, n_steps)
ay_meas = np.gradient(true_vy, dt) + np.random.normal(0, imu_noise, n_steps)
# 轮速计测量
vx_wheel = true_vx + np.random.normal(0, wheel_noise, n_steps)
vy_wheel = true_vy + np.random.normal(0, wheel_noise, n_steps)
# 卡尔曼滤波器
x_est = np.zeros((n_steps, 4))
P_est = np.zeros((n_steps, 4, 4))
x_est[0] = [0, 0, 0, 0]
P_est[0] = np.eye(4) * 10
F = np.array([[1, 0, dt, 0],
[0, 1, 0, dt],
[0, 0, 1, 0],
[0, 0, 0, 1]])
Q = np.eye(4) * 0.01
R_wheel = np.eye(2) * wheel_noise**2
for k in range(1, n_steps):
# 预测
x_pred = F @ x_est[k-1]
P_pred = F @ P_est[k-1] @ F.T + Q
# 更新 (轮速计)
H_wheel = np.array([[0, 0, 1, 0],
[0, 0, 0, 1]])
z_wheel = np.array([vx_wheel[k], vy_wheel[k]])
y_wheel = z_wheel - H_wheel @ x_pred
S_wheel = H_wheel @ P_pred @ H_wheel.T + R_wheel
K_wheel = P_pred @ H_wheel.T @ np.linalg.inv(S_wheel)
x_pred = x_pred + K_wheel @ y_wheel
P_pred = (np.eye(4) - K_wheel @ H_wheel) @ P_pred
# 更新 (磁场)
if k % 5 == 0:
H_mag = np.array([[1, 0, 0, 0],
[0, 1, 0, 0]])
z_mag = np.array([true_x[k] + np.random.normal(0, 0.3),
true_y[k] + np.random.normal(0, 0.3)])
y_mag = z_mag - H_mag @ x_pred
S_mag = H_mag @ P_pred @ H_mag.T + np.eye(2) * 0.3**2
K_mag = P_pred @ H_mag.T @ np.linalg.inv(S_mag)
x_pred = x_pred + K_mag @ y_mag
P_pred = (np.eye(4) - K_mag @ H_mag) @ P_pred
x_est[k] = x_pred
P_est[k] = P_pred
# 计算误差
error_fusion = np.sqrt((x_est[:, 0] - true_x)**2 + (x_est[:, 1] - true_y)**2)
# 可视化...
案例4：定位精度分析
def localization_accuracy_analysis():
"""
定位精度分析
分析磁标数量、噪声水平、网格密度对精度的影响
"""
# 测试不同磁标数量
beacon_counts = [2, 3, 4, 5, 6]
accuracy_vs_beacons = []
room_width, room_height = 10, 8
for n_beacons in beacon_counts:
np.random.seed(n_beacons)
beacons = [(np.random.uniform(1, room_width-1), np.random.uniform(1, room_height-1))
for _ in range(n_beacons)]
test_points = [(5, 4), (3, 3), (7, 5), (2, 6), (8, 2)]
errors = []
for tp in test_points:
B_true = []
for beacon in beacons:
d = np.sqrt((tp[0] - beacon[0])**2 + (tp[1] - beacon[1])**2)
B = MU_0 * 1.0 / (4 * np.pi * (d**3 + 0.01)) * 1e6
B_true.append(B)
B_meas = [b + np.random.normal(0, 0.5) for b in B_true]
# 最近邻定位
min_error = float('inf')
for x in np.linspace(0, room_width, 30):
for y in np.linspace(0, room_height, 24):
B_candidate = []
for beacon in beacons:
d = np.sqrt((x - beacon[0])**2 + (y - beacon[1])**2)
B = MU_0 * 1.0 / (4 * np.pi * (d**3 + 0.01)) * 1e6
B_candidate.append(B)
error = np.linalg.norm(np.array(B_candidate) - np.array(B_meas))
if error < min_error:
min_error = error
best_pos = (x, y)
pos_error = np.sqrt((best_pos[0] - tp[0])**2 + (best_pos[1] - tp[1])**2)
errors.append(pos_error)
accuracy_vs_beacons.append(np.mean(errors) * 100)
# 可视化...
案例5：干扰抑制与鲁棒性
def interference_robustness():
"""
干扰抑制与鲁棒性
分析铁磁性干扰源对定位的影响及RANSAC抑制方法
"""
room_width, room_height = 10, 8
# 磁标位置
beacons = [(2, 2), (8, 2), (2, 6), (8, 6)]
# 干扰源位置
interferences = [
{'pos': (5, 4), 'strength': 5.0},
{'pos': (3, 5), 'strength': 2.0},
]
# 测试轨迹
t = np.linspace(0, 2*np.pi, 80)
true_x = 5 + 2.5 * np.cos(t)
true_y = 4 + 2 * np.sin(t)
# 模拟测量
def measure_field(x, y, beacons, interferences, noise=0.5):
B_meas = []
for beacon in beacons:
d = np.sqrt((x - beacon[0])**2 + (y - beacon[1])**2)
B_beacon = MU_0 * 1.0 / (4 * np.pi * (d**3 + 0.01)) * 1e6
B_interf = 0
for interf in interferences:
d_i = np.sqrt((x - interf['pos'][0])**2 + (y - interf['pos'][1])**2)
B_interf += MU_0 * interf['strength'] / (4 * np.pi * (d_i**3 + 0.01)) * 1e6
B_total = B_beacon + B_interf + np.random.normal(0, noise)
B_meas.append(B_total)
return B_meas
# RANSAC鲁棒定位
estimated_robust = []
for x, y in zip(true_x, true_y):
B_meas = measure_field(x, y, beacons, interferences, noise=0.5)
best_pos = (x, y)
min_median_error = float('inf')
for _ in range(20): # RANSAC迭代
selected = np.random.choice(len(beacons), 3, replace=False)
min_error = float('inf')
candidate_pos = (x, y)
for xi in np.linspace(0, room_width, 30):
for yi in np.linspace(0, room_height, 24):
B_candidate = [measure_field(xi, yi, [beacons[i]], [], noise=0)[0]
for i in selected]
B_meas_selected = [B_meas[i] for i in selected]
error = np.linalg.norm(np.array(B_candidate) - np.array(B_meas_selected))
if error < min_error:
min_error = error
candidate_pos = (xi, yi)
B_pred = measure_field(candidate_pos[0], candidate_pos[1], beacons, [], noise=0)
residuals = np.abs(np.array(B_pred) - np.array(B_meas))
median_residual = np.median(residuals)
if median_residual < min_median_error:
min_median_error = median_residual
best_pos = candidate_pos
estimated_robust.append(best_pos)
estimated_robust = np.array(estimated_robust)
# 计算误差
error_robust = np.sqrt((estimated_robust[:, 0] - true_x)**2 + (estimated_robust[:, 1] - true_y)**2)
# 可视化...
代码深度解析
磁偶极子磁场计算
def dipole_field(X, Y, x0, y0, m):
"""
计算磁偶极子产生的磁场
参数:
X, Y: 计算网格坐标
x0, y0: 磁偶极子位置
m: 磁矩 (A·m²)
返回:
B: 磁场强度 (μT)
原理:
磁偶极子磁场随距离立方衰减
B = μ₀m / (4πr³)
物理意义：
- 距离增加1倍，场强减小为1/8
- 磁矩越大，场强越强
- 近场需要更精确模型
"""
r_x = X - x0
r_y = Y - y0
r = np.sqrt(r_x**2 + r_y**2)
r = np.maximum(r, 0.1) # 避免除零和近场奇异
B = MU_0 * m / (4 * np.pi * r**3) * 1e6 # 转换为μT
return B
关键说明：
- 使用立方反比定律描述磁场衰减
- 限制最小距离避免数值奇异
- 单位转换为微特斯拉便于显示
最近邻定位算法
# 最近邻搜索
distances_to_db = np.linalg.norm(fingerprint_db - B_test, axis=1)
nearest_idx = np.argmin(distances_to_db)
estimated_position = positions_db[nearest_idx]
算法原理：
- 计算实测磁场与指纹库中所有条目的欧氏距离
- 选择距离最小的指纹对应的位置作为估计
- 简单高效，但精度受指纹密度限制
卡尔曼滤波实现
# 预测
x_pred = F @ x_est[k-1]
P_pred = F @ P_est[k-1] @ F.T + Q
# 更新
K = P_pred @ H.T @ np.linalg.inv(H @ P_pred @ H.T + R)
x_est = x_pred + K @ (z - H @ x_pred)
P_est = (np.eye(4) - K @ H) @ P_pred
滤波过程：
- 预测步：根据运动模型预测下一时刻状态
- 更新步：融合传感器测量修正预测
- 卡尔曼增益：自动权衡预测与测量的可信度
RANSAC鲁棒估计
for _ in range(20): # RANSAC迭代
# 随机选择子集
selected = np.random.choice(len(beacons), 3, replace=False)
# 用子集估计位置
candidate_pos = estimate_position(selected)
# 计算所有磁标的残差
residuals = compute_residuals(candidate_pos, all_beacons)
median_residual = np.median(residuals)
# 保留最优模型
if median_residual < min_median_error:
best_pos = candidate_pos
鲁棒性原理：
- 随机采样避免干扰磁标的影响
- 使用中位数而非均值评估模型
- 迭代寻找最一致的内点集合
"""
主题059: 磁导航与定位仿真
本仿真涵盖:
1. 磁场指纹地图构建
2. 磁定位算法实现
3. 多传感器融合定位
4. 定位精度分析
5. 干扰抑制与鲁棒性
"""
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.patches import Circle, FancyArrowPatch, Rectangle
from matplotlib.collections import LineCollection
import os
# 强制使用Agg后端
plt.switch_backend('Agg')
# 设置中文字体
plt.rcParams['font.sans-serif'] = ['SimHei', 'DejaVu Sans']
plt.rcParams['axes.unicode_minus'] = False
# 物理常数
MU_0 = 4 * np.pi * 1e-7 # 真空磁导率 (H/m)
def magnetic_fingerprint_map():
"""
案例1: 磁场指纹地图构建
模拟室内环境中永磁体产生的磁场分布，构建定位用的磁场指纹地图
"""
print("=" * 60)
print("案例1: 磁场指纹地图构建")
print("=" * 60)
# 房间尺寸 (m)
room_width = 10
room_height = 8
# 创建网格
x = np.linspace(0, room_width, 100)
y = np.linspace(0, room_height, 80)
X, Y = np.meshgrid(x, y)
# 永磁体位置 (房间角落和墙壁)
magnets = [
{'pos': (1, 1), 'm': 0.5, 'name': '磁标A'},
{'pos': (9, 1), 'm': 0.6, 'name': '磁标B'},
{'pos': (1, 7), 'm': 0.4, 'name': '磁标C'},
{'pos': (9, 7), 'm': 0.55, 'name': '磁标D'},
{'pos': (5, 4), 'm': 0.8, 'name': '中心标'},
]
# 计算磁场 (磁偶极子模型)
def dipole_field(X, Y, x0, y0, m):
"""计算磁偶极子在2D平面产生的磁场"""
r_x = X - x0
r_y = Y - y0
r = np.sqrt(r_x**2 + r_y**2)
r = np.maximum(r, 0.1) # 避免除零
# 磁场强度 (简化模型)
B = MU_0 * m / (4 * np.pi * r**3) * 1e6 # 转换为μT
return B
# 计算总磁场
B_total = np.zeros_like(X)
for magnet in magnets:
B_magnet = dipole_field(X, Y, magnet['pos'][0], magnet['pos'][1], magnet['m'])
B_total += B_magnet
# 添加环境噪声
np.random.seed(42)
noise = np.random.normal(0, 0.5, B_total.shape)
B_measured = B_total + noise
# 采样点 (用于构建指纹地图)
sample_x = np.linspace(1, 9, 20)
sample_y = np.linspace(1, 7, 16)
sample_X, sample_Y = np.meshgrid(sample_x, sample_y)
# 插值获取采样点磁场值
from scipy.interpolate import griddata
points = np.column_stack((X.ravel(), Y.ravel()))
values = B_total.ravel()
sample_points = np.column_stack((sample_X.ravel(), sample_Y.ravel()))
B_samples = griddata(points, values, sample_points, method='linear')
# 可视化
fig, axes = plt.subplots(2, 2, figsize=(14, 11))
# 图1: 磁场分布热力图
ax1 = axes[0, 0]
im1 = ax1.contourf(X, Y, B_total, levels=50, cmap='viridis')
ax1.set_xlabel('X 位置 (m)', fontsize=11)
ax1.set_ylabel('Y 位置 (m)', fontsize=11)
ax1.set_title('磁场指纹地图 (理论值)', fontsize=12, fontweight='bold')
ax1.set_aspect('equal')
# 标记磁标位置
for magnet in magnets:
ax1.plot(magnet['pos'][0], magnet['pos'][1], 'r*', markersize=15)
ax1.annotate(magnet['name'], xy=magnet['pos'], xytext=(5, 5),
textcoords='offset points', fontsize=9, color='white')
cbar1 = plt.colorbar(im1, ax=ax1)
cbar1.set_label('磁场强度 (μT)', fontsize=10)
# 图2: 带噪声的测量磁场
ax2 = axes[0, 1]
im2 = ax2.contourf(X, Y, B_measured, levels=50, cmap='viridis')
ax2.set_xlabel('X 位置 (m)', fontsize=11)
ax2.set_ylabel('Y 位置 (m)', fontsize=11)
ax2.set_title('磁场指纹地图 (含噪声)', fontsize=12, fontweight='bold')
ax2.set_aspect('equal')
for magnet in magnets:
ax2.plot(magnet['pos'][0], magnet['pos'][1], 'r*', markersize=15)
cbar2 = plt.colorbar(im2, ax=ax2)
cbar2.set_label('磁场强度 (μT)', fontsize=10)
# 图3: 采样点分布
ax3 = axes[1, 0]
scatter = ax3.scatter(sample_X.ravel(), sample_Y.ravel(),
c=B_samples, cmap='viridis', s=30, alpha=0.7)
ax3.set_xlabel('X 位置 (m)', fontsize=11)
ax3.set_ylabel('Y 位置 (m)', fontsize=11)
ax3.set_title('指纹采样点分布 (20×16网格)', fontsize=12, fontweight='bold')
ax3.set_aspect('equal')
ax3.grid(True, alpha=0.3)
for magnet in magnets:
ax3.plot(magnet['pos'][0], magnet['pos'][1], 'r*', markersize=15)
ax3.annotate(magnet['name'], xy=magnet['pos'], xytext=(5, 5),
textcoords='offset points', fontsize=9)
cbar3 = plt.colorbar(scatter, ax=ax3)
cbar3.set_label('磁场强度 (μT)', fontsize=10)
# 图4: 磁场梯度分析
ax4 = axes[1, 1]
grad_x, grad_y = np.gradient(B_total)
gradient_magnitude = np.sqrt(grad_x**2 + grad_y**2)
im4 = ax4.contourf(X, Y, gradient_magnitude, levels=50, cmap='hot')
ax4.set_xlabel('X 位置 (m)', fontsize=11)
ax4.set_ylabel('Y 位置 (m)', fontsize=11)
ax4.set_title('磁场梯度分布 (定位敏感度)', fontsize=12, fontweight='bold')
ax4.set_aspect('equal')
for magnet in magnets:
ax4.plot(magnet['pos'][0], magnet['pos'][1], 'w*', markersize=15)
cbar4 = plt.colorbar(im4, ax=ax4)
cbar4.set_label('梯度 (μT/m)', fontsize=10)
plt.suptitle('磁导航与定位 - 磁场指纹地图构建', fontsize=14, fontweight='bold', y=0.98)
plt.tight_layout()
plt.savefig('fig1_magnetic_fingerprint.png', dpi=150, bbox_inches='tight')
plt.close()
print(f"✓ 磁场指纹地图构建完成")
print(f" 磁标数量: {len(magnets)}")
print(f" 采样点数量: {len(B_samples)}")
print(f" 磁场范围: {B_total.min():.2f} - {B_total.max():.2f} μT")
return X, Y, B_total, magnets, sample_points, B_samples
def magnetic_localization_algorithm():
"""
案例2: 磁定位算法实现
实现基于磁场指纹的最近邻定位算法和三边定位算法
"""
print("\n" + "=" * 60)
print("案例2: 磁定位算法实现")
print("=" * 60)
# 房间尺寸
room_width = 10
room_height = 8
# 构建指纹数据库
x_grid = np.linspace(0, room_width, 50)
y_grid = np.linspace(0, room_height, 40)
# 磁标位置
beacons = [
{'pos': (2, 2), 'm': 1.0},
{'pos': (8, 2), 'm': 1.0},
{'pos': (2, 6), 'm': 1.0},
{'pos': (8, 6), 'm': 1.0},
]
# 生成指纹地图
fingerprint_db = []
positions_db = []
for x in x_grid:
for y in y_grid:
# 计算该位置到各磁标的距离
distances = [np.sqrt((x-b['pos'][0])**2 + (y-b['pos'][1])**2) for b in beacons]
# 模拟磁场测量值 (距离衰减模型)
B_measured = [MU_0 * b['m'] / (4 * np.pi * (d**3 + 0.01)) * 1e6 for b, d in zip(beacons, distances)]
# 添加测量噪声
B_measured = [b + np.random.normal(0, 0.1) for b in B_measured]
fingerprint_db.append(B_measured)
positions_db.append((x, y))
fingerprint_db = np.array(fingerprint_db)
positions_db = np.array(positions_db)
# 测试定位
# 真实轨迹
t = np.linspace(0, 4*np.pi, 100)
true_x = 5 + 2 * np.cos(t)
true_y = 4 + 1.5 * np.sin(2*t)
# 最近邻定位
estimated_positions_knn = []
for tx, ty in zip(true_x, true_y):
# 模拟测量值
distances = [np.sqrt((tx-b['pos'][0])**2 + (ty-b['pos'][1])**2) for b in beacons]
B_test = [MU_0 * b['m'] / (4 * np.pi * (d**3 + 0.01)) * 1e6 for b, d in zip(beacons, distances)]
B_test = [b + np.random.normal(0, 0.2) for b in B_test]
# 最近邻搜索
distances_to_db = np.linalg.norm(fingerprint_db - B_test, axis=1)
nearest_idx = np.argmin(distances_to_db)
estimated_positions_knn.append(positions_db[nearest_idx])
estimated_positions_knn = np.array(estimated_positions_knn)
# 计算定位误差
errors_knn = np.sqrt((estimated_positions_knn[:, 0] - true_x)**2 +
(estimated_positions_knn[:, 1] - true_y)**2)
# 可视化
fig, axes = plt.subplots(2, 2, figsize=(14, 11))
# 图1: 真实轨迹与估计轨迹对比
ax1 = axes[0, 0]
ax1.plot(true_x, true_y, 'b-', linewidth=2, label='真实轨迹', alpha=0.7)
ax1.plot(estimated_positions_knn[:, 0], estimated_positions_knn[:, 1],
'r--', linewidth=1.5, label='KNN估计轨迹', alpha=0.7)
# 绘制磁标
for beacon in beacons:
ax1.plot(beacon['pos'][0], beacon['pos'][1], 'g^', markersize=12, label='磁标' if beacon == beacons[0] else '')
ax1.set_xlabel('X 位置 (m)', fontsize=11)
ax1.set_ylabel('Y 位置 (m)', fontsize=11)
ax1.set_title('定位轨迹对比 (KNN算法)', fontsize=12, fontweight='bold')
ax1.legend(loc='upper right')
ax1.grid(True, alpha=0.3)
ax1.set_aspect('equal')
ax1.set_xlim(0, room_width)
ax1.set_ylim(0, room_height)
# 图2: 定位误差分布
ax2 = axes[0, 1]
ax2.hist(errors_knn * 100, bins=30, color='steelblue', edgecolor='black', alpha=0.7)
ax2.axvline(np.mean(errors_knn) * 100, color='red', linestyle='--', linewidth=2,
label=f'平均误差: {np.mean(errors_knn)*100:.1f} cm')
ax2.set_xlabel('定位误差 (cm)', fontsize=11)
ax2.set_ylabel('频次', fontsize=11)
ax2.set_title('定位误差分布', fontsize=12, fontweight='bold')
ax2.legend()
ax2.grid(True, alpha=0.3)
# 图3: 误差随时间变化
ax3 = axes[1, 0]
time_steps = np.arange(len(errors_knn))
ax3.plot(time_steps, errors_knn * 100, 'b-', linewidth=1, alpha=0.7)
ax3.axhline(np.mean(errors_knn) * 100, color='red', linestyle='--', linewidth=2,
label=f'平均误差: {np.mean(errors_knn)*100:.1f} cm')
ax3.fill_between(time_steps, 0, errors_knn * 100, alpha=0.3)
ax3.set_xlabel('时间步', fontsize=11)
ax3.set_ylabel('定位误差 (cm)', fontsize=11)
ax3.set_title('定位误差随时间变化', fontsize=12, fontweight='bold')
ax3.legend()
ax3.grid(True, alpha=0.3)
# 图4: CDF误差分布
ax4 = axes[1, 1]
sorted_errors = np.sort(errors_knn * 100)
cdf = np.arange(1, len(sorted_errors) + 1) / len(sorted_errors) * 100
ax4.plot(sorted_errors, cdf, 'b-', linewidth=2)
ax4.axvline(np.percentile(errors_knn * 100, 50), color='green', linestyle='--',
label=f'中位数: {np.percentile(errors_knn*100, 50):.1f} cm')
ax4.axvline(np.percentile(errors_knn * 100, 90), color='orange', linestyle='--',
label=f'90%分位: {np.percentile(errors_knn*100, 90):.1f} cm')
ax4.axvline(np.percentile(errors_knn * 100, 95), color='red', linestyle='--',
label=f'95%分位: {np.percentile(errors_knn*100, 95):.1f} cm')
ax4.set_xlabel('定位误差 (cm)', fontsize=11)
ax4.set_ylabel('累积概率 (%)', fontsize=11)
ax4.set_title('定位误差累积分布 (CDF)', fontsize=12, fontweight='bold')
ax4.legend()
ax4.grid(True, alpha=0.3)
plt.suptitle('磁导航与定位 - 定位算法性能分析', fontsize=14, fontweight='bold', y=0.98)
plt.tight_layout()
plt.savefig('fig2_localization_algorithm.png', dpi=150, bbox_inches='tight')
plt.close()
print(f"✓ 定位算法实现完成")
print(f" 平均定位误差: {np.mean(errors_knn)*100:.1f} cm")
print(f" 最大定位误差: {np.max(errors_knn)*100:.1f} cm")
print(f" 95%置信区间: {np.percentile(errors_knn*100, 95):.1f} cm")
return true_x, true_y, estimated_positions_knn, errors_knn
def multisensor_fusion():
"""
案例3: 多传感器融合定位
融合磁场、惯性传感器(IMU)和轮速计数据，提高定位精度
"""
print("\n" + "=" * 60)
print("案例3: 多传感器融合定位")
print("=" * 60)
# 仿真参数
dt = 0.1 # 时间步长 (s)
T = 30 # 总时间 (s)
time = np.arange(0, T, dt)
n_steps = len(time)
# 真实轨迹 (直线运动 + 转弯)
true_x = np.zeros(n_steps)
true_y = np.zeros(n_steps)
true_vx = np.zeros(n_steps)
true_vy = np.zeros(n_steps)
# 生成轨迹
for i in range(1, n_steps):
if i < n_steps // 3:
true_vx[i] = 0.5 # m/s
true_vy[i] = 0
elif i < 2 * n_steps // 3:
true_vx[i] = 0.5 * np.cos((i - n_steps//3) * 0.05)
true_vy[i] = 0.5 * np.sin((i - n_steps//3) * 0.05)
else:
true_vx[i] = 0
true_vy[i] = 0.5
true_x[i] = true_x[i-1] + true_vx[i] * dt
true_y[i] = true_y[i-1] + true_vy[i] * dt
# 传感器噪声参数
imu_noise = 0.05 # m/s²
wheel_noise = 0.1 # m/s
mag_noise = 0.5 # μT
# 磁标位置
beacons = [(2, 2), (8, 2), (2, 6), (8, 6)]
# IMU测量 (加速度)
ax_meas = np.gradient(true_vx, dt) + np.random.normal(0, imu_noise, n_steps)
ay_meas = np.gradient(true_vy, dt) + np.random.normal(0, imu_noise, n_steps)
# 轮速计测量
vx_wheel = true_vx + np.random.normal(0, wheel_noise, n_steps)
vy_wheel = true_vy + np.random.normal(0, wheel_noise, n_steps)
# 磁场测量
B_meas = []
for i in range(n_steps):
B_i = []
for beacon in beacons:
d = np.sqrt((true_x[i] - beacon[0])**2 + (true_y[i] - beacon[1])**2)
B = MU_0 * 1.0 / (4 * np.pi * (d**3 + 0.01)) * 1e6
B_i.append(B + np.random.normal(0, mag_noise))
B_meas.append(B_i)
B_meas = np.array(B_meas)
# 卡尔曼滤波器实现 (简化版)
# 状态: [x, y, vx, vy]
x_est = np.zeros((n_steps, 4))
P_est = np.zeros((n_steps, 4, 4))
# 初始化
x_est[0] = [0, 0, 0, 0]
P_est[0] = np.eye(4) * 10
# 状态转移矩阵
F = np.array([[1, 0, dt, 0],
[0, 1, 0, dt],
[0, 0, 1, 0],
[0, 0, 0, 1]])
# 过程噪声
Q = np.eye(4) * 0.01
# 测量噪声
R_mag = np.eye(4) * mag_noise**2
R_wheel = np.eye(2) * wheel_noise**2
# 卡尔曼滤波迭代
for k in range(1, n_steps):
# 预测
x_pred = F @ x_est[k-1]
P_pred = F @ P_est[k-1] @ F.T + Q
# 更新 (轮速计)
H_wheel = np.array([[0, 0, 1, 0],
[0, 0, 0, 1]])
z_wheel = np.array([vx_wheel[k], vy_wheel[k]])
y_wheel = z_wheel - H_wheel @ x_pred
S_wheel = H_wheel @ P_pred @ H_wheel.T + R_wheel
K_wheel = P_pred @ H_wheel.T @ np.linalg.inv(S_wheel)
x_pred = x_pred + K_wheel @ y_wheel
P_pred = (np.eye(4) - K_wheel @ H_wheel) @ P_pred
# 更新 (磁场 - 简化为位置观测)
# 使用磁场指纹进行位置修正
if k % 5 == 0: # 每5步进行一次磁场更新
# 简化的磁场位置观测
H_mag = np.array([[1, 0, 0, 0],
[0, 1, 0, 0]])
# 从磁场反推位置 (简化处理)
z_mag = np.array([true_x[k] + np.random.normal(0, 0.3),
true_y[k] + np.random.normal(0, 0.3)])
y_mag = z_mag - H_mag @ x_pred
S_mag = H_mag @ P_pred @ H_mag.T + np.eye(2) * 0.3**2
K_mag = P_pred @ H_mag.T @ np.linalg.inv(S_mag)
x_pred = x_pred + K_mag @ y_mag
P_pred = (np.eye(4) - K_mag @ H_mag) @ P_pred
x_est[k] = x_pred
P_est[k] = P_pred
# 仅使用轮速计的航位推算
x_dead = np.zeros((n_steps, 2))
x_dead[0] = [0, 0]
for k in range(1, n_steps):
x_dead[k, 0] = x_dead[k-1, 0] + vx_wheel[k] * dt
x_dead[k, 1] = x_dead[k-1, 1] + vy_wheel[k] * dt
# 计算误差
error_fusion = np.sqrt((x_est[:, 0] - true_x)**2 + (x_est[:, 1] - true_y)**2)
error_dead = np.sqrt((x_dead[:, 0] - true_x)**2 + (x_dead[:, 1] - true_y)**2)
# 可视化
fig, axes = plt.subplots(2, 2, figsize=(14, 11))
# 图1: 轨迹对比
ax1 = axes[0, 0]
ax1.plot(true_x, true_y, 'g-', linewidth=2, label='真实轨迹', alpha=0.8)
ax1.plot(x_dead[:, 0], x_dead[:, 1], 'r--', linewidth=1.5, label='航位推算', alpha=0.7)
ax1.plot(x_est[:, 0], x_est[:, 1], 'b-.', linewidth=1.5, label='融合定位', alpha=0.7)
# 标记磁标
for beacon in beacons:
ax1.plot(beacon[0], beacon[1], 'k^', markersize=10)
ax1.set_xlabel('X 位置 (m)', fontsize=11)
ax1.set_ylabel('Y 位置 (m)', fontsize=11)
ax1.set_title('多传感器融合定位轨迹', fontsize=12, fontweight='bold')
ax1.legend()
ax1.grid(True, alpha=0.3)
ax1.set_aspect('equal')
# 图2: 误差对比
ax2 = axes[0, 1]
ax2.plot(time, error_dead * 100, 'r-', linewidth=1.5, label='航位推算误差', alpha=0.7)
ax2.plot(time, error_fusion * 100, 'b-', linewidth=1.5, label='融合定位误差', alpha=0.7)
ax2.fill_between(time, 0, error_dead * 100, alpha=0.2, color='red')
ax2.fill_between(time, 0, error_fusion * 100, alpha=0.2, color='blue')
ax2.set_xlabel('时间 (s)', fontsize=11)
ax2.set_ylabel('定位误差 (cm)', fontsize=11)
ax2.set_title('定位误差对比', fontsize=12, fontweight='bold')
ax2.legend()
ax2.grid(True, alpha=0.3)
# 图3: 速度估计
ax3 = axes[1, 0]
ax3.plot(time, true_vx, 'g-', linewidth=2, label='真实Vx', alpha=0.8)
ax3.plot(time, vx_wheel, 'r--', linewidth=1, label='轮速计Vx', alpha=0.6)
ax3.plot(time, x_est[:, 2], 'b-.', linewidth=1.5, label='估计Vx', alpha=0.7)
ax3.set_xlabel('时间 (s)', fontsize=11)
ax3.set_ylabel('速度 (m/s)', fontsize=11)
ax3.set_title('X方向速度估计', fontsize=12, fontweight='bold')
ax3.legend()
ax3.grid(True, alpha=0.3)
# 图4: 误差统计
ax4 = axes[1, 1]
methods = ['航位推算', '融合定位']
mean_errors = [np.mean(error_dead) * 100, np.mean(error_fusion) * 100]
max_errors = [np.max(error_dead) * 100, np.max(error_fusion) * 100]
x_pos = np.arange(len(methods))
width = 0.35
bars1 = ax4.bar(x_pos - width/2, mean_errors, width, label='平均误差', color='steelblue')
bars2 = ax4.bar(x_pos + width/2, max_errors, width, label='最大误差', color='coral')
ax4.set_ylabel('误差 (cm)', fontsize=11)
ax4.set_title('定位误差统计对比', fontsize=12, fontweight='bold')
ax4.set_xticks(x_pos)
ax4.set_xticklabels(methods)
ax4.legend()
ax4.grid(True, alpha=0.3, axis='y')
# 添加数值标签
for bar in bars1:
height = bar.get_height()
ax4.annotate(f'{height:.1f}',
xy=(bar.get_x() + bar.get_width() / 2, height),
xytext=(0, 3),
textcoords="offset points",
ha='center', va='bottom', fontsize=9)
for bar in bars2:
height = bar.get_height()
ax4.annotate(f'{height:.1f}',
xy=(bar.get_x() + bar.get_width() / 2, height),
xytext=(0, 3),
textcoords="offset points",
ha='center', va='bottom', fontsize=9)
plt.suptitle('磁导航与定位 - 多传感器融合', fontsize=14, fontweight='bold', y=0.98)
plt.tight_layout()
plt.savefig('fig3_multisensor_fusion.png', dpi=150, bbox_inches='tight')
plt.close()
print(f"✓ 多传感器融合定位完成")
print(f" 航位推算平均误差: {np.mean(error_dead)*100:.1f} cm")
print(f" 融合定位平均误差: {np.mean(error_fusion)*100:.1f} cm")
print(f" 精度提升: {(1 - np.mean(error_fusion)/np.mean(error_dead))*100:.1f}%")
return time, true_x, true_y, x_est, error_fusion, error_dead
def localization_accuracy_analysis():
"""
案例4: 定位精度分析
分析不同因素对定位精度的影响
"""
print("\n" + "=" * 60)
print("案例4: 定位精度分析")
print("=" * 60)
# 测试不同磁标数量对精度的影响
beacon_counts = [2, 3, 4, 5, 6]
accuracy_vs_beacons = []
room_width, room_height = 10, 8
for n_beacons in beacon_counts:
# 随机放置磁标
np.random.seed(n_beacons)
beacons = [(np.random.uniform(1, room_width-1), np.random.uniform(1, room_height-1))
for _ in range(n_beacons)]
# 测试点
test_points = [(5, 4), (3, 3), (7, 5), (2, 6), (8, 2)]
errors = []
for tp in test_points:
# 真实磁场
B_true = []
for beacon in beacons:
d = np.sqrt((tp[0] - beacon[0])**2 + (tp[1] - beacon[1])**2)
B = MU_0 * 1.0 / (4 * np.pi * (d**3 + 0.01)) * 1e6
B_true.append(B)
# 添加噪声的测量值
B_meas = [b + np.random.normal(0, 0.5) for b in B_true]
# 最近邻定位 (简化)
min_error = float('inf')
for x in np.linspace(0, room_width, 30):
for y in np.linspace(0, room_height, 24):
B_candidate = []
for beacon in beacons:
d = np.sqrt((x - beacon[0])**2 + (y - beacon[1])**2)
B = MU_0 * 1.0 / (4 * np.pi * (d**3 + 0.01)) * 1e6
B_candidate.append(B)
error = np.linalg.norm(np.array(B_candidate) - np.array(B_meas))
if error < min_error:
min_error = error
best_pos = (x, y)
pos_error = np.sqrt((best_pos[0] - tp[0])**2 + (best_pos[1] - tp[1])**2)
errors.append(pos_error)
accuracy_vs_beacons.append(np.mean(errors) * 100) # cm
# 测试不同噪声水平对精度的影响
noise_levels = [0.1, 0.3, 0.5, 1.0, 2.0, 5.0] # μT
accuracy_vs_noise = []
beacons_fixed = [(2, 2), (8, 2), (2, 6), (8, 6)]
test_point = (5, 4)
for noise in noise_levels:
errors = []
for _ in range(50): # 多次测量取平均
# 真实磁场
B_true = []
for beacon in beacons_fixed:
d = np.sqrt((test_point[0] - beacon[0])**2 + (test_point[1] - beacon[1])**2)
B = MU_0 * 1.0 / (4 * np.pi * (d**3 + 0.01)) * 1e6
B_true.append(B)
# 添加噪声
B_meas = [b + np.random.normal(0, noise) for b in B_true]
# 最近邻定位
min_error = float('inf')
for x in np.linspace(3, 7, 20):
for y in np.linspace(2, 6, 16):
B_candidate = []
for beacon in beacons_fixed:
d = np.sqrt((x - beacon[0])**2 + (y - beacon[1])**2)
B = MU_0 * 1.0 / (4 * np.pi * (d**3 + 0.01)) * 1e6
B_candidate.append(B)
error = np.linalg.norm(np.array(B_candidate) - np.array(B_meas))
if error < min_error:
min_error = error
best_pos = (x, y)
pos_error = np.sqrt((best_pos[0] - test_point[0])**2 + (best_pos[1] - test_point[1])**2)
errors.append(pos_error)
accuracy_vs_noise.append(np.mean(errors) * 100) # cm
# 测试不同网格密度对精度的影响
grid_densities = [10, 20, 30, 40, 50] # 每维采样点数
accuracy_vs_grid = []
for density in grid_densities:
# 构建指纹地图
x_grid = np.linspace(0, room_width, density)
y_grid = np.linspace(0, room_height, int(density * 0.8))
fingerprint_db = []
positions_db = []
for x in x_grid:
for y in y_grid:
B_measured = []
for beacon in beacons_fixed:
d = np.sqrt((x - beacon[0])**2 + (y - beacon[1])**2)
B = MU_0 * 1.0 / (4 * np.pi * (d**3 + 0.01)) * 1e6
B_measured.append(B + np.random.normal(0, 0.5))
fingerprint_db.append(B_measured)
positions_db.append((x, y))
fingerprint_db = np.array(fingerprint_db)
positions_db = np.array(positions_db)
# 测试定位
B_test = []
for beacon in beacons_fixed:
d = np.sqrt((test_point[0] - beacon[0])**2 + (test_point[1] - beacon[1])**2)
B = MU_0 * 1.0 / (4 * np.pi * (d**3 + 0.01)) * 1e6
B_test.append(B + np.random.normal(0, 0.5))
distances = np.linalg.norm(fingerprint_db - B_test, axis=1)
nearest_idx = np.argmin(distances)
estimated_pos = positions_db[nearest_idx]
error = np.sqrt((estimated_pos[0] - test_point[0])**2 +
(estimated_pos[1] - test_point[1])**2)
accuracy_vs_grid.append(error * 100)
# 可视化
fig, axes = plt.subplots(2, 2, figsize=(14, 11))
# 图1: 磁标数量 vs 精度
ax1 = axes[0, 0]
ax1.plot(beacon_counts, accuracy_vs_beacons, 'bo-', linewidth=2, markersize=8)
ax1.set_xlabel('磁标数量', fontsize=11)
ax1.set_ylabel('平均定位误差 (cm)', fontsize=11)
ax1.set_title('定位精度 vs 磁标数量', fontsize=12, fontweight='bold')
ax1.grid(True, alpha=0.3)
# 图2: 噪声水平 vs 精度
ax2 = axes[0, 1]
ax2.semilogy(noise_levels, accuracy_vs_noise, 'ro-', linewidth=2, markersize=8)
ax2.set_xlabel('磁场测量噪声 (μT)', fontsize=11)
ax2.set_ylabel('平均定位误差 (cm)', fontsize=11)
ax2.set_title('定位精度 vs 测量噪声', fontsize=12, fontweight='bold')
ax2.grid(True, alpha=0.3)
# 图3: 网格密度 vs 精度
ax3 = axes[1, 0]
ax3.plot(grid_densities, accuracy_vs_grid, 'go-', linewidth=2, markersize=8)
ax3.set_xlabel('网格密度 (每维点数)', fontsize=11)
ax3.set_ylabel('定位误差 (cm)', fontsize=11)
ax3.set_title('定位精度 vs 指纹地图密度', fontsize=12, fontweight='bold')
ax3.grid(True, alpha=0.3)
# 图4: 综合精度热力图
ax4 = axes[1, 1]
# 生成不同位置的精度分布
x_test = np.linspace(1, 9, 40)
y_test = np.linspace(1, 7, 32)
X_test, Y_test = np.meshgrid(x_test, y_test)
accuracy_map = np.zeros_like(X_test)
for i in range(X_test.shape[0]):
for j in range(X_test.shape[1]):
pos = (X_test[i, j], Y_test[i, j])
# 计算到最近磁标的距离
min_dist = min([np.sqrt((pos[0] - b[0])**2 + (pos[1] - b[1])**2)
for b in beacons_fixed])
# 精度与距离相关 (简化模型)
accuracy_map[i, j] = 5 + 20 * min_dist + np.random.normal(0, 2)
im = ax4.contourf(X_test, Y_test, accuracy_map, levels=30, cmap='RdYlGn_r')
# 标记磁标
for beacon in beacons_fixed:
ax4.plot(beacon[0], beacon[1], 'w*', markersize=15, markeredgecolor='black')
ax4.set_xlabel('X 位置 (m)', fontsize=11)
ax4.set_ylabel('Y 位置 (m)', fontsize=11)
ax4.set_title('定位精度空间分布', fontsize=12, fontweight='bold')
ax4.set_aspect('equal')
cbar = plt.colorbar(im, ax=ax4)
cbar.set_label('定位误差 (cm)', fontsize=10)
plt.suptitle('磁导航与定位 - 定位精度分析', fontsize=14, fontweight='bold', y=0.98)
plt.tight_layout()
plt.savefig('fig4_accuracy_analysis.png', dpi=150, bbox_inches='tight')
plt.close()
print(f"✓ 定位精度分析完成")
print(f" 最优磁标数量: {beacon_counts[np.argmin(accuracy_vs_beacons)]} (误差: {min(accuracy_vs_beacons):.1f} cm)")
print(f" 噪声敏感度: 噪声每增加1μT, 误差增加约{(accuracy_vs_noise[-1]-accuracy_vs_noise[0])/(noise_levels[-1]-noise_levels[0]):.1f} cm")
return beacon_counts, accuracy_vs_beacons, noise_levels, accuracy_vs_noise
def interference_robustness():
"""
案例5: 干扰抑制与鲁棒性
分析铁磁性干扰源对定位的影响及抑制方法
"""
print("\n" + "=" * 60)
print("案例5: 干扰抑制与鲁棒性")
print("=" * 60)
# 场景设置
room_width, room_height = 10, 8
# 磁标位置
beacons = [(2, 2), (8, 2), (2, 6), (8, 6)]
# 干扰源位置 (铁磁性物体)
interferences = [
{'pos': (5, 4), 'strength': 5.0}, # 强干扰
{'pos': (3, 5), 'strength': 2.0}, # 中等干扰
]
# 测试轨迹
t = np.linspace(0, 2*np.pi, 80)
true_x = 5 + 2.5 * np.cos(t)
true_y = 4 + 2 * np.sin(t)
# 模拟测量 (含干扰)
def measure_field(x, y, beacons, interferences, noise=0.5):
"""模拟磁场测量"""
B_meas = []
for beacon in beacons:
# 磁标贡献
d = np.sqrt((x - beacon[0])**2 + (y - beacon[1])**2)
B_beacon = MU_0 * 1.0 / (4 * np.pi * (d**3 + 0.01)) * 1e6
# 干扰源贡献 (简化模型)
B_interf = 0
for interf in interferences:
d_i = np.sqrt((x - interf['pos'][0])**2 + (y - interf['pos'][1])**2)
B_interf += MU_0 * interf['strength'] / (4 * np.pi * (d_i**3 + 0.01)) * 1e6
B_total = B_beacon + B_interf + np.random.normal(0, noise)
B_meas.append(B_total)
return B_meas
# 无干扰情况下的定位
estimated_clean = []
for x, y in zip(true_x, true_y):
B_meas = measure_field(x, y, beacons, [], noise=0.5)
# 最近邻定位
min_error = float('inf')
best_pos = (x, y)
for xi in np.linspace(0, room_width, 40):
for yi in np.linspace(0, room_height, 32):
B_candidate = measure_field(xi, yi, beacons, [], noise=0)
error = np.linalg.norm(np.array(B_candidate) - np.array(B_meas))
if error < min_error:
min_error = error
best_pos = (xi, yi)
estimated_clean.append(best_pos)
estimated_clean = np.array(estimated_clean)
# 有干扰但无抑制
estimated_noisy = []
for x, y in zip(true_x, true_y):
B_meas = measure_field(x, y, beacons, interferences, noise=0.5)
min_error = float('inf')
best_pos = (x, y)
for xi in np.linspace(0, room_width, 40):
for yi in np.linspace(0, room_height, 32):
B_candidate = measure_field(xi, yi, beacons, [], noise=0)
error = np.linalg.norm(np.array(B_candidate) - np.array(B_meas))
if error < min_error:
min_error = error
best_pos = (xi, yi)
estimated_noisy.append(best_pos)
estimated_noisy = np.array(estimated_noisy)
# 有干扰 + RANSAC鲁棒定位
estimated_robust = []
for x, y in zip(true_x, true_y):
B_meas = measure_field(x, y, beacons, interferences, noise=0.5)
# RANSAC: 随机采样一致性
best_pos = (x, y)
min_median_error = float('inf')
for _ in range(20): # RANSAC迭代
# 随机选择3个磁标
selected = np.random.choice(len(beacons), 3, replace=False)
# 使用选中的磁标定位
min_error = float('inf')
candidate_pos = (x, y)
for xi in np.linspace(0, room_width, 30):
for yi in np.linspace(0, room_height, 24):
B_candidate = [measure_field(xi, yi, [beacons[i]], [], noise=0)[0]
for i in selected]
B_meas_selected = [B_meas[i] for i in selected]
error = np.linalg.norm(np.array(B_candidate) - np.array(B_meas_selected))
if error < min_error:
min_error = error
candidate_pos = (xi, yi)
# 计算所有磁标的残差
B_pred = measure_field(candidate_pos[0], candidate_pos[1], beacons, [], noise=0)
residuals = np.abs(np.array(B_pred) - np.array(B_meas))
median_residual = np.median(residuals)
if median_residual < min_median_error:
min_median_error = median_residual
best_pos = candidate_pos
estimated_robust.append(best_pos)
estimated_robust = np.array(estimated_robust)
# 计算误差
error_clean = np.sqrt((estimated_clean[:, 0] - true_x)**2 + (estimated_clean[:, 1] - true_y)**2)
error_noisy = np.sqrt((estimated_noisy[:, 0] - true_x)**2 + (estimated_noisy[:, 1] - true_y)**2)
error_robust = np.sqrt((estimated_robust[:, 0] - true_x)**2 + (estimated_robust[:, 1] - true_y)**2)
# 可视化
fig, axes = plt.subplots(2, 2, figsize=(14, 11))
# 图1: 干扰环境示意
ax1 = axes[0, 0]
# 绘制磁场分布 (含干扰)
x_grid = np.linspace(0, room_width, 80)
y_grid = np.linspace(0, room_height, 64)
X_grid, Y_grid = np.meshgrid(x_grid, y_grid)
B_field = np.zeros_like(X_grid)
for i in range(X_grid.shape[0]):
for j in range(X_grid.shape[1]):
B_field[i, j] = sum(measure_field(X_grid[i,j], Y_grid[i,j], beacons, interferences, noise=0))
im1 = ax1.contourf(X_grid, Y_grid, B_field, levels=40, cmap='viridis')
# 标记磁标
for beacon in beacons:
ax1.plot(beacon[0], beacon[1], 'g^', markersize=12, label='磁标' if beacon == beacons[0] else '')
# 标记干扰源
for i, interf in enumerate(interferences):
circle = Circle(interf['pos'], 0.3, color='red', alpha=0.5)
ax1.add_patch(circle)
ax1.annotate(f'干扰{i+1}', xy=interf['pos'], xytext=(5, 5),
textcoords='offset points', fontsize=9, color='white')
ax1.plot(true_x, true_y, 'w--', linewidth=2, label='真实轨迹')
ax1.set_xlabel('X 位置 (m)', fontsize=11)
ax1.set_ylabel('Y 位置 (m)', fontsize=11)
ax1.set_title('干扰环境下的磁场分布', fontsize=12, fontweight='bold')
ax1.legend(loc='upper right')
ax1.set_aspect('equal')
cbar1 = plt.colorbar(im1, ax=ax1)
cbar1.set_label('总磁场 (μT)', fontsize=10)
# 图2: 轨迹对比
ax2 = axes[0, 1]
ax2.plot(true_x, true_y, 'g-', linewidth=2, label='真实轨迹', alpha=0.8)
ax2.plot(estimated_clean[:, 0], estimated_clean[:, 1], 'b--',
linewidth=1.5, label='无干扰', alpha=0.7)
ax2.plot(estimated_noisy[:, 0], estimated_noisy[:, 1], 'r--',
linewidth=1.5, label='有干扰(无抑制)', alpha=0.7)
ax2.plot(estimated_robust[:, 0], estimated_robust[:, 1], 'm-.',
linewidth=1.5, label='有干扰(RANSAC)', alpha=0.7)
# 标记干扰源
for interf in interferences:
circle = Circle(interf['pos'], 0.3, color='red', alpha=0.3)
ax2.add_patch(circle)
ax2.set_xlabel('X 位置 (m)', fontsize=11)
ax2.set_ylabel('Y 位置 (m)', fontsize=11)
ax2.set_title('干扰抑制效果对比', fontsize=12, fontweight='bold')
ax2.legend()
ax2.grid(True, alpha=0.3)
ax2.set_aspect('equal')
# 图3: 误差对比
ax3 = axes[1, 0]
steps = np.arange(len(error_clean))
ax3.plot(steps, error_clean * 100, 'b-', linewidth=1.5, label='无干扰', alpha=0.7)
ax3.plot(steps, error_noisy * 100, 'r-', linewidth=1.5, label='有干扰(无抑制)', alpha=0.7)
ax3.plot(steps, error_robust * 100, 'm-', linewidth=1.5, label='有干扰(RANSAC)', alpha=0.7)
ax3.fill_between(steps, 0, error_noisy * 100, alpha=0.2, color='red')
ax3.fill_between(steps, 0, error_robust * 100, alpha=0.2, color='magenta')
ax3.set_xlabel('轨迹点', fontsize=11)
ax3.set_ylabel('定位误差 (cm)', fontsize=11)
ax3.set_title('定位误差对比', fontsize=12, fontweight='bold')
ax3.legend()
ax3.grid(True, alpha=0.3)
# 图4: 误差统计
ax4 = axes[1, 1]
methods = ['无干扰', '有干扰\n(无抑制)', '有干扰\n(RANSAC)']
mean_errors = [np.mean(error_clean) * 100, np.mean(error_noisy) * 100, np.mean(error_robust) * 100]
max_errors = [np.max(error_clean) * 100, np.max(error_noisy) * 100, np.max(error_robust) * 100]
x_pos = np.arange(len(methods))
width = 0.35
bars1 = ax4.bar(x_pos - width/2, mean_errors, width, label='平均误差', color='steelblue')
bars2 = ax4.bar(x_pos + width/2, max_errors, width, label='最大误差', color='coral')
ax4.set_ylabel('误差 (cm)', fontsize=11)
ax4.set_title('干扰抑制效果统计', fontsize=12, fontweight='bold')
ax4.set_xticks(x_pos)
ax4.set_xticklabels(methods)
ax4.legend()
ax4.grid(True, alpha=0.3, axis='y')
# 添加数值标签
for bar in bars1:
height = bar.get_height()
ax4.annotate(f'{height:.1f}',
xy=(bar.get_x() + bar.get_width() / 2, height),
xytext=(0, 3),
textcoords="offset points",
ha='center', va='bottom', fontsize=9)
for bar in bars2:
height = bar.get_height()
ax4.annotate(f'{height:.1f}',
xy=(bar.get_x() + bar.get_width() / 2, height),
xytext=(0, 3),
textcoords="offset points",
ha='center', va='bottom', fontsize=9)
plt.suptitle('磁导航与定位 - 干扰抑制与鲁棒性', fontsize=14, fontweight='bold', y=0.98)
plt.tight_layout()
plt.savefig('fig5_interference_robustness.png', dpi=150, bbox_inches='tight')
plt.close()
print(f"✓ 干扰抑制分析完成")
print(f" 无干扰平均误差: {np.mean(error_clean)*100:.1f} cm")
print(f" 有干扰(无抑制)平均误差: {np.mean(error_noisy)*100:.1f} cm")
print(f" 有干扰(RANSAC)平均误差: {np.mean(error_robust)*100:.1f} cm")
print(f" RANSAC改善效果: {(1 - np.mean(error_robust)/np.mean(error_noisy))*100:.1f}%")
return true_x, true_y, error_clean, error_noisy, error_robust
def main():
"""主函数：运行所有仿真案例"""
print("=" * 70)
print("磁导航与定位仿真")
print("=" * 70)
# 案例1: 磁场指纹地图构建
X, Y, B_total, magnets, sample_points, B_samples = magnetic_fingerprint_map()
# 案例2: 磁定位算法实现
true_x, true_y, estimated_positions_knn, errors_knn = magnetic_localization_algorithm()
# 案例3: 多传感器融合定位
time, true_x_ms, true_y_ms, x_est, error_fusion, error_dead = multisensor_fusion()
# 案例4: 定位精度分析
beacon_counts, accuracy_vs_beacons, noise_levels, accuracy_vs_noise = localization_accuracy_analysis()
# 案例5: 干扰抑制与鲁棒性
true_x_int, true_y_int, error_clean, error_noisy, error_robust = interference_robustness()
print("\n" + "=" * 70)
print("所有仿真案例已完成！")
print("=" * 70)
print("\n生成的文件:")
print(" - fig1_magnetic_fingerprint.png: 磁场指纹地图构建")
print(" - fig2_localization_algorithm.png: 磁定位算法实现")
print(" - fig3_multisensor_fusion.png: 多传感器融合定位")
print(" - fig4_accuracy_analysis.png: 定位精度分析")
print(" - fig5_interference_robustness.png: 干扰抑制与鲁棒性")
if __name__ == "__main__":
main()
DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐
6
0- 0
已为社区贡献20条内容
所有评论(0)