米喜1. 项目概述：多无人机协同定位系统仿真
在移动目标跟踪领域，多无人机协同定位系统正成为研究热点。这次我们要实现的是三架搭载被动雷达导引头的无人机对机动目标的协同定位仿真。每架无人机只能获取目标的高低角和方位角信息，无法直接测量距离——这就好比几个人闭着眼睛听声音判断声源位置，单靠一个人的判断误差很大，但多人协同就能显著提高定位精度。
这个项目的核心在于设计基于扩展卡尔曼滤波(EKF)的分布式融合算法。实际工程中遇到过这样的情况：单无人机定位时Z轴误差能达到12米，而三机协同后误差能控制在5米以内。特别是在目标做蛇形机动时，多无人机的观测数据互补特性能够有效抑制单视角观测的误差放大效应。
需要模型API调用？ 免费领10W Token，多模型网关一键接入 Claude、DeepSeek 等主流模型。
2. 系统建模与坐标系定义
2.1 坐标系建立
采用右手大地坐标系作为全局参考系：
- X轴指向正东
- Y轴指向正北
- Z轴垂直地面向上
目标状态向量定义为：
code复制X = [x, y, z, vx, vy, vz]^T
包含三维位置和速度分量。无人机位置假设通过RTK等定位系统精确已知，这在仿真中简化为预设的航迹点。
2.2 量测模型实现
被动雷达导引头的量测模型用Python实现如下：
python复制import numpy as np
def measurement_model(state, uav_pos):
dx = state[0] - uav_pos[0]
dy = state[1] - uav_pos[1]
dz = state[2] - uav_pos[2]
# 使用arctan2处理全象限方位角
azimuth = np.arctan2(dy, dx)
# 高低角计算先投影到水平面
horizontal_dist = np.sqrt(dx**2 + dy**2)
elevation = np.arctan2(dz, horizontal_dist)
return np.array([azimuth, elevation])
这里有两个关键细节：
- 方位角计算必须使用arctan2而非arctan，否则会导致象限判断错误。曾经在实测中出现过因为用错函数导致180度方向误判的情况。
- 高低角计算需要先将目标位置投影到水平面，避免当dx和dy接近零时出现除零错误。这在无人机飞越目标正上方时尤为重要。
3. 扩展卡尔曼滤波器设计
3.1 状态转移模型
采用匀速运动模型(CV)，状态转移矩阵为：
matlab复制dt = 0.1; % 采样间隔
F = [1 0 0 dt 0 0;
0 1 0 0 dt 0;
0 0 1 0 0 dt;
0 0 0 1 0 0;
0 0 0 0 1 0;
0 0 0 0 0 1];
过程噪声协方差矩阵Q需要根据目标机动特性调整：
matlab复制Q = diag([0.1, 0.1, 0.05, 0.3, 0.3, 0.1]);
其中位置噪声分量较小，速度噪声分量较大，这符合匀速运动模型的特点。Z轴噪声通常设置得更小，因为大多数目标的垂直机动能力有限。
3.2 观测融合策略
当多架无人机的观测数据同步到达时，需要纵向堆叠各机的观测矩阵：
python复制H_total = np.vstack([H1, H2, H3])
z_total = np.concatenate([z1, z2, z3])
R_total = block_diag(R1, R2, R3)
实测中发现两个常见问题：
- 时间同步误差超过100ms会导致融合性能急剧下降
- 各无人机的观测噪声水平不一致时，需要根据信噪比动态调整R矩阵权重
4. 仿真实现与结果分析
4.1 目标机动轨迹设计
设置目标做蛇形机动：
python复制def target_motion(t):
x = 50 * np.sin(0.1 * t)
y = 2 * t
z = 10 + 2 * np.sin(0.2 * t)
return np.array([x, y, z])
这种运动模式能充分测试滤波器对非匀速运动的适应能力。特别当目标做急转弯时，匀速模型会出现明显的跟踪滞后。
4.2 协方差调参经验
量测噪声协方差R的设置对性能影响显著：
- 方位角噪声通常比高低角噪声大（0.005 vs 0.003弧度）
- 实际环境中建议先进行静态标定测试，统计出实际噪声水平
- 遇到过将R设置过小导致滤波器发散的情况，这时需要适当增大R值
4.3 定位性能对比
通过100次蒙特卡洛仿真得到：
| 指标 | 单无人机 | 三机协同 |
|---|---|---|
| X轴RMSE(m) | 8.2 | 3.1 |
| Y轴RMSE(m) | 7.8 | 2.9 |
| Z轴RMSE(m) | 12.4 | 4.7 |
| 最大误差(m) | 25.6 | 9.8 |
特别是在t=45s时，当两架无人机与目标接近共线时，单无人机定位误差急剧增大到18米，而协同定位仍能保持5米以内的精度。这验证了多视角观测对病态几何条件的改善作用。
5. 工程实践中的问题与解决
5.1 通信延迟补偿
在实际系统中遇到的通信延迟问题可通过以下方法缓解：
- 为每个量测数据打上精确的时间戳
- 使用状态预测器将量测数据对齐到同一时刻
- 设置最大允许延迟阈值（建议≤200ms）
5.2 观测异常值处理
导引头可能因遮挡等原因产生野值，我们采用：
python复制if abs(z_innov) > 3 * np.sqrt(S):
# 使用预测值代替异常观测
z_corrected = H @ x_pred
# 自适应增大R矩阵
R *= 2
5.3 计算效率优化
多无人机系统面临的计算瓶颈可通过：
- 采用序贯滤波替代批量处理
- 使用UD分解替代直接矩阵求逆
- 将雅可比矩阵计算改为定点数运算
在树莓派4B上的实测表明，优化后单次滤波耗时从15ms降至6ms，能满足实时性要求。
6. 扩展改进方向
对于高机动目标，建议尝试：
- 交互多模型(IMM)滤波：组合CV/CA/CT模型
- 自适应滤波：根据新息自动调整Q/R矩阵
- 考虑引入距离信息（如射频测距）改善Z轴精度
在最近的城市环境测试中，我们结合UWB测距将Z轴误差进一步降低到3米以内。不过要注意的是，任何额外的传感器都会带来新的校准和同步问题，需要根据具体应用场景权衡利弊。