4、定位系统故障排查：AMCL定位丢失与粒子发散、EKF/UKF滤波器发散、全局定位与局部定位冲突、重定位策略失效

定位系统，说白了就是机器人的「眼睛」和「大脑」之间的桥梁。眼睛看到的东西，大脑得知道自己在哪儿，才能决定下一步怎么走。我在项目里见过太多因为定位问题导致机器人「原地转圈」或者「撞墙」的案例了。今天咱们就聊聊定位系统里最常见的几个坑，以及怎么填。

4.1 AMCL定位丢失与粒子发散

AMCL（自适应蒙特卡洛定位）是ROS2里最常用的2D定位方法。它的核心思想是用一堆粒子来猜测机器人的位置。正常情况下，粒子会聚集在机器人真实位置附近。但有时候，粒子会「炸开」，散得到处都是——这就是粒子发散。

为什么会发生粒子发散？

• 初始位姿不对：启动时给的初始位姿偏差太大，粒子根本覆盖不到真实位置。
• 传感器数据异常：激光雷达被遮挡、数据跳变，或者里程计打滑。
• 地图与真实环境不匹配：比如地图里有一堵墙，但实际墙被拆了。
• 参数设置不合理：粒子数量太少，或者运动模型噪声参数太小。

怎么排查？

我个人习惯先看/amcl_pose话题的粒子分布。用rviz2打开粒子显示，如果粒子散成一团，说明定位已经丢了。这时候我会检查以下几点：

1. 检查初始位姿：用2D Pose Estimate工具重新给一个粗略的初始位姿。记得要尽量准确。
2. 检查传感器数据：看/scan话题有没有异常。我在项目中遇到过激光雷达被灰尘遮挡，导致数据全是0的情况。
3. 调整AMCL参数：

# 增加粒子数量
amcl:
  ros__parameters:
    max_particles: 3000
    min_particles: 500
    # 增大运动模型噪声
    odom_alpha1: 0.2
    odom_alpha2: 0.2
    odom_alpha3: 0.2
    odom_alpha4: 0.2

4.2 EKF/UKF滤波器发散

EKF（扩展卡尔曼滤波）和UKF（无迹卡尔曼滤波）常用于融合多个传感器数据。滤波器发散，说白了就是估计值和真实值越差越远，最后完全对不上。

滤波器发散的典型表现：

• 机器人明明在往前走，但定位显示它在后退。
• 机器人转弯时，定位角度突然跳变。
• 协方差矩阵的值越来越大，甚至变成无穷大。

为什么会发散？

我记得有一次，项目里的机器人定位突然崩了。查了半天，发现是IMU的安装方向反了。EKF把IMU数据当成正确的，结果越算越偏。嗯，这里要注意：

• 传感器标定错误：IMU的安装角度、里程计的轮距等参数不对。
• 噪声协方差设置不合理：过程噪声和测量噪声的比值失衡。
• 数据时间戳不同步：不同传感器的时间戳相差太大，滤波器会「混乱」。

排查步骤：

1. 检查传感器标定：用imu_tools查看IMU数据是否正常。我习惯先让机器人静止，看IMU的角速度和加速度是否接近0。
2. 调整协方差参数：

# 增大测量噪声，让滤波器更信任预测
ekf_filter_node:
  ros__parameters:
    # 测量噪声协方差
    imu0:
      noise_covariance: [0.1, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.1]
    # 过程噪声协方差
    process_noise_covariance: [0.05, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.05]

4.3 全局定位与局部定位冲突

全局定位（比如AMCL）和局部定位（比如里程计+IMU）给出的位置不一致，这是很常见的问题。你想想看，全局定位说机器人在(1, 1)，局部定位说它在(2, 2)，机器人该听谁的？

冲突的原因：

• 坐标系定义不一致：全局坐标系和局部坐标系的原点、方向定义不同。
• 累积误差：局部定位长时间运行后，误差会越来越大。
• 地图更新不及时：全局定位依赖地图，如果地图变了，全局定位会突然跳变。

怎么解决？

我建议的做法是：

1. 统一坐标系：确保所有传感器和定位算法使用同一个map坐标系。
2. 设置优先级：在导航中，通常让全局定位（AMCL）作为最终参考，局部定位只用于短时间内的平滑运动。
3. 使用协方差判断：如果AMCL的协方差很大，说明它不可靠，这时候可以暂时信任局部定位。

4.4 重定位策略失效

重定位，就是机器人丢失位置后重新找回自己的过程。但有时候，重定位策略会失效——机器人明明在A点，却认为自己到了B点。

失效的常见原因：

• 重定位参数设置不当：比如recovery_behavior_enabled设为false。
• 环境特征不足：在长走廊、空旷场地等特征少的地方，重定位很难成功。
• 重定位算法选择错误：AMCL的重定位依赖于粒子滤波，如果粒子数量太少，很难收敛到正确位置。

我的经验：

我曾经在一个仓库项目里遇到重定位失效的问题。仓库里全是货架，特征很丰富，但机器人就是找不到自己。后来发现，是因为货架经常移动，地图和实际环境对不上。解决办法是：

1. 增加重定位尝试次数：让机器人多试几次，每次用不同的初始位姿。
2. 使用辅助传感器：比如加一个UWB标签，或者用视觉特征点辅助定位。
3. 手动干预：如果自动重定位失败，就通过遥控器把机器人开到已知位置，然后手动设置位姿。

# 启用重定位行为
amcl:
  ros__parameters:
    recovery_behavior_enabled: true
    recovery_behavior_min_dist: 0.1
    recovery_behavior_max_dist: 1.0

4.5 知识体系总览

下面这张图总结了定位系统故障排查的核心逻辑。你可以把它当作一个「故障树」，从问题出发，一步步找到根因。

这张图把定位系统的四个主要故障类型和它们的根因都列出来了。你排查的时候，可以顺着这个树形结构，从根节点往下走。先确定是哪种故障，再找对应的原因。

---