基于等变滤波器的雷达-惯性里程计（EqF-RIO）：融合多普勒速度与多状态约束的无人机导航方法

《IEEE Robotics and Automation Letters》：Equivariant Filter for Radar-Inertial Odometry

　　

研究人员提出了一种离散时间等变滤波框架（Equivariant Filter, EqF），用于雷达辅助惯性导航系统（Radar-Inertial Odometry, RIO）。该框架将雷达多普勒速度与三维点云测量融合，同时联合估计雷达与惯性测量单元（Inertial Measurement Unit, IMU）之间的外参标定参数，并支持多状态约束更新。通过引入基于伽利略群（Galilean group）的等变对称性，结合传感器噪声与模型不确定性的精确建模，该方法保持了滤波一致性并提升了鲁棒性。解析形式的离散时间线性化矩阵避免了滤波传播中矩阵指数运算的需求，降低了计算复杂度；多状态约束则强化了连续机器人位姿间的空间一致性。在两个无人机数据集上的真实世界实验表明，在精确标定条件下，该方法的位置漂移通常低于行驶距离的1%；即便从高达120°的初始雷达-IMU姿态误差中恢复，仍能维持稳定的收敛性，性能优于传统基于扩展卡尔曼滤波（Extended Kalman Filter, EKF）的方法。相关评估代码已开源。

论文解读

研究背景与意义

雷达-惯性里程计（RIO）是无人机自主导航的核心技术之一，传统方法多基于扩展卡尔曼滤波（EKF）实现传感器融合，但在初始外参标定误差较大、运动激励不足的场景下易出现收敛失败、一致性差的问题。伽利略群（Galilean group）虽已被用于惯性测量单元（IMU）的运动学建模，但尚未被应用于完整的滤波框架中。为解决上述问题，研究人员开展了基于等变滤波（EqF）的雷达-惯性里程计研究，相关成果发表于《IEEE Robotics and Automation Letters》。该研究的核心目标是利用等变对称性提升滤波的鲁棒性与一致性，扩大初始外参标定的收敛域，同时降低计算成本。

关键技术方法

研究人员构建了包含导航位姿、偏差与雷达点云变换的24维流形状态，定义由SE₂(3)、其李代数se₂(3)与SE(3)半直积构成的对称群G。通过设计状态作用?与输入作用ψ，证明系统在该群下的等变性，推导离散时间提升（lift）映射Λ，构建等变误差定义。滤波传播阶段采用解析形式的离散线性化矩阵A与输入噪声矩阵B，避免矩阵指数运算；观测阶段融合雷达多普勒速度测量与多状态约束，多普勒速度更新采用球坐标系建模点云噪声，多状态约束通过滑动窗口内的历史位姿克隆强化空间一致性。实验采用arena与ardea两个无人机真实数据集，arena数据集由搭载VectorNav VN-100 IMU、IWR6843AOP雷达与Ouster OS0-128激光雷达的自定义无人机采集，ardea数据集由ARDEA-X无人机采集，均包含多段不同长度与运动模式的轨迹。

研究结果

III-A 连续与离散系统建模

研究人员首先定义状态向量ξ包含导航变换T∈SE₂(3)、陀螺与加速度计偏差b∈se₂(3)、雷达点云变换L∈SE(3)，引入重力向量g_N与校准输入μ，推导连续时间无噪声系统动力学，采用Gal(3)指数映射实现离散化，得到式(9)的离散传播方程。

III-B 雷达-惯性系统的对称性

定义对称群G=SE₂(3)?se₂(3)×SE(3)，证明状态作用?与输入作用ψ均为自由传递的右群作用，给出逆映射?^-1与提升映射Λ的解析形式，验证系统满足等变性质，为后续滤波设计奠定理论基础。

III-C 等变误差与传播矩阵

基于等变误差e=?(X?^-1, ξ)定义局部参数化ε=log(?^-1_ξ?(e))^∨∈?²⁴，线性化得到离散状态矩阵A∈?^24×24与输入噪声矩阵B∈?^24×25，其中A由Γ、Υ、A₁、A₂分块构成，利用Gal(3)左雅可比与指数映射简化计算。

III-D 多普勒速度更新

设计多普勒速度测量函数h^v，将点云位置p_f与偏置角速度ω作为输入，线性化得到输出矩阵C^v∈?^1×24；采用球坐标系φ(p)=(κ, ρ)建模点云噪声，推导输出噪声矩阵D^v∈?^1×7，提升噪声建模精度。

IV 多状态约束

IV-A 状态增广与传播矩阵

将原始状态增广为包含k个历史雷达位姿P_i∈SE(3)的扩展状态ξ_a，对应提升状态X_a包含k个静态位姿F_i，推导增广后的传播矩阵A^a∈?^{(24+6k)×(24+6k)}与B^a∈?^(24+6k)×25，历史位姿无动态仅作为约束项。

IV-B 多状态约束更新

定义单特征点多帧观测的测量函数h^p，计算当前位姿与历史位姿下点云的空间一致性误差，线性化得到输出矩阵C^p∈?^1×30与输出噪声矩阵D^p∈?^1×6，利用特征匹配强化时序约束。

V 实验与结果

实验采用绝对位姿误差（APE）、根均方误差（RMSE）与平均归一化估计误差平方（ANEES）作为评价指标。仅使用多普勒速度更新时，arena数据集位置漂移低于1%，ardea数据集同样保持低漂移；加入多状态约束后，长轨迹的漂移进一步降低。在外参标定误差测试中，EKF-RIO在10°误差时已出现部分收敛，80°误差时完全发散；EqF-RIO在80°误差下所有序列均完全收敛，120°误差下仍保持部分或完全收敛，ANEES值始终接近1，一致性显著优于EKF。球坐标噪声建模（EqF）与未采用该建模的变体（EqF*）性能差异较小，说明等变对称性是提升收敛域的核心因素。

讨论与结论

该研究首次将伽利略群对称性应用于完整的雷达-惯性滤波框架，解决了传统EKF在大初始外参误差下的发散问题。等变误差的定义避免了局部线性化的不一致性，解析离散矩阵降低了计算开销，多状态约束有效抑制了长轨迹漂移。实验验证了方法在无人机平台上的实用性，位置漂移低于1%，外参收敛域最高可达120°，为雷达-惯性导航提供了更鲁棒的解决方案。研究人员已开源评估代码，推动相关技术的落地应用。