260522

继我们上次的进度继续。

接下来我们可以给 MPPI controller 加上速度边界。

这个 MPPI controller 是 Nav2 里的一个路径跟随控制器，全称是：Model Predictive Path Integral Controller

即——模型预测路径积分控制器。

简单的来说就是：这个控制器通过内部的物理模型和小车当前的物理数据，如线速度、角速度等，来预测小车接下来在不同的候选速度指令下可能走出来的轨迹，然后它会给这些轨迹进行打分，并选择代价最低的一条，然后把对应的速度指令给发送出去。

我们先给 FollowPath 具体指定为 MPPI controller:

controller_server:
  ros__parameters:
    controller_frequency: 20.0
    controller_plugins: ["FollowPath"]
    FollowPath:
      plugin: "nav2_mppi_controller::MPPIController"
      motion_model: "DiffDrive"

这我们选择的 motion_model 为 DiffDrive，命中我们小车为差速的小车，避免 controller 生成横向平移。

然后是收敛速度：

controller_server:
  ros__parameters:
    controller_frequency: 20.0
    controller_plugins: ["FollowPath"]
    FollowPath:
      plugin: "nav2_mppi_controller::MPPIController"
      motion_model: "DiffDrive"
      vx_max: 0.45
      vx_min: -0.20
      wz_max: 1.2

这里的 vx 代表的是前后的线速度，而 wz 代表的是绕 Z 轴旋转时候的角速度。

这里面理应有一个新的参数叫 vy，但是我们这里的差速小车因为并不能直接左右拐弯，因此在平滑速度的时候 vy 会自动被限制为 0，我们这里就不需要多加这个参数了。

加入加速度边界：

controller_server:
  ros__parameters:
    controller_frequency: 20.0
    controller_plugins: ["FollowPath"]
    FollowPath:
      plugin: "nav2_mppi_controller::MPPIController"
      motion_model: "DiffDrive"
      vx_max: 0.45
      vx_min: -0.20
      wz_max: 1.2
      ax_max: 0.8
      ax_min: -0.8
      az_max: 1.5

可以这里的 ax 对应的是 X 轴上线速度的加速度，而 az 则是指绕 Z 轴旋转的加速度。

即，在这里：

• ax -> vx
• az -> wz

虽然在命名上可能有点困惑，但是考虑到我们目前小车还是在一个 2D 的平面上运行，因此这样的命名目前来说还是可以接受的。

我们的 MPPI controller 以及对应的速度限制完成了后，接下来就是建立本地的代价地图 local costmap了。

local_costmap:
  local_costmap:
    ros__parameters:
      global_frame: odom
      robot_base_frame: base_link
      rolling_window: true

可以看到和我们之前的代码不一样的一点是，我们的 local_costmap则是在外面多了一层 local_costmap 的嵌套。

这是因为 Nav2 的代价地图不是普通的单层节点名，而是一个节点下还带有一个同名的子节点结构，所以参数才会这样写成两层。

我们可以这么理解：

• 第一层 local_costmap： ROS2 的节点名称。
• 第二层 local_costmap：这个节点内部的代价地图名称/命名空间。

我们后面的全局代价地图同理，到时候也会这么写。

这里我们给代价地图定义的参考坐标系为 odom，机器人的本体坐标系还是 base_link，因为我们本地代价地图只需要确保局部范围内的代价就可以了，而 odom 在局部是连续的，就很适合作为我们的参考坐标系。

而滚动窗口我们设置为true，方便我们的机器人的局部地图随着距离的移动而进行滚动，适合只关心局部的情况。

如果设置为false，则就是窗口固定在某个区域内了，这只适合静态全局地图或固定区域的地图。

然后我们设置窗口的大小和分辨率：

local_costmap:
  local_costmap:
    ros__parameters:
      global_frame: odom
      robot_base_frame: base_link
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      robot_radius: 0.34

这里我们将窗口设置为一个以小车本体为中心，长宽为 3m 的正方形范围。

分辨率为 5cm，即 5cm/格。

相当于一个窗口内，我们有 60 x 60 个格子。

然后是 robot_radius，这表示将我们的小车按照一个 0.34m 为半径的圆形进行处理来带入到代价地图中。

这将会影响在 MPPI 中小车可以通过哪些地方，而哪些地方不能通过。

虽然有 footprint 这个更为精确的描述，但是在我们当前的场景下，使用 robot_radius 就足够用了。

最后加入激光障碍物层：

local_costmap:
  local_costmap:
    ros__parameters:
      global_frame: odom
      robot_base_frame: base_link
      rolling_window: true
      width: 3
      height: 3
      resolution: 0.05
      robot_radius: 0.34
      plugins: ["voxel_layer", "inflation_layer"]
      voxel_layer:
        plugin: "nav2_costmap_2d::VoxelLayer"
        observation_sources: scan
        scan:
          topic: /scan
          date_type: "LaserScan"
          clearing: True
          marking: True
      inflation_layer:
        plugin: "nav2_costmap_2d::InflationLayer"
        inflation_radius: 0.70

这里大部分参数都不难理解，我们解释一下其中几个：

• clearing: 把传感器确认为空的区域从本地代价地图中进行移除。
• marking: 将传感器看到的障碍物添加到本地代价地图中。

这两个参数就是实时更新本地代价地图的关键参数。

然后是是 inflation_radius，这代表障碍物的膨胀半径。

通常来说，你并不预期小车贴着障碍物走，也不期望小车距离障碍物太远导致较窄的通道被认为无法通行。

因此我们就可以引入膨胀半径这个参数。

通常来说，原始障碍物可能是这样的：

. . . . .
. . X . .
. . . . .

我们在膨胀后就会变成这样的：

. + + + .
. + X + .
. + + + .

其中，X代表实际的障碍物，不可通行；+ 代表高代价区域。

我们的规划器在进行规划的时候，选择路径时会主动规避高代价区域，所以路径才会选择和障碍物保持一定距离，同时也不至于完全不会进入某些较窄的通道。

我们这里设定这个膨胀半径为 0.70，即 0.7m，在 0.7m 内行动时有额外的代价，同时距离越近，代价越高。

接着，我们建立适合探索前置阶段的全局代价地图：

global_costmap:
  global_costmap:
    ros__parameters:
      global_frame: map
      robot_base_frame: base_link
      robot_radius: 0.34
      rolling_window: true
      width: 20
      height: 20
      track_unknown_space: false

这里我们设置的参考系为 map，同时，由于 SLAM 在初始状态下的地图范围实际很小，我们需要实时建模，因此我们也需要使用滚动窗口而非固定窗口大小，不过我们可以将这个窗口的长宽设置为 20m。