代价地图与AMCL

这份笔记中主要记录在导航过程中遇见的一些名词与其作用，和一些简单的原理性内容。

代价地图

经过建图后，我们会得到一张格式为pgm的地图文件:

但是显然，这张地图中仅仅包含三种信息，即哪里是自由空间，哪里是障碍物，哪里是未知区域。如果我们使用这个地图进行路径规划的话，由于规划时规划算法并不知道机器人的实际尺寸和形状，最后规划出的仅仅只是一条可以到达目标点的路径，但是由于真正的机器人是有尺寸的，因此此时机器人在运动时，可能就会出现碰撞障碍物的情况。

代价地图（Costmap）就是为了解决这个问题而提出的，它通过在地图中为每个格子赋予一个代价值，代表当前格子对于机器人通过的“代价”，即通过该格子的难易程度或风险程度。其示意图可见下图：

在Nav2中，代价地图通常由以下几个层次组成：

• 静态层（Static Layer）：包含地图中固定的障碍物信息，如墙壁、家具等。
• 障碍层（Obstacle Layer）：包含实时感知的障碍物信息，如移动的人或物体。
• 膨胀层（Inflation Layer）：在障碍物周围生成一个膨胀区域，表示机器人应避免进入的区域。
• 自定义层（Custom Layer）：用户可以根据具体需求添加的自定义信息层，如传感器数据或特定任务相关的信息。

我们可以使用Rviz2的Map可视化，通过订阅不同的地图发布来查看各个代价地图层的信息。

• obstacle layer
  

由于obstacle layer包含的是实时感知的障碍物信息，因此它会随着机器人的传感器数据不断更新。其中图中灰色的部分表示尚未被感知的区域；黑色或者说透明的部分表示已被感知且为自由空间的区域；而粉色高亮部分代表该处区域为已被感知的障碍物。

• static layer
  

由于static layer就是直接订阅/map，所以其显示的内容与原始地图一致，不会包含实时感知的障碍物信息。

当然，上述展示的是全局代价地图(global costmap)，全局代价地图主要是用在全局路径规划阶段，即在机器人移动之前规划好到达目标点的路径。

而在机器人实际运动过程中，还会使用局部代价地图(local costmap)来进行局部路径规划和避障，通过局部代价地图，机器人可以实时感知周围环境的变化，并动态调整自己的运动轨迹，防止出现撞墙以及贴着障碍物运动的危险情况。

当我们查看局部代价地图时，只能看到机器人周围一定范围内的环境信息，而无法看到全局的地图信息。

当然，我们可以在nav2的配置文件中对代价地图的各个层进行配置，例如设置局部代价地图的大小，设置全局代价地图膨胀层的半径等。

base_footprint

base_footprint是机器人底盘的脚印，或者说是机器人底盘在地面上的投影，即机器人在地面上所占的区域。由于在2D导航任务中，机器人的三维形状被简化为二维平面上的一个区域，因此base_footprint在导航算法中起到了重要的作用，它帮助规划算法考虑机器人的实际尺寸，从而避免机器人在运动过程中与障碍物发生碰撞。

在nav2中，我们可以在其配置文件的局部代价地图的参数去设置base_footprint的形状与大小。

local_costmap:
  local_costmap:
    ros__parameters:
      .......
      footprint: "[ [0.35, 0.21], [0.35, -0.21], [-0.11, -0.21], [-0.11, 0.21] ]"
# 上面四个点是一个多边形的四点，顺序为顺时针或逆时针均可，只要保证形状连续闭合即可

当然，nav2提供了两种形式的base_footprint配置，分别是多边形形式和圆形形式，当机器人比较简单时，可以使用圆形形式来简化配置，只需要将上面的footprint参数替换为radius参数即可，例如：

local_costmap:
  local_costmap:
    ros__parameters:
      .......
      radius: 0.35
# footprint为一个半径为0.35的圆形

下面图片中，机器人外围的绿色方形框即为设置的base_footprint。

AMCL

AMCL，即Adaptive Monte Carlo Localization，自适应蒙特卡洛定位，是nav2采用的机器人定位算法。通过该算法，机器人可以通过里程计数据和雷达扫描数据来估计自身处于地图的哪个位置。

其基本的流程如下所示：

1.  初始化粒子集
    生成一组粒子，每个粒子代表机器人可能的位姿
    如果有初始位姿，就在初始位姿附近生成；如果做全局定位，粒子可能分布在更大范围。

2.  运动更新（Prediction）
    根据里程计 odom 的运动量，预测每个粒子的新位姿。
    同时加入一定随机噪声，用来表示里程计的不确定性。

3.  传感器更新（Measurement Update）
    使用激光雷达 LaserScan 和已有地图进行匹配。
    对每个粒子判断：
    如果机器人真的在这个粒子的位置，那么理论上的激光扫描和实际扫描有多相似？
    越相似，粒子的 权重越高。

4.  重采样（Resampling）
    根据粒子的权重重新生成粒子集：
    高权重粒子更容易被选中、复制
    低权重粒子逐渐消失
    新粒子通常会集中在高概率区域

5.  循环运行
    机器人继续运动时，不断重复：

可以用下面的动画来直观展示AMCL的工作流程：

在导航过程中，我们通常会先通过2d Pose Estimate来手动指定一下机器人的初始位姿(initial pose),这也作为AMCL的最开始运行的位置，这样可以大大减少AMCL收敛所需的时间。当然，你也可以手动向/initialpose这个话题去发布初始位姿信息。

同时，我们在机器人周围看到的绿色的小箭头，表示的是AMCL算法中粒子的分布情况，每个小箭头代表一个粒子，箭头的方向表示粒子的朝向。可以看到收敛到后面，粒子的分布逐渐集中在机器人真实位姿附近。