4、定位技术：蒙特卡洛定位（MCL）、自适应蒙特卡洛定位（AMCL）、卡尔曼滤波基础
定位，说白了就是回答三个问题：我在哪？我周围是什么？我下一步该往哪走？
在机器人导航里，定位是核心中的核心。没有准确的定位，路径规划再漂亮也是白搭。我刚开始做机器人项目时，就吃过这个亏——明明地图建得挺准，结果机器人一跑起来就撞墙。后来才发现，是定位算法没选对。
这一章，咱们就聊聊三种最常用的定位方法：蒙特卡洛定位（MCL）、自适应蒙特卡洛定位（AMCL），以及卡尔曼滤波的基础。它们各有各的脾气，也各有各的用武之地。
4.1 蒙特卡洛定位（MCL）——用粒子猜位置
蒙特卡洛定位，名字听着挺唬人。其实原理很简单：用一堆随机粒子来猜测机器人的位置。
你想想看，如果我问你“机器人在哪”，你可能会说“大概在走廊中间，偏左一点”。MCL 的做法更直接——它在地图上撒几千个粒子，每个粒子代表一个可能的位置。然后根据传感器数据，不断淘汰那些“看起来不对”的粒子，保留并繁殖那些“看起来靠谱”的粒子。
核心思想：粒子越多，定位越准；粒子越少，计算越快。这是一个典型的“精度 vs 速度”的权衡。
4.1.1 MCL 的工作流程
- 初始化：在地图范围内随机撒 N 个粒子，每个粒子权重相同。
- 预测：根据运动模型（里程计数据），让每个粒子移动一步。
- 更新：用激光雷达或摄像头数据，计算每个粒子的“可信度”（权重）。
- 重采样：淘汰权重低的粒子，复制权重高的粒子。粒子总数不变。
- 迭代：回到第 2 步，继续下一轮。
我在项目中遇到过一个问题：当机器人被“绑架”（突然被人搬到另一个地方）时，MCL 会彻底懵掉。因为所有粒子都集中在原来的位置，而实际位置已经变了。这时候，粒子会慢慢扩散，重新收敛——但这个过程可能需要好几秒。
小技巧：如果机器人经常被“绑架”，可以在重采样时故意加入少量随机粒子（比如 5%）。这样即使所有粒子都错了，也能快速恢复。
4.1.2 MCL 的优缺点
| 优点 | 缺点 |
|---|---|
| 能处理非线性、非高斯分布 | 计算量大，粒子数影响实时性 |
| 实现简单，容易理解 | 粒子退化问题（权重集中在少数粒子） |
| 适合全局定位和绑架恢复 | 高维空间下效率极低 |
4.2 自适应蒙特卡洛定位（AMCL）——聪明的 MCL
AMCL 是 MCL 的升级版。它解决了一个实际问题：粒子数量到底该设多少？
设少了，定位不准；设多了，CPU 扛不住。AMCL 的做法是——动态调整粒子数。
我记得第一次用 AMCL 时，心里还挺没底的。结果跑起来发现，机器人在地图中间时，粒子数自动降到几百个；一到走廊尽头或者拐角，粒子数立刻飙到几千。嗯，这才是聪明的做法。
4.2.1 AMCL 的自适应机制
- KLD 采样：根据粒子分布的“熵”来决定粒子数。分布越集中，粒子越少；分布越分散，粒子越多。
- 短期平均权重：如果最近几轮的平均权重突然下降，说明定位可能出了问题，自动增加粒子数。
- 长期平均权重：如果长期权重稳定，说明定位可靠，可以适当减少粒子数。
注意：AMCL 虽然聪明，但也不是万能的。如果传感器数据噪声太大（比如激光雷达被遮挡），自适应机制可能会误判，导致粒子数忽高忽低。我曾经在仓库里遇到过这种情况——货架之间的激光反射太乱，AMCL 的粒子数一直在 100 到 5000 之间来回跳，CPU 直接拉满。
4.2.2 MCL vs AMCL 对比
| 特性 | MCL | AMCL |
|---|---|---|
| 粒子数 | 固定 | 动态调整 |
| 计算效率 | 中等 | 高（大部分时间） |
| 绑架恢复 | 慢 | 快（自动增加粒子） |
| 实现复杂度 | 低 | 中 |
4.3 卡尔曼滤波基础——线性世界的王者
如果说 MCL 是“广撒网”，那卡尔曼滤波就是“精准狙击”。
卡尔曼滤波假设系统是线性的，噪声是高斯分布。它用两个步骤——预测和更新——来不断修正对状态的估计。
我刚开始学卡尔曼滤波时，被那一堆矩阵公式搞得头大。后来发现，其实核心就五个公式。你只要理解了“预测”和“更新”这两个概念，剩下的就是套公式。
4.3.1 卡尔曼滤波的五个公式
- 预测状态：\( \hat{x}_{k|k-1} = F_k \hat{x}_{k-1|k-1} + B_k u_k \)
- 预测协方差：\( P_{k|k-1} = F_k P_{k-1|k-1} F_k^T + Q_k \)
- 卡尔曼增益：\( K_k = P_{k|k-1} H_k^T (H_k P_{k|k-1} H_k^T + R_k)^{-1} \)
- 更新状态：\( \hat{x}_{k|k} = \hat{x}_{k|k-1} + K_k (z_k - H_k \hat{x}_{k|k-1}) \)
- 更新协方差：\( P_{k|k} = (I - K_k H_k) P_{k|k-1} \)
说白了，卡尔曼滤波就是在“相信模型”和“相信传感器”之间找一个平衡点。卡尔曼增益 \( K \) 就是那个天平——传感器越准，\( K \) 越大；模型越准，\( K \) 越小。
关键理解：卡尔曼滤波不是“消除噪声”，而是“在噪声中找出最可能的真实值”。
4.3.2 卡尔曼滤波的适用场景
- 线性系统：比如轮式机器人，运动模型基本是线性的。
- 高斯噪声：传感器噪声符合正态分布（大部分传感器都满足）。
- 实时性要求高：卡尔曼滤波计算量小，适合嵌入式平台。
避坑指南：我曾经在一个项目中直接用卡尔曼滤波做定位，结果机器人一转弯就飘。后来才发现，转弯时系统是非线性的，卡尔曼滤波根本处理不了。最后换成了扩展卡尔曼滤波（EKF），问题才解决。所以记住：非线性系统别用标准卡尔曼滤波。
4.4 三种定位方法的选择策略
你可能会问：那我到底该用哪个？
我的建议是：
- 全局定位 / 绑架恢复：用 AMCL。粒子滤波天生适合处理多峰分布。
- 局部跟踪 / 平滑运动：用卡尔曼滤波。计算快，精度高。
- 资源受限的嵌入式平台：用卡尔曼滤波。粒子滤波的粒子数一多，CPU 就扛不住。
- 不确定环境 / 动态障碍物多：用 AMCL。自适应机制能应对突发情况。
其实很多实际项目会把两者结合起来——用 AMCL 做全局定位，用卡尔曼滤波做局部平滑。我在做仓储机器人时就是这么干的，效果还不错。
4.5 实战中的避坑指南
最后，分享几个我踩过的坑：
- 粒子数不是越多越好。我曾经为了追求精度，把粒子数设到 10000，结果机器人每走一步都要卡半秒。后来发现，对于大多数室内场景，2000-3000 个粒子就足够了。
- 卡尔曼滤波的 Q 和 R 矩阵要调参。Q 是过程噪声，R 是测量噪声。调不好，滤波要么发散，要么滞后。我习惯先让机器人静止，调 R 让滤波输出稳定；再让机器人运动，调 Q 让跟踪不滞后。
- AMCL 的“绑架恢复”不是万能的。如果机器人被搬到完全不同的楼层，AMCL 可能需要很长时间才能恢复。这时候，最好加一个“重定位”按钮，让用户手动触发全局重定位。
- 传感器数据质量比算法更重要。再好的定位算法，也救不了坏掉的激光雷达。我见过有人花几个月调参数，最后发现是传感器安装角度歪了 2 度。
重要提醒：定位算法不是越复杂越好。对于简单的轮式机器人，卡尔曼滤波可能就够用了。别一上来就上 AMCL，先问问自己：我的场景真的需要全局定位吗？