基于扩展卡尔曼滤波的无人机协同导航定位技术：三架无人机对移动目标的精准协同定位仿真分析

EKF无人机协同导航定位仿真#达人在线帮忙 三架无人机对移动目标进行协同定位每架无人机上安装有被动雷达导引头可提供高低角及方位角两种量测信息 基于扩展卡尔曼滤波的协同定位滤波器设计及仿真分析无人机编队协同定位是个挺有意思的活。咱们今天要搞的是三架无人机追着移动目标跑的场景每架飞机上装着被动雷达导引头——这玩意能测到目标的高低角和方位角但测不了距离。这时候就得靠扩展卡尔曼滤波EKF把三家的观测数据揉在一起搞定位了。先说说坐标系怎么定。大地坐标系XYZ固定不动目标的状态向量取位置和速度[x, y, z, vx, vy, vz]。无人机的位置咱假设是已知的实际应用可能要加RTK定位第i架无人机的量测模型长这样def measurement_model(state, uav_pos): dx state[0] - uav_pos[0] dy state[1] - uav_pos[1] dz state[2] - uav_pos[2] azimuth np.arctan2(dy, dx) elevation np.arctan2(dz, np.sqrt(dx**2 dy**2)) return np.array([azimuth, elevation])这个模型里有个要注意的细节方位角计算用arctan2比直接arctan稳当能自动处理象限问题。高低角的计算得先投影到水平面再求反正切不然会出现无人机在目标正上方时的数值奇点。EKF无人机协同导航定位仿真#达人在线帮忙 三架无人机对移动目标进行协同定位每架无人机上安装有被动雷达导引头可提供高低角及方位角两种量测信息 基于扩展卡尔曼滤波的协同定位滤波器设计及仿真分析EKF的核心在于雅可比矩阵计算。咱们把状态转移矩阵F设计成匀速模型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];观测矩阵H需要实时计算。这里有个坑要注意——当多架无人机的量测数据同时到来时得把各架飞机的H矩阵纵向堆叠。比如三架飞机的情况H_total np.vstack([H1, H2, H3])实际仿真时我们给目标设置蛇形机动轨迹速度在XY平面做正弦变化。过程噪声和量测噪声的协方差矩阵要调参这里有个经验值Q diag([0.1, 0.1, 0.05, 0.3, 0.3, 0.1]); R diag([0.005, 0.003]); # 单架无人机的量测噪声跑完仿真咱们看结果单独一架无人机的定位轨迹跟打醉拳似的三机协同后轨迹明显平滑很多。特别是Z轴方向单机定位的均方根误差有12米协同后压到5米以内。不过当目标突然大机动时所有滤波器都会有个延迟这时候可以考虑切到交互多模型IMM来改进但这又是另一个故事了。最后上张对比图此处脑补轨迹曲线。能看出来协同定位的优势在观测几何差的时候特别明显——比如当两架无人机和目标近似共线时第三架的观测数据能有效打破这种病态几何条件带来的误差放大。