3、抖动成因分析（下）：运动模型误差、轮速计打滑、地面不平整、SLAM前端匹配误差、后端优化收敛不足
上一讲我们聊了传感器本身的噪声。今天继续挖剩下的几个坑——运动模型、轮速计、地面、前端匹配和后端优化。这些因素叠加起来，才是轨迹抖动的真正全貌。
核心观点：轨迹抖动不是单一原因造成的。它是多个误差源在时间轴上的叠加与放大。你只修一个环节，往往治标不治本。
3.1 运动模型误差：你猜的路径不准
运动模型说白了就是「根据上一帧的状态，猜这一帧机器人该在哪儿」。但猜的永远不是真的。
我见过不少团队，直接用匀速模型（CV）或匀加速模型（CA）做预测。室内低速小车还好说，一旦遇到急转弯、急刹车，模型瞬间崩掉。为什么？因为真实运动根本不是匀速的。
| 模型类型 | 适用场景 | 抖动风险 |
|---|---|---|
| 匀速模型 (CV) | 直线巡航、低速 | 转弯时抖动明显 |
| 匀加速模型 (CA) | 加减速频繁 | 急停时过冲抖动 |
| 恒速转弯模型 (CTRV) | AGV/AMR 常见 | 直线段仍存在误差 |
我个人习惯是：不要只用一种模型。在滤波框架里，我会根据 IMU 角速度实时切换模型权重。比如角速度大于阈值时，加大 CTRV 的权重；角速度接近零时，切回 CV。这样能减少模型切换带来的跳变。
小技巧：运动模型误差导致的抖动，往往在转弯结束后的 3-5 帧最明显。你可以在这几帧里临时增大观测噪声协方差，让滤波器「不那么相信预测」，抖动就能压下去不少。
3.2 轮速计打滑：你测的里程是假的
轮速计打滑，是移动机器人最头疼的问题之一。我在做仓储机器人时遇到过：地面刚拖过，有点湿，机器人直行 10 米，轮速计报了 12 米。结果轨迹直接飘出去 2 米，然后被激光匹配硬拉回来——一拉一回，就是一次剧烈抖动。
打滑的本质是：轮子转了，但车没动。轮速计以为你走了，实际上你在原地空转。这会导致里程增量被高估，轨迹被「撑大」。
怎么判断打滑？我一般用两个指标：
- 轮速 vs IMU 加速度对比：轮速显示加速很快，但 IMU 加速度很小，大概率打滑。
- 左右轮速差异常：直行时左右轮速差突然变大，说明一侧打滑。
避坑指南：我曾经在项目中直接丢弃打滑时刻的轮速数据，结果导致位姿更新中断，轨迹出现空洞。后来改成「降低权重但不丢弃」——把打滑帧的轮速协方差放大 10 倍，让滤波器自动降低它的信任度。效果比直接丢弃好得多。
3.3 地面不平整：你的假设错了
大多数轮式机器人的运动模型，都假设地面是平的。但现实世界哪有那么平？
地面不平整带来的问题有两个：
- 高度方向上的抖动：轮子碾过小石子或地砖接缝，车体上下颠簸。如果你用 2D SLAM，这个抖动会直接映射到 x-y 平面，造成水平抖动。
- 轮速计有效半径变化：地面不平导致轮子实际滚动半径不断变化。轮速计按固定半径算里程，误差就来了。
我记得有一次在工厂调试，地面有 5mm 的起伏，机器人每走 1 米就抖一下。查了半天，最后发现是地面问题。解决方案其实不复杂：给轮速计加一个地面自适应补偿。根据 IMU 的俯仰角变化，实时修正轮速计的有效半径。
经验值：地面起伏超过 3mm，2D SLAM 的轨迹抖动就会明显增加。如果条件允许，优先用 3D SLAM + 轮速计 + IMU 紧耦合，能天然吸收地面不平的影响。
3.4 SLAM 前端匹配误差：你找的对应点不对
前端匹配，不管是 ICP、NDT 还是特征匹配，本质都是找两帧之间的对应关系。对应关系找错了，位姿就偏了，轨迹就抖了。
前端匹配误差的典型场景：
- 重复结构：在走廊、货架区，激光扫描到的点云高度相似。匹配算法容易「认错」对应点，产生随机跳变。
- 动态物体：行人、叉车经过，点云里多了不该有的点。匹配结果被带偏。
- 稀疏环境：空旷场地，有效特征点太少。匹配自由度约束不足，位姿在无约束方向上漂移。
我个人的做法是：给匹配结果加一个「置信度打分」。如果匹配得分低于阈值，就不直接用这个结果，而是用运动模型预测的值顶上去。等下一帧匹配好了再拉回来。虽然会引入一点点延迟，但能避免单帧剧烈抖动。
具体实现：在 ICP 里，我习惯用匹配的均方根误差（RMSE）和内点比例两个指标做联合判断。RMSE 小于 0.05m 且内点比例大于 80%，才认为匹配可靠。否则，降低这一帧的权重。
3.5 后端优化收敛不足：你算的轨迹没稳住
后端优化（图优化、位姿图优化）是 SLAM 的「总装车间」。前端给一堆相对位姿，后端把它们拧成一条平滑轨迹。但如果后端没收敛好，轨迹就会「拧歪了」。
收敛不足的原因：
- 迭代次数不够：有些实时系统为了保帧率，只跑 5-10 次迭代。图还没稳住就停了。
- 初始值太差：后端优化是迭代法，初始值离真值太远，容易陷入局部最优。
- 信息矩阵设置不合理：不同约束的权重没调好。比如回环约束太弱，拉不住累积漂移。
我记得有个项目，后端优化后轨迹还是抖。查了半天，发现是信息矩阵里轮速计的权重设得太高了。轮速计本身有打滑误差，权重一高，反而把回环约束给「顶」开了。后来把轮速计的信息矩阵乘以 0.3，回环约束权重保持不变，轨迹一下就稳了。
注意：后端优化收敛不足导致的抖动，往往表现为「低频大振幅」——轨迹整体漂移，而不是高频小抖动。如果你看到轨迹像蛇一样慢慢扭动，优先检查后端收敛状态。
3.6 知识体系总览
下面这张图把今天讲的五个成因串起来了。你可以看到，每个环节的误差都会沿着 pipeline 向下传递，最终在轨迹上表现出来。
3.7 小结：五个成因的排查优先级
如果你现在正被轨迹抖动折磨，不知道从哪下手，我建议按这个顺序排查：
- 先看轮速计：打滑是最容易确认的。录一段数据，对比轮速里程和激光匹配里程，偏差超过 10% 基本就是打滑。
- 再看地面：IMU 俯仰角标准差超过 1°，地面不平大概率是主因。
- 检查前端匹配：把匹配得分和 RMSE 打出来，看抖动时刻的匹配质量是不是明显下降。
- 检查后端收敛：看优化后的残差是否还在下降。如果迭代结束时的梯度还很大，说明没收敛。
- 最后看运动模型：模型误差通常影响最小，但如果你前面四个都查过了没问题，那可能就是模型选错了。