4、定位系统调试（AMCL）：粒子滤波原理、初始位姿设置、粒子发散与收敛问题、里程计标定对定位的影响
4.1 AMCL 粒子滤波到底在干什么？
说实话，很多初学者一听到「粒子滤波」四个字就头大。我当年也是。
其实你把它想简单点：AMCL 就是让一群「虚拟机器人」在空间里瞎猜自己的位置。每个虚拟机器人就是一个「粒子」，它带着一个坐标 (x, y, yaw) 和一个权重。权重越高，说明它越像真的机器人。
算法每跑一次，就做三件事：
- 预测：根据里程计数据，让所有粒子往前走一步。里程计有误差，所以这一步会加一点随机噪声。
- 更新：激光雷达扫到一堵墙，跟地图一比对。哪个粒子看到的墙跟实际最像，它的权重就变高。
- 重采样：扔掉权重低的粒子，复制权重高的粒子。粒子们慢慢就聚到一起了。
嗯，这里要注意：粒子数量不是越多越好。我见过有人设 5000 个粒子，结果 CPU 跑满，定位还没好。一般室内场景 500~2000 个就够用了。
核心参数速查表：
| 参数名 | 作用 | 我常用的值 |
|---|---|---|
| max_particles | 粒子数量上限 | 2000 |
| min_particles | 粒子数量下限 | 500 |
| update_min_d | 触发更新的最小位移 | 0.2 米 |
| update_min_a | 触发更新的最小转角 | 0.2 弧度 |
| resample_interval | 重采样间隔（次数） | 1 |
4.2 初始位姿设置——别让机器人「裸奔」
你想想看，如果 AMCL 不知道机器人大概在哪，它就得在全地图撒粒子。地图一大，粒子根本覆盖不过来。
我习惯的做法是：先给一个粗略的初始位姿，再让 AMCL 自己去收敛。比如用 2D Pose Estimate 点一下，或者从上一帧保存的位姿加载。
这里有个坑：初始位姿的 yaw 角度一定要给对。方向错了 180 度，粒子全往反方向跑，永远收敛不了。我曾经在一个仓库项目里，就因为初始角度差了 90 度，折腾了两天。
我的小技巧：
启动时先让机器人原地转一圈。这样激光雷达能扫到周围环境，AMCL 的粒子能快速收敛。说白了就是「让机器人自己看一眼周围再猜自己在哪」。
4.3 粒子发散与收敛——定位的「心跳」
粒子发散，说白了就是机器人不知道自己在哪了。你会在 RViz 里看到粒子散成一大片，像炸开的烟花。
为什么会这样？常见原因有三个：
- 里程计突然跳变：比如轮子打滑，或者编码器丢脉冲。粒子跟着里程计跑偏了。
- 环境对称：走廊、长过道，左右看起来一样。AMCL 分不清自己在左边还是右边。
- 激光数据异常：被遮挡、反光、或者干脆没扫到特征。
怎么解决？我一般先看 /amcl_pose 话题的协方差。协方差突然变大，说明定位在漂。这时候可以：
- 增加
recovery_alpha_slow和recovery_alpha_fast参数，让 AMCL 在发散时自动增加随机粒子。 - 或者手动触发一次全局重定位。
注意：不要一看到粒子发散就加粒子数量。我曾经试过把粒子数加到 10000，结果 CPU 满载，定位反而更慢。先找根因，再调参数。
4.4 里程计标定——定位的「地基」
里程计不准，AMCL 再强也救不了。你想想看，粒子预测这一步全靠里程计。里程计偏了 10%，粒子就偏 10%。
我遇到过最典型的情况：机器人直走 10 米，里程计显示走了 10.5 米。结果 AMCL 的粒子一直往前飘，最后定位完全跑偏。
里程计标定其实不复杂。我常用的方法是：
- 让机器人走一个矩形路径，比如 2 米 x 2 米。
- 记录实际终点和里程计计算的终点。
- 用
robot_pose_ekf或者imu_filter_madgwick来修正。
如果你用差速轮，重点标定两个参数：
- 轮距（wheel_separation）：影响转弯精度。轮距偏大，转弯半径就偏大。
- 每米脉冲数（ticks_per_meter）：影响直线精度。这个值偏小，里程计显示的距离就偏短。
一个简单的标定公式：
实际轮距 = 理论轮距 × (实际转角 / 里程计转角)
实际每米脉冲 = 理论每米脉冲 × (里程计距离 / 实际距离)嗯，这个公式我用了好几年，简单粗暴但有效。
4.5 一张图看懂 AMCL 调试流程
下面这张图是我自己总结的调试思路。每次遇到定位问题，我就按这个流程走一遍，基本都能找到问题在哪。
4.6 实战中的几个小经验
最后分享几个我在项目里踩过的坑：
- 不要迷信默认参数：AMCL 的默认参数是针对 PR2 机器人调的。你的机器人轮距、传感器都不一样，一定要重新调。
- 激光雷达的安装高度很重要：装得太低，扫不到环境特征；装得太高，容易扫到天花板。我一般装在离地 30~40 厘米。
- 多看看 /tf 树：有时候定位不准，不是 AMCL 的问题，而是 tf 树没连对。用
tf2_monitor看一眼，一目了然。
一个快速验证方法：
在 RViz 里打开 /particlecloud 话题。如果粒子聚成一团，且跟随机器人移动，说明定位基本正常。如果粒子散开或者不动，那就要按上面的流程排查了。
嗯，定位调试说难也难，说简单也简单。核心就是：先保证里程计准，再保证初始位姿对，最后看环境特征够不够。按这个顺序来，大部分问题都能搞定。