搞机器人定位的人十有八九都在Fast-LIO2上碰过壁。本来激光雷达建图效果好好的一上轮式机器人底盘就翻车长走廊左右横跳、玻璃墙旁边位姿乱飘、低速直行时航向慢慢歪掉。我一开始的直觉是换更强的激光雷达或者加更多IMU后来才意识到问题不在传感器硬件而在缺了“轮速计”这一路约束。Fast-LIO2本身是激光-惯性紧耦合的框架原版没有把轮式编码器当成正经观测来源可差速底盘上编码器精度其实很高短时间内的位移增量比激光在大面积空白区域里靠谱得多。这篇文章把我从零开始往Fast-LIO2里融合轮速计的完整过程写清楚重点是两个最容易翻车的环节打滑检测和协方差调参。前半部分会讲融合思路和代码层面怎么插入轮速观测后半部分是实车调试的记录包括参数初值怎么算、现场怎么微调、坏数据长什么样。给正在做类似事情的朋友一个可以照着走的路线。1. Fast-LIO2融合轮速计之前先想清楚这几个问题1.1 Fast-LIO2内部信息流与融合卡点Fast-LIO2的核心是一个迭代误差状态卡尔曼滤波器IESKF大致信息流是这样的IMU数据以较高频率通常是200Hz到500Hz做状态前向传播给出机器人位姿和速度的预测值激光雷达每一帧点云到达后通过去畸变和ikd-tree增量建图提取点到局部地图的残差作为观测去更新滤波器状态。整个过程里轮速计没有任何角色。这意味着在没有视觉特征、没有几何特征的区域激光雷达的观测退化了滤波器就只能靠IMU积分往前推而IMU积分最大的问题就是不断累积的漂移尤其在长走廊和开阔空地上重力对齐以后水平方向的加速度噪声和偏置会直接变成速度和位置的误差。轮速计能补的正是这个空缺轮式编码器通过轮子转过的圈数推算移动距离短时间内的相对位移非常准确不随时间漂移。把它作为一路观测塞进滤波器的更新阶段就相当于给系统加了一副“地面约束”让状态在激光退化时依然有东西拽着。但融合也不是没有代价。轮速计误差和地面附着力强相关一旦打滑整车移动距离和轮子转过的角度就对不上了。这时候如果还拿编码器数据更新滤波器系统会被带偏而且这种误差有持续性等激光雷达找回几何特征时想把状态拉回来往往已经很费劲了。1.2 轮速计到底能补什么、不能补什么差分底盘和四轮底盘上轮式编码器能提供两个量线速度 v_odo 和角速度 ω_odo。线速度来自左右轮速度的平均值角速度来自左右轮速度差除以轮距。这两个量在三个方向上有意义水平前进方向的速度、绕竖直轴偏航方向的角速度、以及一定程度上绕横滚轴的侧向速度约束。补不了的东西也很明显第一是没有绝对位置编码器从零开始累积位移误差随距离增加第二是完全没有竖直方向的信息轮子在地上转机器人可能被抬起来或者过坎时轮子悬空编码器数据完全失效第三是侧向速度普通轮式底盘在转向时会有侧偏编码器只能测轮子自己转的速度测不了车体相对地面的横向滑动。我做融合时给自己定了两条原则只在速度残差层面信任轮速计绝不让它直接控制位置增量打滑标志一旦触发立刻把轮速观测的信息矩阵降为零不让错误数据进入更新。1.3 哪些底盘配置不建议硬融合轮速计融合不是所有机器人都有正收益。我测过三种底盘差速底盘表现最好四轮阿克曼转向底盘次之全向麦克纳姆轮底盘最糟糕。差速底盘结构最简单轮子方向始终和车体纵轴对齐编码器测的就是实际前进方向的速度融合模型简单可靠。阿克曼底盘转弯时前轮有转向角车体往往有轻微侧偏如果不单独建运动学模型直接用差速公式算角速度会有系统性偏差。麦克纳姆轮底盘每个轮子运动方向复杂打滑概率极高编码器数据和真实速度之间误差几乎不可预测我后来直接放弃在这类底盘上做轮速计融合。另外还有一种情况我建议直接跳过如果你的底盘轮胎很小、悬挂很软、经常在松软地面泥土、砂石、地毯上跑那打滑检测会频繁触发轮速计大部分时间处于被禁用状态等于白做。不如把精力放在改善激光退化场景的处理上比如增加反光柱或者视觉特征。2. 融合方案选型与工程实现思路2.1 三种主流融合路径对比往Fast-LIO2里加轮速计业界最常见的有三条路我分别搭过原型各有各的坑。第一种是“轮速预积分运动模型约束”把编码器数据按照IMU预积分的思路在前后两帧激光之间积出位移增量和角度增量当成一个虚拟的IMU测量参与状态传播。优点是好实现很多开源代码已经把这套写好了缺点是预积分过程本身会放大编码器噪声而且轮速计短时间积分的误差和实际走过的弧长有偏差遇到打滑时误差还会沿时间轴传播。第二种是“松耦合后处理”把Fast-LIO2跑出来的位姿序列和轮速积分出来的轨迹做一个因子图融合用一个滑动窗口优化。优点是不改动Fast-LIO2内部风险低缺点是Fast-LIO2内部的激光-惯性紧耦合优势在松耦合层被稀释了系统整体精度提升有限尤其在快速动态场景下两个模块的时间戳对不齐会带来肉眼可见的轨迹撕裂。第三种是“在IESKF观测模型里直接插入轮速测量”把编码器输出的线速度、角速度写成一个非线性观测方程在每次滤波器更新时和多源观测一起参与迭代。这是我现在在用的方案信息利用最充分融合得也最自然但代码改动量最大需要对Fast-LIO2的IESKF更新逻辑有清晰理解。从工程角度看如果只是想快速验证融合收益推荐先做第一种如果追求精度和大规模长期运行稳定我个人建议直接上第三种不要在第二种上花时间。2.2 我选择在IESKF测量模型里做更新Fast-LIO2源码里IESKF的更新阶段会计算观测残差和对应的雅可比矩阵然后通过卡尔曼增益更新状态。激光点云匹配是其中一种观测来源不同传感器可以作为额外观测来源加入。我的具体做法是构造一个二维观测向量z [v_odo, omega_odo]^T对应的预测值从当前滤波器状态 x 里提取v_pred v_x * cos(yaw) v_y * sin(yaw) 车体坐标系的前向速度 omega_pred omega_z 车身偏航角速度然后构造残差r z - [v_pred, omega_pred]^T这一步观测方程非常简单难点全在雅可比计算需要分别对状态向量里的位置、速度、姿态角求偏导。Fast-LIO2的代码风格是直接把状态变量集中排布我一开始在雅可比矩阵J的行列索引上搞错了几次后来发现最好的对照方式是把误差状态定义的注释打印出来照着索引逐列检查。代码骨架长这样节选自我在实际项目里的实现做了简化// 轮速计观测方程: z h(x) n // z[0] v_x * cos(yaw) v_y * sin(yaw) // z[1] omega_z Eigen::Matrixdouble, 2, 1 h; h.setZero(); h(0) state.v(0) * cos(state.R.eulerAngles(2)) state.v(1) * sin(state.R.eulerAngles(2)); h(1) state.omega(2); // 残差 Eigen::Matrixdouble, 2, 1 r; r wheel_meas - h; // 雅可比矩阵 Eigen::Matrixdouble, 2, DIM_OF_STATES H; H.setZero(); H(0, 3) cos(yaw); // 对 x 方向速度分量 v_x 求偏导 H(0, 4) sin(yaw); // 对 v_y 求偏导 H(0, 6) -v_x * sin(yaw) v_y * cos(yaw); // 对偏航角求偏导 H(1, 5) 1.0; // 对偏航角速度 omega_z 求偏导实现的时候整段代码都在原版IESKF::update_iterated_dyn_share的循环里做和激光观测的残差加在一起参与整个迭代更新流程。2.3 外参标定与轮速观测方程轮速计融合里最容易忽略但最致命的问题是机器人运动学模型中的参数不准。差速底盘需要两个参数轮子半径 R 和轮距左右轮中心距B。这两个参数如果标定不准编码器算出来的线速度、角速度和真实运动之间有系统性偏差无论协方差调得多精细都没用。轮子半径和轮距的标定方法并不复杂我在实验室地板上画了一条10米直线控制机器人匀速直行量实际走的距离和编码器积分距离的比值就能算出轮距的一个初始修正系数。角速度标定则是让机器人原地转多圈对照外部运动捕捉系统反复迭代几次。千万不要只看厂商给的CAD参数。我见过一个底盘标称轮径150mm实测有效滚动半径只有146mm差这4毫米直行10米就偏了差不多0.27米。这种误差在融合里会让滤波器的残差始终偏大协方差调不出来。3. 打滑检测全流程从传感器交叉验证到状态机3.1 为什么打滑检测是轮速计融合的第一道闸门轮速计融合系统最怕的不是精度差而是短暂失灵。机器人急加速、急转弯、压过水洼、上坡起步时轮子会发生滑转编码器记录的轮速远大于车体实际移动速度。如果这时候轮速计还以正常协方差参与滤波器更新状态估计会沿着错误方向被拉过去。更麻烦的是打滑不像传感器失效那样是二值的它是渐变过程可能同一个轮子滑转率从零迅速升到50%也可能在地面附着力恢复时瞬间消失。所以检测系统不能只做二值判断要在“完全正常”和“完全失效”之间留过渡带。3.2 三种交叉验证手段与阈值标定我实际测试下来最可靠的打滑检测不是靠单一信号而是交叉验证三个方向信号同时看第一个是IMU加速度比对。正常行驶时车体加速度应当等于轮速导数。用差分计算轮速的加速度再和IMU加速度计测得的水平加速度比较两者差值如果超过设定阈值说明轮子相对地面在打滑。注意这里要用经过重力对齐和低通滤波后的IMU平动加速度原始高频噪声直接对比会导致大量误报。第二个是IMU角速度比对。差速底盘正常转向时车体偏航角速度应当等于左右轮速度差除以轮距。IMU陀螺仪直接输出角速度两者残差如果持续超过阈值说明某个轮子在地面失去附着力。第三个是激光里程计交叉验证。Fast-LIO2在激光退化不严重时给出的短期速度估计本身是靠谱的可以拿它作为参考真值来标定打滑阈值。具体做法是跑一段正常直线和几个急转弯统计轮速计速度与激光速度之间的差值的标准差取3倍标准差作为阈值初值再在实际打滑场景里验证覆盖率。3.3 打滑状态机设计与代码骨架我在实现时最终采用了三状态状态机NORMAL正常融合 → 检测到打滑 → HOLD保持禁用 → 连续正常一段时间 → RECOVER恢复融合 → NORMAL相比直接二值切换这套状态机的好处是避免在打滑和正常之间反复横跳。轮速计在打滑临界状态下可能每隔几十毫秒就抖进抖出如果每次都立即启用融合滤波器会频繁收到不稳定的观测噪声效果反而更差。状态机代码如下我在项目里直接用的这个逻辑class SlipDetector: def __init__(self): self.state NORMAL self.bad_count 0 self.good_count 0 self.threshold_bad 3 # 连续若干次超阈值判定为打滑 self.threshold_good 5 # 连续若干次正常才允许恢复 def update(self, residual_acc, residual_omega, residual_v, dt): slip (residual_acc 0.8) or (residual_omega 0.4) or (residual_v 0.25) if slip: self.bad_count 1 self.good_count 0 else: self.good_count 1 self.bad_count 0 if self.state NORMAL and self.bad_count self.threshold_bad: self.state HOLD return False elif self.state HOLD and self.good_count self.threshold_good: self.state NORMAL return True return self.state NORMAL这段代码里的阈值加速度0.8m/s²、角速度0.4rad/s、速度0.25m/s只是我当前底盘的标定值真要往别的底盘搬务必要重新标定。3.4 打滑阈值现场标定实操第一次跑这套状态机时我犯了先定阈值后标定的错误以为阈值能凭空拍脑袋定。跑了几圈下来误报和漏报都碰了一轮才整理出一套可行的标定流程。先把机器人在附着良好、干燥的水泥地上跑一遍记录正常工况下三类残差的统计分布然后在相同地面做急加速和急转弯记录打滑工况下的残差分布。两个分布之间的分界点就是阈值初值先把阈值放在两类分布均值的中间位置再跑真实场景看误报率。之后在湿滑地面上重复同样流程。我建议在条件允许时多试几种地面瓷砖、水泥、环氧地坪、塑胶跑道不同地面的附着系数差异很大打滑残差出现突变的幅度也不同。真正上线的时候把湿滑地面的打滑阈值设成最低值宁可信打滑不可信不打滑最多只是损失轮速计融合带来的提升但至少不会让定位结果被带飞。提示千万别为了减少误报就把打滑阈值调得过高。我犯过的最大错误就是阈值调太松导致急转弯时轮速计数据被正常融合进去轨迹在弯道出现明显的“甩尾”形状后期想修正非常痛苦。4. 协方差初始值与现场调参方法4.1 从编码器硬件参数推导协方差初值协方差调参前先要把初值定在一个物理合理的范围内。很多做融合的人看到调参就把协方差放大器当旋钮来回拧但真正靠谱的思路是先从传感器硬件参数出发给出一个符合物理意义的初值再基于实测数据做小范围微调。编码器测速的误差来源主要是量化误差和轮子半径误差。以我用的1024线增量编码器为例四倍频后一圈4096个脉冲轮子直径0.24m轮周长0.754m每个脉冲对应的弧长是0.754 / 4096 ≈ 0.000184m。如果输出频率是50Hz每个周期的量化步长对应速度约为0.0092m/s。假设误差在量化步长内均匀分布方差按均匀分布计算是步长的平方除以12算出来量级大约是7×10⁻⁶(m/s)²。这个数值几乎没有参考价值因为实际误差远不止量化误差还有轮子打滑、轮胎形变、轮径磨损带来的误差。实际工程中我通常把初始协方差设成实测速度噪声标准差的平方在平坦地面匀速直行记录轮速计速度与激光参考速度的残差计算标准差取平方作为v的初始协方差。角速度观测的协方差类似只是换用IMU角速度和轮速差角速度的残差。4.2 IMU与激光观测协方差的对应关系轮速计观测协方差不是孤立存在的要和其他观测来源匹配。IESKF里不同传感器观测本质上是通过信息矩阵协方差矩阵的逆来竞争权重的如果你把轮速计协方差设得特别小意味着你认为它极其可靠那么在激光观测退化时系统会偏好轮速计预测的状态反过来如果激光特征很丰富系统应该依然以激光为主。我个人的调参目标是让轮速计在正常工况下“有一点话语权但不能喧宾夺主”。具体做法是先把激光观测协方差设为Fast-LIO2原版参数跑一段包含长走廊和开阔区域的路径记录激光退化区段的状态精度。然后逐步缩小轮速计协方差每调整一次就重跑同一段路径观察退化区域的位姿误差变化。实际操作中有个很好用的判断指标在直线直行段如果融合轮速计后车辆横向抖动增加说明轮速计权重过大它把地面很小的不平整当成真实速度扰动导致横摆方向被过分修正。这时候应该增大轮速计线速度的协方差。4.3 现场调参的六个检查点我把现场调参的经验总结成六个检查点照着这个顺序过一遍基本能稳住大部分场景第一确认打滑状态机在正常路段不会被误触发。如果频繁误触发说明交叉验证阈值偏低或者IMU数据没有做好低通滤波。第二把轮速计协方差先设为激光协方差对应信息矩阵的1/10观察融合后位姿是否稳定不变差。第三在长走廊里往返跑两遍观察航向角是否对称。如果来回两次系统性偏差方向相反基本都是轮距偏差或者轮速计角速度标定有问题不是协方差问题。第四急加速起步再急刹停看打滑检测是否能被触发。如果不能触发说明打滑阈值太高这套状态机形同虚设。第五上坡和下坡各跑一段观察竖直方向位置误差是否被轮速计引入异常。轮速计理论上不参与竖直方向约束但运动学模型里线速度会影响到水平面速度估计进而经由姿态耦合改变竖直位置要特别留意。第六全路径来回跑三遍用同一轨迹做一致性对比把位置漂移控制在可接受范围内后再对不同场景微调阈值。4.4 参数敏感性速查我把实际调参过程中踩过的几种现象整理成了速查表方便后面排障现象大概率原因调整方向直线段横向抖动明显轮速计线速度协方差过小增大线速度协方差长走廊位姿Y呈弧形漂移轮距标定不准重新标定轮距而不是调协方差急转弯轨迹出现甩尾打滑检测阈值过高或协方差过大调低打滑阈值减小角速度协方差融合后精度反而下降轮径磨损或充气不足检查轮径标定不要急着调协方差打滑状态机频繁震荡交叉验证阈值和状态机参数不匹配增大转态切换的迟滞时间全局位姿在开阔区缓慢漂移激光退化但轮速计权重不足适当减小轮速计线速度协方差这张表不是万能答案但每次调参前翻一遍至少能防止把问题弄拧了。5. 典型问题实录与排查速查表5.1 打滑检测失灵的五种现场表现第一种是起步滑转检测慢。机器人原地起步瞬态扭矩最大轮子最容易出现零点几秒的滑转。如果状态机按帧累积每帧检测到滑转后要连续3帧才进入HOLD这中间三五帧已经被正常融合进去了。解决办法是在状态机里针对起步阶段单独做触发逻辑检测到轮速突增且IMU速度几乎为零时立刻进入HOLD状态不等计数累积。第二种是低速爬坡误报。低速爬坡时轮速计速度和IMU加速度比对会存在静态偏差因为重力分量被IMU加速度计看到而轮速计看不到。如果比对时不把重力分量扣除就会误判成打滑。我在实现时只拿水平面内的速度做比对竖直方向彻底不看。第三种是高速急刹时编码器无输出。急刹时轮子可能抱死编码器瞬时输出为零车体还在滑行这时候速度残差会巨大。检测逻辑要防住这个靠的是状态机的HOLD窗口即使残差从大变小也要维持一段时间的禁用避免刚刚判定打滑就立刻恢复融合。第四种是弹跳路段。路面有坑洼时车轮可能短时离地编码器转速要么突增要么突降。这个时候激光里程计往往也是乱的交叉验证反而全部失灵。我的处理比较粗暴如果三个交叉验证通道里至少两个通道同时残差过大直接判定为打滑并保持HOLD直到所有通道连续正常超过0.5秒才恢复。第五种是橡胶轮磨损后打滑特征变化。同一辆车的轮子跑了几百公里后轮胎表面温度升高附着系数会变打滑特征和干燥新轮胎有本质区别阈值如果不更新检测漏报概率会逐步上升。这个问题只能定期重新标定没有一劳永逸的方案。5.2 协方差调参引发的滤波器病态现象协方差调参不当会直接引发滤波器数值不稳定我在实验里观察到的典型现象有两个。第一个是信息矩阵奇异。当轮速计协方差设得极小观测方程又和激光观测高度相关时两个观测提供的信息可能近似线性相关信息矩阵条件数飙升卡尔曼增益计算出现数值误差状态估计会出现无规律的跳变。处理方式是在滤波器的信息矩阵累加时做一次条件数检查超过阈值就把轮速计协方差扩大一个数量级再重新叠加。第二个是协方差提前收敛导致的状态“锁死”。如果IMU过程噪声Q设置得过小轮速计协方差一开始就取得很小滤波器前几十帧就会把状态方差压得非常小之后无论观测质量如何增益矩阵都会趋近于零系统对外界修正响应变得极其迟钝。这种“锁死”在实车上的表现是激光已经检测到明显的回环修正但位姿就是不动。排查时先看IESKF的状态协方差对角元是否在几百帧内缩小了几个数量级如果是说明Q太小或初始状态方差太小需要相应地放大系统噪声。5.3 我踩过的坑与最后的稳定配置参考写这篇文章前我在实验室里搭了一套比较标准的差速底盘做回归测试底盘参数如下轮径0.24m轮距0.41m编码器1024线四倍频IMU为BMI088激光雷达为16线机械式Fast-LIO2跑在NUC上传感器消息统一走ROS。最后稳定下来的轮速计部分参数是线速度协方差为(0.05m/s)²量级角速度协方差为(0.06rad/s)²量级打滑检测三通道阈值分别为加速度差0.6m/s²、角速度差0.35rad/s、线速度差0.2m/s状态机进入HOLD需要连续3帧异常恢复需要连续8帧正常。这套参数在长走廊、开阔广场、室内办公区三种场景下都跑出了比原版Fast-LIO2更稳的结果尤其在长走廊场景原来横向漂移最大0.35m融合后降到0.08m。直观感受是滤波器在激光退化时不再慌张被打滑干扰时也能及时抽身不会让单帧坏数据污染后续几百帧状态。最后再分享一个调整技巧调试打滑检测和协方差时一定要把轮速计融合前后的位姿曲线、残差曲线、打滑标志同时录下来离线回放比对。联调阶段我在现场盯着RVIZ看了半天才定位到问题后来改成录包离线分析效率起码快了三倍。调融合这种事儿先看数据再调代码最后才是调参数。