雷达-惯性里程计：紧耦合EKF框架设计与无人机导航应用

1. 雷达-惯性里程计技术背景与挑战

在无人机自主导航领域，多传感器融合已成为解决复杂环境下定位问题的标准范式。传统视觉-惯性里程计(VIO)虽然在多数场景表现良好，但在低光照、烟雾或高速运动导致的图像模糊等条件下性能会显著下降。毫米波雷达凭借其不受光照条件影响、可穿透部分障碍物以及对运动模糊不敏感的特性，成为视觉传感器的理想补充。

FMCW(调频连续波)雷达通过发射线性调频信号并接收目标反射信号，能够同时获取目标的距离和径向速度信息。典型的77GHz车载雷达距离精度可达厘米级，速度测量精度优于0.1m/s。然而，雷达点云具有显著区别于视觉特征点的特性：

• 稀疏性：单帧点云通常仅包含几十到几百个点
• 噪声特性：距离测量相对精确(σ≈0.1m)，但方位角误差较大(σ≈5°)
• 动态特性：多普勒效应提供径向速度信息，但存在镜像模糊问题

这些特性使得传统的基于ICP的点云配准方法在雷达数据上表现不佳，促使研究者转向紧耦合的滤波框架。我们实验室采用的TI AWR1843BOOST雷达模组重仅30克，功耗2.5W，非常适合小型无人机搭载。该雷达在20Hz更新率下可同时跟踪多达64个目标，为实时状态估计提供了充足的数据源。

2. 紧耦合EKF框架设计

2.1 系统状态定义与误差模型

我们的误差状态EKF采用15维导航状态加6维克隆状态的组合设计：

x = [x_N; x_C] = [[^Gp_I; ^Gq_I; ^Gv_I; b_a; b_ω]; [^Gp_I1; ^Gq_I1]]

其中各分量含义如下：

• $^Gp_I$：IMU在全局坐标系下的位置
• $^Gq_I$：IMU到全局坐标系的旋转四元数
• $^Gv_I$：IMU在全局坐标系下的速度
• $b_a$, $b_ω$：加速度计和陀螺仪的零偏
• $^Gp_I1$, $^Gq_I1$：通过随机克隆保存的历史位姿

误差状态采用最小参数化表示：

δx = [δp_I; δθ_I; δv_I; δb_a; δb_ω; δp_I1; δθ_I1]

其中旋转误差采用3维扰动角表示，避免了四元数过参数化带来的协方差矩阵奇异问题。

2.2 IMU状态预测模型

IMU的连续时间运动学模型为：

^Gp˙_I = ^Gv_I ^Gv˙_I = ^GR_I(a_m - b_a - n_a) + g ^Gq˙_I = 1/2^Gq_I ⊗ (ω_m - b_ω - n_ω) b˙_a = n_ba b˙_ω = n_bω

其中$a_m$和$ω_m$为IMU原始测量，$n_a$和$n_ω$为白噪声，$n_ba$和$n_bω$为零偏随机游走噪声。

在实际实现中，我们采用mid-point积分方法进行离散化：

ΔR = Exp((ω_m - b_ω)Δt) ^GR_I(k+1) = ^GR_I(k)ΔR ^Gv_I(k+1) = ^Gv_I(k) + [^GR_I(k)(a_m - b_a) + g]Δt ^Gp_I(k+1) = ^Gp_I(k) + ^Gv_I(k)Δt + 0.5[^GR_I(k)(a_m - b_a) + g]Δt²

这种积分方式相比欧拉法具有二阶精度，特别适合无人机剧烈运动场景。

2.3 随机克隆技术实现

随机克隆是处理跨时段测量的关键技术，其核心思想是将历史关键帧的状态和协方差矩阵完整复制到当前状态向量中。我们的实现要点包括：

1. 克隆触发机制：当收到新的雷达扫描时，克隆当前IMU位姿
2. 协方差管理：克隆后的状态与原状态完全相关，协方差矩阵扩展为：

Σ = [ Σ_I Σ_IO Σ_I Σ_OI Σ_O Σ_OI Σ_I Σ_IO Σ_I ]

3. 状态预测：克隆状态不随时间演化，仅原始状态参与IMU预测

这种处理保证了历史测量与新测量之间的严格相关性，是紧耦合方案优于松耦合的关键所在。在实际部署中，我们维护一个长度为N的克隆状态滑动窗口(N=5)，通过边缘化最旧状态来限制计算复杂度。

3. 雷达测量模型构建

3.1 点云匹配算法

针对FMCW雷达点云特性，我们设计了三级匹配策略：

1. 初匹配阶段：采用Munkres算法求解二分图最小权匹配

min ΣΣ C_ij X_ij C_ij = ||Rp_Pi(t_c) - Rp_Pj(t_p)||

2. 置信度筛选：构建得分矩阵S，剔除低反射强度或距离过大的匹配

s_ij = 1/(1 + ||Rp_Pi(t_c) - Rp_Pj(t_p)||)

3. 几何一致性验证：对候选匹配检查局部几何一致性

