Fast-LIO2实战：如何用轮速计拯救点云发散时的轨迹漂移（附代码解析）

Fast-LIO2实战：轮速计约束在点云发散场景下的轨迹优化策略

当你在深夜调试SLAM系统时，突然发现机器人轨迹像喝醉酒一样开始乱飘——这可能是每个SLAM开发者都经历过的噩梦时刻。Fast-LIO2作为目前最先进的激光惯性里程计之一，虽然在大多数场景表现优异，但当点云特征不足导致发散时，单纯的IMU预测会带来灾难性的轨迹漂移。本文将带你深入解决这个工程痛点，通过轮速计约束实现轨迹"急救"。

1. 问题本质与轮速计约束原理

点云发散本质上是一种观测退化问题。当环境特征单一（如长走廊、空旷场地）时，激光雷达无法提供足够的几何约束，导致迭代误差卡尔曼滤波(IKEF)中的观测更新失效。此时系统完全依赖IMU进行状态预测，而IMU的零偏误差会随时间累积，最终造成轨迹"放飞自我"。

轮速计在这个场景下能提供关键的速度约束：

• 绝对速度参考：轮速计测量的是机器人本体坐标系下的线速度，不受点云退化影响
• 低成本补充：相比增加更多传感器，轮速计是大多数机器人平台已有的硬件
• 运动学约束：对于地面机器人，轮速与IMU速度存在确定的运动学关系

在Fast-LIO2框架中，状态量包含位置、旋转、IMU-轮速计外参、速度、陀螺零偏、加速度计零偏和重力向量共23维。轮速计的观测主要作用于速度分量，通过以下方式建立观测模型：

Eigen::Matrix<double, 3, 23> H_WHEEL = Eigen::Matrix<double, 3, 23>::Zero(); H_WHEEL.template block<3, 3>(0, 12) = Eigen::Matrix<double, 3, 3>::Identity(); // 观测矩阵作用于速度分量

2. 工程实现四步法

2.1 数据接口与坐标转换

轮速计数据通常通过ROS话题订阅获取，需要特别注意坐标系转换：

1. 物理安装确认：测量轮速计与IMU的相对位置关系，特别是轮轴与IMU的偏移量
2. 运动方向校准：对于差速驱动机器人，需明确左右轮对应的正方向
3. 外参标定：通过手持运动或特定轨迹标定T_wheel_imu变换矩阵

典型的速度转换代码实现：

// IMU坐标系(右前上)到轮速计坐标系的转换 Eigen::Vector3d vel_odom(0, sqrt(pow(v_odom[wheel_num]->twist.linear.x, 2) + pow(angvel_last[2] * Wheel_T_wrt_IMU[1], 2)), 0.0); Eigen::Vector3d vel_world = imu_state.rot * vel_odom; // 转换到世界坐标系

2.2 发散检测与协方差自适应

点云发散检测是触发轮速计约束的关键。我们采用两种互补的检测策略：

检测到发散时，需要调整各传感器的协方差权重：

Eigen::Matrix3d odom_noise = Eigen::Matrix3d::Zero(); if(flag_degen_) { if(slip_cov_[1] < 1) odom_noise.diagonal() << cov_wheel_, cov_wheel_, cov_wheel_; else odom_noise.diagonal() << 3.0 * cov_wheel_, 3.0 * cov_wheel_, 3.0 * cov_wheel_; } else { odom_noise.diagonal() << 10.0 * cov_wheel_, 10.0 * cov_wheel_, 10.0 * cov_wheel_; }

2.3 打滑检测与失效保护

轮速计最大的风险在于轮子打滑时会产生误导性观测。我们实现了一个基于运动一致性的打滑检测器：

• 速度-IMU一致性检查：比较轮速计推算的速度与IMU积分速度
• 零速检测：当IMU检测到静止时，轮速计读数应为零
• 历史滑动窗口：维护最近N次观测的滑动窗口统计

打滑时的典型处理流程：

1. 计算速度差异率：diff_ratio = |v_wheel - v_imu| / max(v_wheel, v_imu)
2. 当连续3帧diff_ratio > 0.4时触发打滑标志
3. 增大轮速计噪声协方差，降低其观测权重

2.4 状态更新与协方差调整

完整的轮速计更新包含以下步骤：

1. 计算卡尔曼增益：

   Eigen::Matrix<double, 23, 3> K_WHEEL = kf_cov_ * H_WHEEL.transpose() * (H_WHEEL * kf_cov_ * H_WHEEL.transpose() + odom_noise_).inverse();

2. 状态量修正：

   Eigen::Matrix<double, 23, 1> dx_ = K_WHEEL * (vel_world - imu_state.vel); imu_state.pos += dx_.template block<3, 1>(0, 0); imu_state.vel += dx_.template block<3, 1>(12, 0);

3. 协方差更新：

   kf_cov_ = (Eigen::Matrix<double, 23, 23>::Identity() - K_WHEEL * H_WHEEL) * kf_cov_;

3. 实战效果与参数调优

在实际测试中，我们对比了三种配置的轨迹表现：

关键参数调优建议：

• 初始协方差cov_wheel_：从0.01开始，按0.5倍步长调整
• 发散检测阈值：先设为点云密度的2倍，再微调
• 滑动窗口大小：通常取5-10帧，运动越快窗口越小

调试技巧：在rviz中实时可视化协方差椭圆，当看到z轴椭圆突然变大时，就是发散发生的时刻

4. 进阶优化方向

对于追求极致性能的开发者，可以考虑以下扩展方案：

1. 多传感器权重动态调整：

   def update_weights(): wheel_weight = 1.0 - min(1.0, pointcloud_density / 10.0) imu_weight = 0.1 if wheel_weight > 0.7 else 1.0 return wheel_weight, imu_weight

2. 基于学习的外参在线标定：利用神经网络实时估计T_wheel_imu的变化
3. 紧耦合的轮速计-IMU标定：在初始化阶段加入特定的"8字形"标定轨迹

在最后测试阶段，记得检查以下checklist：

• [ ] 轮速计话题频率是否与IMU同步
• [ ] 机器人急停时打滑检测是否灵敏
• [ ] 从发散恢复到正常时协方差能否快速收敛