企业官网建设流程全解析

搞机器人定位的人，十有八九都在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::Matrix<double, 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::Matrix<double, 2, 1> r; r = wheel_meas - h; // 雅可比矩阵 Eigen::Matrix<double, 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 参数敏感性速查

我把实际调参过程中踩过的几种现象整理成了速查表，方便后面排障：

这张表不是万能答案，但每次调参前翻一遍至少能防止把问题弄拧了。

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看了半天才定位到问题，后来改成录包离线分析，效率起码快了三倍。调融合这种事儿，先看数据，再调代码，最后才是调参数。