4. 定位与里程计：AMCL定位原理、自适应蒙特卡洛定位配置、里程计校准、融合传感器数据

定位，说白了就是回答三个问题：我在哪？我要去哪？怎么去？ 在ROS2导航系统中，AMCL（自适应蒙特卡洛定位）就是解决第一个问题的核心工具。我刚开始接触机器人时，总觉得定位很简单——有GPS不就行了？但室内机器人哪来的GPS信号？嗯，这就是AMCL登场的时候了。

4.1 AMCL定位原理

AMCL的全称是Adaptive Monte Carlo Localization，自适应蒙特卡洛定位。它的核心思想其实很朴素：用一堆随机粒子来猜测机器人的位置。

想象一下，你闭着眼睛在一个房间里，手里抓着一把豆子。每扔一颗豆子，就代表一个可能的位置。然后你摸到一堵墙，就把那些“认为自己在墙里”的豆子扔掉，剩下的就是更靠谱的位置。AMCL就是这么干的，只不过它用的是数学，不是豆子。

我在项目中遇到过一个问题：粒子数量设得太多，CPU直接拉满；设得太少，定位精度又不够。后来我学乖了——让AMCL自己决定粒子数量，这就是“自适应”的由来。

4.2 自适应蒙特卡洛定位配置

配置AMCL其实不难，但有几个参数特别关键。我习惯把这些参数分成三类：粒子参数、激光参数、运动模型参数。

你可能会问：这些alpha参数怎么调？我的经验是：先跑一次bag，然后看定位效果。如果粒子发散得太快，说明alpha值设大了；如果粒子跟不上实际运动，说明设小了。说白了，这就是个试错的过程。

4.3 里程计校准

里程计校准，说白了就是让机器人知道自己到底走了多远。我见过太多人忽略这一步，结果机器人走直线都走不直。

为什么会这样？因为轮子有直径误差、底盘有装配误差、地面摩擦不均匀……这些因素叠加起来，里程计误差会越来越大。

校准方法其实很简单：让机器人走一段已知距离，然后对比实际距离和里程计读数。

# 里程计校准示例
# 让机器人直线前进1米，记录odom数据

def calibrate_odometry():
    # 实际测量距离：1.0米
    actual_distance = 1.0
    
    # 里程计读数：假设是0.95米
    odom_distance = 0.95
    
    # 计算校准系数
    scale_factor = actual_distance / odom_distance
    print(f"校准系数: {scale_factor}")
    
    # 应用校准
    # 在ROS2中，可以通过修改odom发布者的参数实现

校准步骤我建议这样做：

1. 让机器人直线前进2米，记录实际距离和odom距离
2. 计算平移校准系数
3. 让机器人原地旋转360度，记录实际角度和odom角度
4. 计算旋转校准系数
5. 重复3次取平均值

4.4 融合传感器数据

单一传感器总有局限性。激光雷达在空旷环境会失效，里程计会累积误差，IMU有漂移……所以我们需要传感器融合。

在ROS2中，最常用的融合方式是扩展卡尔曼滤波（EKF）。我习惯用robot_localization包来做这件事。

你想想看，如果只用里程计，机器人走10米可能误差就有20厘米。但加上激光雷达的修正，误差能控制在5厘米以内。这就是融合的魅力。

# robot_localization的EKF配置示例
ekf_filter_node:
  ros__parameters:
    frequency: 30.0  # 更新频率30Hz
    
    # 里程计输入
    odom0: /odom
    odom0_config: [true, true, false,  # x, y, z位置
                   false, false, true,  # roll, pitch, yaw角度
                   false, false, false, # x, y, z速度
                   false, false, true]  # 角速度
    
    # IMU输入
    imu0: /imu/data
    imu0_config: [false, false, false,  # 位置
                  true, true, true,     # 角度
                  false, false, false,  # 速度
                  true, true, true]     # 角速度
    
    # 输出坐标系
    odom_frame: odom
    base_link_frame: base_link
    world_frame: odom

4.5 知识体系总览

下面这张图展示了本章的核心逻辑。你可以看到，里程计、IMU、激光雷达三个数据源，经过不同的处理路径，最终汇聚到AMCL中，输出稳定的定位结果。

嗯，这张图其实概括了本章的全部内容。从三个传感器输入，到各自的预处理，再到AMCL核心，最后输出稳定的定位。每一步都环环相扣，缺一不可。