nav2_amcl节点概况

AMCL是一个针对二维移动机器人的概率定位系统。它实现了自适应(或KLD采样)蒙特卡罗定位方法(如Dieter Fox所述)，该方法使用粒子过滤器来跟踪机器人在已知地图上的姿势。

算法

从Probabilistic Robotics书中的算法

• sample_motion_model_odometry
• beam_range_finder_model
• likelihood_field_range_finder_model
• Augmented_MCL, and KLD_Sampling_MCL

amcl接收基于激光的地图，激光扫描数据和tf转换消息，并输出姿态估计pose estimates。启动时，AMCL根据提供的参数初始化其粒子过滤器。请注意，由于默认设置，如果未设置任何参数，则初始过滤状态将是以(0，0，0)为中心的中等大小的粒子云

订阅的主题

• scan (sensor_msgs/LaserScan)
  Laser scans.转换到odom_frame_id
• (tf/tfMessage)
  Transforms.
• initialpose (geometry_msgs/PoseWithCovarianceStamped)
  Mean and covariance with which to (re-)initialize the particle filter.用于（重新）初始化粒子滤波器的平均值和协方差
• map (nav_msgs/OccupancyGrid)
  When the use_map_topic parameter is set, AMCL subscribes to this topic to retrieve the map used for laser-based localization. 当设置了use_map_topic参数时，AMCL订阅此主题以检索用于基于激光的本地化的映射

发布的主题

• amcl_pose (geometry_msgs/PoseWithCovarianceStamped)
  Robot’s estimated pose in the map, with covariance.机器人在地图上的估计姿态，具有协方差后验位姿和一个6*6的协方差矩阵（xyz+三个转角）
• particlecloud (geometry_msgs/PoseArray)
  The set of pose estimates being maintained by the filter.由过滤器维护的姿态估计集合。粒子位姿的数组
• tf (tf/tfMessage)
  Publishes the transform from odom (which can be remapped via the ~odom_frame_id parameter) to map. 发布从odom到map的transform。

服务

• global_localization（std_srvs / Empty）
  启动全局定位，其中所有粒子随机分散通过地图中的自由空间。没有给定初始位姿的情况下在全局范围内初始化粒子位姿。callback调用pf_init_model，然后调用uniformPseGenerator，在地图上随机生成pf -> max_samples个粒子。
• request_nomotion_update:没有运动模型更新的情况下也更新粒子群。
• dynamix_configure:动态参数配置器

粒子滤波

通过寻找一组在状态空间中传播的随机样本来近似的表示概率密度函数，用样本均值代替积分运算，进而获得系统状态的最小方差估计的过程，这些样本被形象的称为“粒子”，故而叫粒子滤波。

• 初始化粒子群
• 按照上一时刻的$x_{t-1}$及控制数据$u_t$（可能是编码器数据，可能是odom）,产生t时刻的假象状态$x_t$（从$p(x_t|u_t,x_{t-1})$中采样）
• 计算每个$x_t$的重要性因子。$w_t=p(z_t|x_t)$
• 粒子$x_t$和权重$w_t$构成t时刻的新的粒子集
• 重采样/重要性采样:
  • 从包含M个粒子的粒子集中，按照每个粒子的权重，作为概率，抽取M次。这样粒子集的分布发生变化。这时粒子集中有许多重复粒子。

---

具体参考ros官方navigation功能介绍