4、里程计与定位调试：轮式里程计模型、IMU数据融合、EKF配置与调参、定位漂移排查
定位，是AMR的命根子。车跑得再快，定位不准，一切都是白搭。我调试过的机器人里，十台有八台的“疑难杂症”最后都归结到定位问题上。这一章，咱们就把轮式里程计、IMU融合、EKF配置这些硬骨头啃下来。
4.1 轮式里程计模型：从轮子转圈到位置变化
轮式里程计，说白了就是靠轮子转了多少圈，来推算车走了多远。听起来简单，但坑不少。
差分驱动模型是最常见的。两个驱动轮，一个左轮一个右轮。车往前走，左右轮转速一样。车转弯，左右轮就有速度差。
核心公式就两个：
线速度 V = (V_left + V_right) / 2
角速度 ω = (V_right - V_left) / wheel_base这里的 wheel_base 是左右轮之间的距离。我遇到过一台车，标称轮距是0.5米，但实际跑起来转弯半径总是不对。后来一量，实际轮距是0.48米。就差了2厘米，定位误差累积起来吓死人。
关键参数：
- 轮距（wheel_base）：左右轮中心距离，直接影响角速度计算
- 轮径（wheel_diameter）：轮子实际直径，影响线速度计算
- 编码器分辨率（ticks_per_rev）：轮子转一圈，编码器输出多少个脉冲
里程计模型还有个变种——阿克曼模型，就是汽车那种前轮转向的结构。这种模型计算更复杂，但原理一样：通过轮速和转向角推算位姿。
嗯，这里要注意：轮式里程计有个天生的缺陷——打滑。地面有水、有油、有沙子，轮子空转，里程计就“虚报”位移了。我见过最夸张的一次，机器人在湿滑地面上原地打转，里程计却报告它走了两米远。
4.2 IMU数据融合：给里程计装上“陀螺仪”
光靠轮子不行，得加上IMU。IMU提供角速度和加速度，能弥补轮式里程计的短板。
IMU数据融合，我习惯用互补滤波做第一道处理。为什么？因为IMU有高频噪声，但短期精度高；里程计有低频漂移，但长期稳定。互补滤波就是把两者的优点结合起来。
// 互补滤波伪代码
angle = 0.98 * (angle + gyro_y * dt) + 0.02 * accel_angle;这个0.98和0.02的比例，是我调了无数遍才找到的“黄金比例”。当然，不同IMU、不同底盘，这个系数要微调。我在一个项目里用过MPU6050，0.98/0.02就挺好。换了个工业级的IMU，系数得改成0.995/0.005。
我的经验：IMU安装位置很重要。装在底盘中心，振动最小。装在边缘，转弯时的离心力会引入额外噪声。我曾经把IMU装在车头，结果每次加速都读到很大的“俯仰角”，其实是加速度计被线加速度干扰了。
IMU数据融合还有个关键点——坐标系对齐。IMU的坐标系和机器人的坐标系必须一致。如果IMU装歪了，读出来的角度就是错的。我一般会在调试阶段打印IMU原始数据，让机器人原地转一圈，看看三个轴的数据变化是否符合预期。
4.3 EKF配置与调参：状态估计的“心脏”
EKF（扩展卡尔曼滤波）是定位融合的核心。它把里程计、IMU、甚至激光雷达的数据揉在一起，输出一个最优的位姿估计。
EKF的配置，说白了就是调两个矩阵：过程噪声协方差矩阵Q和测量噪声协方差矩阵R。
| 参数 | 含义 | 调大 | 调小 |
|---|---|---|---|
| Q（过程噪声） | 模型预测的不确定性 | 更相信测量值 | 更相信预测值 |
| R（测量噪声） | 传感器测量的不确定性 | 更相信预测值 | 更相信测量值 |
调参没有标准答案，但我有个“三步法”：
- 先粗调：把Q和R都设成单位矩阵的倍数，跑起来看看效果
- 再细调：哪个传感器不准，就把它的R调大。比如轮子打滑严重，就把里程计的R调大
- 最后验证：让机器人走一个闭环路径，看终点和起点的误差
警告：EKF不是万能的。如果传感器数据本身就有系统性偏差，EKF只会把偏差“平滑”掉，但不会消除。我曾经遇到过激光雷达安装位置有5厘米偏移，EKF怎么调都调不好，最后发现是机械安装问题。
EKF的更新频率也很关键。我一般让里程计以50Hz更新，IMU以100Hz更新，激光雷达以10Hz更新。频率太低，定位会“卡顿”；频率太高，CPU吃不消。
4.4 定位漂移排查：从现象到根因
定位漂移，是AMR调试中最头疼的问题。车跑着跑着，位置就偏了。怎么排查？我有一套“排查清单”：
- 第一步：检查里程计。让机器人走直线，看里程计报告的位移和实际位移是否一致。不一致？检查轮径、编码器分辨率、轮距。
- 第二步：检查IMU。让机器人静止，看IMU输出的角速度是否接近0。有偏差？做零偏校准。
- 第三步：检查时间戳。传感器数据的时间戳必须同步。我遇到过里程计和IMU的时间戳差了50毫秒，定位直接飘到天上去。
- 第四步：检查EKF参数。Q和R调得对不对？尤其是转弯时的角速度噪声，很多人会忽略。
漂移排查口诀：
先看轮子后看陀螺，时间同步不能忘。
Q大R小信测量，Q小R大信预测。
闭环验证是王道，漂移大小看终点。
我曾经排查过一个“幽灵漂移”问题。机器人每次走到同一个位置，定位就会突然跳变。查了三天，最后发现是那个位置的地面有一块金属板，干扰了磁传感器。嗯，这种环境因素导致的漂移，最难查。
还有一个常见原因——轮子磨损。新轮子和旧轮子的直径不一样，里程计参数却没更新。我建议每三个月校准一次轮径，尤其是全向轮，磨损更快。
最后说一句：定位调试没有捷径。你得把每个环节都摸透，从传感器原始数据到融合算法，一步步排查。我见过太多人一上来就调EKF参数，结果发现是轮子没拧紧。先做基础检查，再动高级参数，这是铁律。
公众号:蓝海资料掘金营，微信deep3321