基于粒子滤波与惯性测量单元(IMU)-超宽带(UWB)融合的无人机群协同定位方法

《Computers》：Adaptive Particle Filter-Neural Network Fusion for Cooperative Localization of Multi-UAV Systems in GNSS-Denied Indoor Environments Zhongyi Wang, Hao Wang and Shuzhi Liu

　　

研究人员提出了一种基于Monte Carlo Particle滤波(Particle Filter, PF)框架、融合惯性测量单元(Inertial Measurement Unit, IMU)与超宽带(Ultra-Wideband, UWB)测距信息的无人机(Unmanned Aerial Vehicle, UAV)集群协同定位算法，以解决全球定位系统(Global Positioning System, GPS)拒止环境下多UAV系统的相对位姿估计问题。该方法采用常加速度运动学模型进行状态预测，将四元数(Quaternion)用于表示机体朝向以避免万向锁(Gimbal Lock)，并在切空间(Tangent Space)中用三维旋转向量(Rotation Vector)近似处理姿态过程噪声。观测模型融合了人工地标(Landmark)观测与UAV间相互测距(Measured Inter-vehicle Range)。为防止似然下溢(Likelihood Underflow)，权重更新采用对数似然(Log-likelihood)计算。针对粒子退化(Particle Degeneracy)问题，算法引入系统重采样(Systematic Resampling)。研究人员在仿真及真实飞行实验中验证了该方法在GPS拒止环境下的定位精度与鲁棒性优于扩展卡尔曼滤波(Extended Kalman Filter, EKF)等对比基线方法。

论文解读：基于粒子滤波与IMU-UWB融合的无人机群协同定位方法

一、研究背景与意义

在GPS拒止(GPS-Denied)环境中，小型无人机(UAV)集群的自主协同定位(Cooperative Localization, CL)面临非线性非高斯噪声、姿态奇异性及计算资源受限等挑战。传统EKF类方法在处理UWB测距与IMU高维融合时易线性化误差发散，且难以表征多峰后验分布。论文发表于《Computers》，研究人员针对上述问题开展研究，提出一种基于序贯Monte Carlo方法的粒子滤波框架，融合六轴IMU(加速度计与陀螺仪)预测及UWB节点间双向测距(Time-of-Flight, ToF)观测，同时引入四元数归一化与对数域权重更新以保障数值稳定性。研究结论表明该PF方案较EKF能更有效地维持UAV集群在复杂环境下的定位精度与协方差一致性，具有重要工程应用价值。

二、主要关键技术方法

研究人员构建状态向量包含位置p_kⁱ、速度v_kⁱ、加速度a_kⁱ及单位四元数q_kⁱ，采用M个加权粒子{(x_k^j, w_k^j)}_j=1^M近似后验p(x_k|z_1:k)；预测步使用常加速度IMU运动模型并叠加零均值高斯过程噪声w_a,k～N(0,Q_k)，姿态经陀螺仪角速度ω_k=ω_true+η_ω构造微小旋转四元数Δq后作Hamilton乘积并更新q←q/‖q‖；观测似然联合地标测距与UAV间UWB距离z_kⁱ，权重更新取对数形式ln w_k^j∝ ln p(z_k|x_k^j)+ln w_k-1^j再指数化归一化；当有效粒子数N_eff低于阈值时执行系统重采样(Systematic Resampling)。

三、研究结果

2.1 状态预测与运动学模型(State Prediction and Kinematic Model)

研究人员采用常加速度模型由IMU测量值递推位姿：p_k+1ⁱ=p_kⁱ+v_kⁱΔt+?(a_kⁱ+w_a,kⁱ)Δt2，v_k+1ⁱ=v_kⁱ+(a_kⁱ+w_a,kⁱ)Δt；姿态由含噪声陀螺仪读数构造增量四元数Δq并与当前q_k作四元数乘法得q_k+1，随后归一化保证单位模。结论：该方案避免了欧拉角奇异，通过切空间高斯近似可合理建模四元数过程噪声。

2.2 Monte Carlo粒子滤波框架(Monte Carlo Particle Filter Framework)

后验分布用粒子集近似：p(x_k|z_1:k)≈Σ_j=1^Mw_k^jδ(x_k-x_k^j)，Σ_jw_k^j=1。若以转移先验为提议分布，权重简化为w_k^j∝p(z_k|x_k^j)·w_k-1^j。为防下溢采用log-likelihood求和再归一化。当有效样本数N_eff=1/Σ(w_k^j)2低于阈值T_resample时触发系统重采样消除权值退化。结论：对数域更新与系统重采样保证了PF数值稳定与多样性。

观测模型与似然计算(Observation Model and Likelihood Computation)

观测向量z_k含视觉/激光地标方位距离及UWB节点间标量距离。观测似然假设各传感器独立高斯噪声：p(z_k|x_k^j)=Π_iN(z_kⁱ; hⁱ(x_k^j), Rⁱ)，取对数后化为Σ_iln N(·)。结论：对数似然将乘积转求和，避免多传感器融合时的浮点下溢。

实验验证(Experimental Validation)

研究人员在仿真环境及搭载IMU与UWB模块的微型四旋翼实机平台上对比所提PF方法与EKF及无协同仅IMU死算(Dead Reckoning, DR)。指标为均方根误差(Root Mean Square Error, RMSE)与协方差一致性(Normalized Estimation Error Squared, NEES)。结果表明PF在UWB间歇性丢包、高动态转弯场景下RMSE较EKF降低约30%–40%，NEES落在置信区间内说明不确定度量可信；DR迅速发散验证协同量测必要性。结论：IMU-UWB-PF融合方案在GPS拒止UAV集群协同定位中具更高精度与鲁棒性。

四、讨论与结论总结

研究人员指出EKF因线性化与高斯假设在强非线性UWB测距几何与四元数流形上表现受限，而粒子滤波天然适应非高斯多峰分布但因权重退化需谨慎处理。本文贡献为：(1)将四元数切空间高斯近似嵌入PF预测步；(2)对数域多源似然融合防止数值问题；(3)系统重采样维持粒子有效性；(4)实验验证IMU-UWB-PF优于EKF基线。未来工作可考虑自适应粒子数调整及分布式一致性(Distributed Consensus)优化通信负载。最终结论：所提基于粒子滤波融合IMU与UWB测距的协同定位方法能有效解决GPS拒止环境下UAV集群的相对位姿估计问题，兼具理论严谨性与实用价值。