d_ij = |Σ||Rp_Pi(t_c) - Rp_Pk(t_c)|| - Σ||Rp_Pj(t_p) - Rp_Pk(t_p)|||

实测表明，这种组合策略在点云密度为0.1点/m³时仍能保持85%以上的匹配正确率。图3展示了匹配结果的xy平面投影，可见即使在大量噪声点干扰下，算法仍能保持稳定的匹配性能。

3.2 距离测量模型

考虑到雷达方位角测量不精确，我们仅利用距离信息构建观测模型：

1. 将上一帧匹配点变换到当前雷达坐标系：

Rp'_Pj = I_R^T(-Ip_R + G_R_I^T(-Gp_I + Gp_I1 + G_R_I1(Ip_R + I_R_R Rp_Pj)))

2. 计算预测距离：

d_Pj = ||Rp'_Pj||

3. 构建残差：

r_d = d_meas - d_Pj H_d = ∂r_d/∂x

该模型的创新点在于：

• 避免使用不可靠的方位信息
• 通过克隆状态建立跨时段几何约束
• 支持单点更新，不依赖最小点集

3.3 多普勒速度测量模型

雷达提供的径向速度测量包含重要运动约束：

1. 计算当前速度在点方向上的投影：

v_proj = (Rp_Pi/||Rp_Pi||)^T [I_R^T G_R_I^T Gv_I + I_R^T(ω×Ip_R)]

2. 构建残差：

r_v = v_meas - v_proj H_v = ∂r_v/∂x

我们采用马氏距离检验剔除异常测量：

r^T (H P H^T + R)^{-1} r < χ^2_threshold

其中R=0.1m/s为测量噪声协方差。

4. 系统实现与实验验证

4.1 硬件配置

实验平台采用自定义四旋翼无人机，主要传感器包括：

雷达以45°倾角安装于机头位置，这种布局兼顾了地面覆盖范围和速度测量灵敏度。传感器时空标定采用我们提出的基于连续运动的方法，外参标定精度达到：位置误差<1cm，姿态误差<0.5°。

4.2 软件架构

系统采用ROS2框架实现，主要模块包括：

1. 前端处理：

   • 雷达信号处理：距离FFT、CFAR检测、Doppler解算
   • IMU预处理：艾伦方差标定、同步时间对齐

2. 核心算法：

   • 异步IMU预测：100Hz更新
   • 雷达测量更新：20Hz触发
   • 关键帧管理：基于平移/旋转阈值触发克隆

3. 后端优化：

   • 滑动窗口优化：维护5帧克隆状态
   • 零偏估计：动态调整随机游走噪声参数

代码经过SIMD指令优化，在i7-10850H处理器上单线程运行时间仅8ms/帧，完全满足实时性要求。

4.3 性能评估

我们在20m×15m的室内场地进行实验，布置了随机反射物模拟复杂环境。无人机以3m/s速度执行矩形轨迹飞行，总行程116.4米。图4展示了估计轨迹与动捕系统(GT)的对比。

关键性能指标如下：

特别值得注意的是，在模拟视觉失效的快速旋转阶段(角速度>300°/s)，我们的方法仍能保持位置误差<2%，而同期VIO方案误差已超过15%。这验证了雷达惯性系统在极端运动条件下的优势。

5. 工程实践中的关键问题

5.1 雷达信号处理技巧

原始ADC信号的质量直接影响最终性能，我们总结了几点经验：

1. ** chirp配置优化**：

   • 带宽B=4GHz(77-81GHz)
   • 扫频时间T=50μs
   • 采样率fs=10MHz 这种配置下距离分辨率ΔR=c/(2B)=3.75cm，最大不模糊速度v_max=λ/(4T)=0.97m/s。

2. 静态杂波抑制： 采用相量均值相消法：

S_clutter = 1/N Σ S_i S_target = S_raw - S_clutter

可有效抑制墙体等固定反射体干扰。

5.2 状态估计调试方法

系统调试时需要重点关注：

1. 噪声参数标定：

   • IMU噪声：通过静态艾伦方差分析获取
   • 雷达噪声：通过重复实验统计测量方差

2. 可观测性分析：

   • 位置/yaw不可观测，需监控协方差增长
   • 速度/pitch/roll可观测，应检查收敛速度

3. 一致性检验：

   • NEES测试：验证滤波器一致性
   • 残差监测：检测模型失配

5.3 实际部署注意事项

1. 雷达安装：

   • 避免金属部件遮挡天线
   • 确保与IMU刚性连接
   • 考虑天线极化方向与目标特性匹配

2. 环境适应性：

   • 雨天性能下降约30%
   • 金属密集环境需调整CFAR阈值
   • 动态物体需配合聚类算法过滤

3. 计算资源分配：

   • 80%资源给点云匹配
   • 15%给EKF预测
   • 5%给状态管理

这套系统已在多个无人机项目中成功应用，包括隧道巡检、灾害救援等GNSS拒止场景。实践表明，通过合理调参，在100米范围内可保持3%以内的相对定位精度，完全满足自主飞行需求。