两轮差速底盘SLAM系列(10)--导航配置

本文最后更新于：2022年5月29日 上午

前言

整个机器人SLAM实现基本都是参考ROS wiki——Setup and Configuration of the Navigation Stack on a Robot来进行开发:

Contents

  1. Robot Setup
    1. ROS
    2. Transform Configuration (other transforms)
    3. Sensor Information (sensor sources)
    4. Odometry Information (odometry source)
    5. Base Controller (base controller)
    6. Mapping (map_server)
  2. Navigation Stack Setup
    1. Creating a Package
    2. Creating a Robot Configuration Launch File
    3. Costmap Configuration (local_costmap) & (global_costmap)
    4. Base Local Planner Configuration
    5. Creating a Launch File for the Navigation Stack
    6. AMCL Configuration (amcl)
  3. Running the Navigation Stack
  4. Troubleshooting

从上面的目录可以看到第一部分在前面都已经完成了，现在继续来完成第二部分——导航功能。

ROS中的导航框架

机器人的自主导航功能基本全靠Navigation中的pacakge：

V3PiM6.png

上图中位于导航功能正中心的蓝色方框是move_base节点，可以理解为一个强大的路径规划器，在实际的导航任务中，你只需要启动这一个node，并且给它提供数据，就可以规划出路径和速度。 move_base之所以能做到路径规划，是因为它包含了很多的插件，像图中的圆圈global_planner、local_planner、global_costmap、local_costmap、recovery_behaviors。这些插件用于负责一些更细微的任务：全局路径规划、局部路径规划、全局代价地图、局部代价地图、恢复行为。而每一个插件其实也都是一个package，放在Navigation Stack里。

base_global_planner插件

  • parrot_planner: 实现了较简单的全局规划算法
  • navfn: 实现了Dijkstra和A*全局规划算法
  • global_planner: 重新实现了Dijkstra和A*全局规划算法,可以看作navfn的改进版

注意：这里还有一个点直接注意的是，在navigation包里面move_base的实现里面，是添加了动态规划的，也就是在小车的移动过程中，globalplanner也是有个线程一直在计算当前位置到终点的路径。在这里是我觉得奇怪的地方，本来就有D*这样的算法可以实现动态规划，可是navigation并没有采用这种方法，而且启动了一个线程，在移动过程中对路径不断的进行重新规划。不过这种方法有个优点，在不妨碍原有实现上(也就是dijkstra和A* 寻路上) 添加了动态规划。

base_local_planner插件

  • base_local_planner: 实现了Trajectory Rollout和DWA两种局部规划算法
  • dwa_local_planner: 实现了DWA局部规划算法，可以看作是base_local_planner的改进版本

注意：对于全向机器人来说，也就是存在x方向的速度，y方向的速度，和角速度。DWA确实效率高一点。但是如果是非全向机器人，比如说只存在角速度和线速度。trajectorylocalplanner会更适用一点。

recovery_behavior插件

  • clear_costmap_recovery: 实现了清除代价地图的恢复行为
  • rotate_recovery: 实现了旋转的恢复行为
  • move_slow_and_clear: 实现了缓慢移动的恢复行为

除了以上三个需要指定的插件外，还有一个costmap插件，该插件默认已经选择好，默认即为costmap_2d，不可更改，但costmap_2d提供了不同的Layer可以供我们设置。

上面这些插件只是官方实现的，我们也可以来实现自己的规划算法，以插件的形式包含进move_base，这样就可以来改进这些路径规划算法了。我们可以根据move_base的launch文件来看看也就明白配置哪个文件了。

新建move_base启动文件

参考它的官方说明，我们先直接新建一个move_base.launch启动文件：

1
2
roscd agv_car/launch
gedit move_base.launch

文件代码如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
<launch>

  <!-- move_base -->
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <rosparam file="$(find agv_car)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find agv_car)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find agv_car)/param/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find agv_car)/param/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find agv_car)/param/dwa_local_planner_params.yaml" command="load" />
    <rosparam file="$(find agv_car)/param/global_planner_params.yaml" command="load" />
    <rosparam file="$(find agv_car)/param/move_base_params.yaml" command="load" />
  </node>

</launch>

从上面可以看到，在启动move_base节点时，前四个参数是配置代价地图相关参数，首先加载了costmap_common_params.yaml到global_costmap和local_costmap两个命名空间中，因为该配置文件是一个通用的代价地图配置参数，即local_costmap和global_costmap都需要配置的参数。然后下面的local_costmap_params.yaml是专门为了局部代价地图配置的参数，global_costmap_params.yaml是专门为全局代价地图配置的参数。而后面三个是配置路径规划相关参数。

代价地图的配置

通用配置文件

在agv_car功能包根目录下新建一个param文件夹，专门用来存放配置参数文件。在该文件夹下创建costmap_common_params.yaml文件，配置的参数如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
footprint: [[-0.26,-0.23],[-0.26,0.23],[0.26,0.23],[0.26,-0.23]]
map_type: costmap

obstacle_layer:
  enabled: true
  combination_method: 1        # default 1
  track_unknown_space: true    # default false
  obstacle_range: 2.5          # default 2.5   检测障碍物的最大范围
  raytrace_range: 3.0          # default 3.0   检测自由空间的最大范围
  max_obstacle_height: 0.6     # default 2.0   描述障碍物的最大高度
  min_obstacle_height: 0.0     # 描述障碍物的最小高度
  observation_sources: scan    # 代价地图需要关注的传感器信息
  scan: {
    sensor_frame: laser_frame, # 激光雷达传感器的坐标系名称;
    topic: scan_filtered,      # 该激光雷达发布的话题名;
    data_type: LaserScan,      # 激光雷达数据类型;
    marking: true,             # 是否使用该传感器来标记障碍物;
    clearing: true,            # 是否使用该传感器来清除障碍物标记为自由空间;
  }

inflation_layer:
  enabled: true
  cost_scaling_factor: 5.0     # default 10.0
  inflation_radius: 0.2        # default 0.55   设置障碍物的膨胀半径 

static_layer:
  enabled: true

下面来依次解释下各参数的意义：

  • footprint：每一个坐标代表机器人上的一点，设置机器人的中心为[0,0]，根据机器人不同的形状，找到机器人各凸出的坐标点即可，具体可参考下图来设置(如果是圆形底盘机器人，直接设置半径大小即可：例如 robot_radius: 0.5)：

    V3CLrT.png

  • map_type: 地图类型，这里为costmap(代价地图)。另一种地图类型为为voxel(体素地图)。这两者之间的区别是前者是世界的2D表示，后者为世界的3D表示。

  • obstacle_layer：配置障碍物图层

    • enabled: 是否启用该层

    • combination_method(default: 1): 只能设置为0或1，用来更新地图上的代价值，一般设置为1;

    • track_unknown_space (default: false): 如果设置为false，那么地图上代价值就只分为致命碰撞和自由区域两种，如果设置为true，那么就分为致命碰撞，自由区域和未知区域三种。意思是说假如该参数设置为false的话，就意味着地图上的未知区域也会被认为是可以自由移动的区域，这样在进行全局路径规划时，可以把一些未探索的未知区域也来参与到路径规划，如果你需要这样的话就将该参数设置为false。不过一般情况未探索的区域不应该当作可以自由移动的区域，因此一般将该参数设置为true;

    • obstacle_range(default: 2.5): 设置机器人检测障碍物的最大范围，意思是说超过该范围的障碍物，并不进行检测，只有靠近到该范围内才把该障碍物当作影响路径规划和移动的障碍物;

    • raytrace_range(default: 3.0): 在机器人移动过程中，实时清除代价地图上的障碍物的最大范围，更新可自由移动的空间数据。假如设置该值为3米，那么就意味着在3米内的障碍物，本来开始时是有的，但是本次检测却没有了，那么就需要在代价地图上来更新，将旧障碍物的空间标记为可以自由移动的空间

    • observation_sources: 设置导航中所使用的传感器，这里可以用逗号形式来区分开很多个传感器，例如激光雷达，碰撞传感器，超声波传感器等，我这里只设置了激光雷达;

      • data_type: 激光雷达数据类型;

      • topic: 该激光雷达发布的话题名;

      • marking: 是否可以使用该传感器来标记障碍物;

      • clearing: 是否可以使用该传感器来清除障碍物标记为自由空间;

      • max_obstacle_height(default: 2.0): 以米为单位插入costmap的任何障碍物的最大高度。此参数应设置为略高于机器人的高度。

      • min_obstacle_height: 传感器读数的最小高度（以米为单位）视为有效。通常设置为地面高度。

  • inflation_layer: 膨胀层，用于在障碍物外标记一层危险区域，在路径规划时需要避开该危险区域

    • enabled: 是否启用该层;

    • cost_scaling_factor(default: 10.0): 膨胀过程中应用到代价值的比例因子，代价地图中到实际障碍物距离在内切圆半径到膨胀半径之间的所有cell可以使用如下公式来计算膨胀代价：

      exp(-1.0 * cost_scaling_factor * (distance_from_obstacle – inscribed_radius)) * (costmap_2d::INSCRIBED_INFLATED_OBSTACLE – 1)

      公式中costmap_2d::INSCRIBED_INFLATED_OBSTACLE目前指定为254，

      注意： 由于在公式中cost_scaling_factor被乘了一个负数，所以增大比例因子反而会降低代价。

    • inflation_radius(default: 0.55): 膨胀半径，膨胀层会把障碍物代价膨胀直到该半径为止，一般将该值设置为机器人底盘的直径大小。

  • Static_layer: 静态地图层，即SLAM中构建的地图层

    • enabled: 是否启用该地图层;

更多详细参数说明可以参考ROS官网说明文档：

[1] costmap_2d——obstacle

[2] costmap_2d——Inflation

全局规划配置文件

该文件是为全局代价地图配置的参数，我们需要在param目录中，创建global_costmap_params.yaml文件，具体配置的参数如下：

1
2
3
4
5
6
7
8
9
10
11
12
global_costmap:
  global_frame: /map
  robot_base_frame: /base_footprint
  update_frequency: 3.0
  publish_frequency: 0.5
  static_map: true
  rolling_window: false
  transform_tolerance: 0.5
  plugins:
  - {name: static_layer,    type: "costmap_2d::StaticLayer"}
  - {name: obstacle_layer,  type: "costmap_2d::ObstacleLayer"}
  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

下面是该全局代价地图配置文件中各参数的意义：

  • global_frame：全局代价地图需要在哪个坐标系下运行；
  • robot_base_frame：在全局代价地图中机器人本体的基坐标系，就是机器人上的根坐标系。通过global_frame和robot_base_frame就可以计算两个坐标系之间的变换，得知机器人在全局坐标系中的坐标了；
  • update_frequency：全局代价地图更新频率(单位:Hz），一般全局代价地图更新频率设置的比较小；
  • publish_frequency：全局代价地图发布的频率(单位:Hz）。
  • static_map：配置是否使用map_server提供的地图来初始化;如果不需要使用已有的地图或者map_server，最好设置为false；
  • rolling_window：是否在机器人移动过程中需要滚动窗口，始终保持机器人在当前窗口中心位置；
  • transform_tolerance：坐标系间的转换可以忍受的最大延时。
  • plugins：在global_costmap中使用下面三个插件来融合三个不同图层，分别是static_layer、obstacle_layer和inflation_layer，合成一个master_layer来进行全局路径规划。

本地规划配置文件

局部代价地图配置参数所建立的地图主要是为局部路径规划所使用，我们可以在parma目录下，创建local_costmap_params.yaml文件，完整内容如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
local_costmap:
  global_frame: /odom
  robot_base_frame: /base_footprint
  update_frequency: 3.0
  publish_frequency: 2.0
  static_map: false
  rolling_window: true
  width: 2.5
  height: 2.5
  resolution: 0.05
  transform_tolerance: 0.5
  plugins:
  - {name: obstacle_layer,  type: "costmap_2d::ObstacleLayer"}
  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}

下面是详细解释每个参数的意义：

  • global_frame：在局部代价地图中的全局坐标系，一般需要设置为odom_frame;
  • robot_base_frame：机器人本体的基坐标系;
  • update_frequency：局部代价地图的更新频率;
  • publish_frequency：局部代价地图的发布频率;
  • static_map：局部代价地图一般不设置为静态地图，因为需要检测是否在机器人附近有新增的动态障碍物;
  • rolling_window：使用滚动窗口，始终保持机器人在当前局部地图的中心位置;
  • width：滚动窗口的宽度，单位是米;
  • height：滚动窗口的高度，单位是米;
  • resolution：地图的分辨率，该分辨率可以从加载的地图相对应的配置文件中获取到;
  • transform_tolerance：局部代价地图中的坐标系之间转换的最大可忍受延时;
  • plugins：在局部代价地图中，不需要静态地图层，因为我们使用滚动窗口来不断的扫描障碍物，所以就需要融合两层地图（inflation_layer和obstacle_layer）即可，融合后的地图用于进行局部路径规划。

全局规划器配置

我们在进行全局路径规划时，需要由外部来告知目标点。同时还需要知道全局代价地图，因为在路径规划时需要避开代价高的危险区域，不然规划的路径就撞到墙上了。
在param目录下，创建global_planner_params.yaml文件，代码如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
GlobalPlanner:
  allow_unknown: false
  default_tolerance: 0.2
  visualize_potential: false
  use_dijkstra: false
  use_quadratic: true
  use_grid_path: false
  old_navfn_behavior: false
 
  lethal_cost: 253
  neutral_cost: 66
  cost_factor: 0.55
  
  publish_potential: true
  orientation_mode: 0
  orientation_window_size: 1

下面来依次解释下各参数意义：

  • allow_unknown(default: true): 是否允许规划器规划穿过未知区域的路径,只设计该参数为true还不行,还要在costmap_commons_params.yaml中设置track_unknown_space参数也为false才行。
  • default_tolerance(default: 0.0): 当设置的目的地被障碍物占据时,需要以该参数为半径寻找到最近的点作为新目的地点.
  • visualize_potential(default: false): 是否显示从PointCloud2计算得到的势区域.
  • use_dijkstra(default: true): 如果设置为true,将使用dijkstra算法,否则使用A*算法.
  • use_quadratic(default: true): 如果设置为true,将使用二次函数近似函数,否则使用更加简单的计算方式,这样节省硬件计算资源.
  • use_grid_path(default: false): 如果如果设置为true,则会规划一条沿着网格边界的路径,偏向于直线穿越网格,否则将使用梯度下降算法,路径更为光滑点.
  • old_navfn_behavior(default: false): 若在某些情况下,想让global_planner完全复制navfn的功能,那就设置为true,但是需要注意navfn是非常旧的ROS系统中使用的,现在已经都用global_planner代替navfn了,所以不建议设置为true.
  • lethal_cost(default: 253): 致命代价值
  • neutral_cost(default: 50): 中等代价值
  • cost_factor(default: 3.0): 代价地图与每个代价值相乘的因子
  • publish_potential(default: true): 是否发布costmap的势函数
  • orientation_mode(default: 0): 如何设置每个点的方向（None = 0,Forward = 1,Interpolate = 2,ForwardThenInterpolate = 3,Backward = 4,Leftward = 5,Rightward = 6）
  • orientation_window_size(default: 1): 根据orientation_mode指定的位置积分来得到使用窗口的方向。

参考链接：

[1] ROS wiki——global_planner

[2] ROS小课堂——6.在STDR中配置planner路径规划器参数

关于代价值的三个参数决定了全局路径规划的性能，下面给出详细解释，我是搬运了ZONG_XP大神的原文——[ROS Navigation Tuning Guide]翻译过来，如侵删。

全局路径规划参数

因为global规划器是一种更常使用的方法，我们来看它的一些关键参数。注意：不是所有的参数都能在ROS网页上找到，但是你可以通过运行rosrun rqt_reconfigure rqt_reconfigure来查看。

我们可以设置这些默认值：allow unknown(true), use dijkstra(true), use quadratic(true), use grid path(false), old navfn behavior(false) 。如果我们想在RVIZ中查看势力图可以将visualize_potential值由FALSE设为TRUE。

3k2lNQ.png

除了这些参数外， 还有三个没有列出来的参数会决定全局路径规划的性能。分别是cost_factor, neutral_cost, lethal_cost。事实上，这些参数在navfn算法中也提到了。这个开源代码详细的解释了navfn如何计算这些代价值。

3k21hj.png

navfn代价值的计算方法如下：cost = COST_NEUTRAL + COST_FACTOR * costmap_cost_value

传入的代价值设置在0到252的范围，进一步分析：

COST_NEUTRAL为50时，COST_FACTOR需要约为0.8，从而确保输入值的分布能使输出值在50到253间变化。如果COST_FACTOR较高，cost值将会在障碍物附近有一个峰值，然后规划器将视狭窄走廊的整个宽度为较差，将不会沿着中心规划路径。

实验观察：实验也证实了这些解释。将cost_factor设置过高或过低都会降低路径质量。这些路径没有穿过每侧障碍物的中间，也没有相对光滑的曲率。极端的COST_NEUTRAL值也有类似的效果。对于关键的cost值，如果设置的过低可能会无法产生任何路径，即使可行路径是明显的。上图显示了COST_FACTOR和COST_NEUTRAL对全局路径规划的影响。绿色的线是全局规划器产生的全局路径。

经过几次实验，我们发现当COST_FACTOR=0.55，COST_NEUTRAL=66，lethal_cost=253时，全局路径是很好的。

局部规划器配置

局部规划器是直接控制机器人的移动底盘运动的插件，负责来向移动底盘的/cmd_vel话题中发布控制命令。机器人移动的效果好不好，这个局部路径规划可是影响最大的。在param目录下，创建dwa_local_planner_params.yaml文件，代码如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
DWAPlannerROS:
  # Robot Configuration Parameters
  acc_lim_x: 1.25                 # default 2.5  机器人在x方向的加速度极限
  acc_lim_y: 0.0                  # default 2.5  机器人在y方向的加速度极限
  acc_lim_th: 5                   # default 3.2  机器人的角加速度极限
  
  max_trans_vel: 0.5              # default 0.55 机器人最大平移速度的绝对值,单位m/s
  min_trans_vel: 0.01             # default 0.1  机器人最小平移速度的绝对值,单位m/s
  
  max_vel_x: 0.5                  # default 0.55 机器人在x方向的最大速度,单位m/s
  min_vel_x: -0.25                # default 0.0  机器人在x方向的最小速度，若设置为负数，则机器人向后移动
  max_vel_y: 0.0                  # default 0.1  机器人在y方向的最大速度
  min_vel_y: 0.0                  # default -0.1 机器人在y方向的最小速度
  
  max_rot_vel: 0.5                # default 1.0  机器人的最大角速度的绝对值
  min_rot_vel: -0.5               # default 0.4  机器人的最小角速度的绝对值

  # Goal Tolerance Parameters
  yaw_goal_tolerance: 0.1         # default 0.05  达到目标时，机器人在偏航/旋转中的弧度容忍误差。
  xy_goal_tolerance: 0.10         # default 0.10  达到目标时，机器人在x和y距离内的容忍误差。
  latch_xy_goal_tolerance: false  # default false 如果设置为true,机器人到达容错距离内,就会原地旋转,即使转动是会跑出容错距离外.

  # Forward Simulation Parameters
  sim_time: 3.0                   # default 1.7    前向模拟轨迹的时间，单位:s
  sim_granularity: 0.1            # default 0.025  给定轨迹上的点之间的间隔尺寸，单位:m
  vx_samples: 20                  # default 3      x方向速度的样本数
  vy_samples: 1                   # default 10     y方向速度的样本数
  vth_samples: 40                 # default 20     角速度的样本数

  # Trajectory Scoring Parameters
  path_distance_bias: 34.0        # default 32.0   定义控制器与给定路径接近程度
  goal_distance_bias: 24.0        # default 24.0   定义控制器与局部目标点的接近程度，并控制速度
  occdist_scale: 0.05             # default 0.01   定义控制器躲避障碍物的程度
  forward_point_distance: 0.325   # default 0.325
  stop_time_buffer: 0.5           # default 0.2    为防止碰撞，机器人必须提前停止的时间长度(单位:s)。
  scaling_speed: 0.25             # default 0.25   启动机器人底座的速度
  max_scaling_factor: 0.2         # default 0.2    最大缩放参数
  publish_cost_grid: false        # default false	是否发布规划器在规划路径时的代价网格.如果设置为true,那么就会在~/cost_cloud话题上发布sensor_msgs/PointCloud2类型消息.

  # Oscillation Prevention Parameters
  oscillation_reset_dist: 0.05    # default 0.05   机器人运动多远距离才会重置振荡标记

  # Global Plan Parameters
  prune_plan: false               # default true   机器人前进时是否清除身后1m外的轨迹

该配置文件声明机器人本地规划采用DWA算法，并设置算法中需要用到的机器人速度、加速度阈值等参数。

DWA算法的基本思想

  1. 在机器人控制空间离散采样(dx, dy, dtheta)；
  2. 对每一个采样的速度进行前向模拟，看看在当前状态下，使用该采样速度移动一小段时间后会发生什么；
  3. 评价前向模拟得到的每个轨迹，是否接近障碍物，是否接近目标，是否接近全局路径以及速度等等，舍弃非法路径；
  4. 选择得分最高的路径，发送对应的速度给底座。

DWA与Trajectory Rollout的区别主要是在机器人的控制空间采样差异.Trajectory Rollout采样点来源于整个前向模拟阶段所有可用速度集合,而DWA采样点仅仅来源于一个模拟步骤中的可用速度集合.这意味着相比之下DWA是一种更加有效算法,因为其使用了更小采样空间;然而对于低加速度的机器人来说可能Trajectory Rollout更好, 因为DWA不能对常加速度做前向模拟。

具体的参数说明可以查看：

[1] ROS wiki——dwa_local_planner

[2] 中国大学MOOC———《机器人操作系统入门》讲义

move_base参数配置

完成上面的配置后，我们还需要对move_base进行参数配置，如果不进行配置它会有默认值，我们需要修改一些参数以达到我们机器人的要求。在param目录下，创建move_base_params.yaml文件，代码如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
base_global_planner: "global_planner/GlobalPlanner"   # 指定用于move_base的全局规划器插件名称
base_local_planner: "dwa_local_planner/DWAPlannerROS" # 指定用于move_base的局部规划器名称

shutdown_costmaps: false        # default false  当move_base在不活动状态时,是否关掉costmap

controller_frequency: 5.0       # default 20.0   向底盘控制移动话题cmd_vel发送命令的频率
controller_patience: 3.0        # default 15.0   在空间清理操作执行前,控制器花多长时间等有效控制指令下发

planner_frequency: 0.5          # default 0.0    全局规划操作的执行频率(Hz)。如果频率设置为0.0，则仅当收到新目标或局部规划器报告其路径堵塞时，才会重新执行规划操作。
planner_patience: 5.0           # default 5.0    在空间清理操作执行前,留给规划器多长时间(s)来找出一条有效规划

conservative_reset_dist: 0.1    # default 3.0    尝试清除地图中的空间时，距离机器人几米远的障碍将会从代价地图清除。注意，仅当默认恢复行为用于move_base时，才使用此参数。

oscillation_timeout: 10.0       # default 0.0    执行恢复行为之前允许振荡的秒数
oscillation_distance: 0.2       # default 0.5    机器人必须移动多米才能被视为没有振动

recovery_behavior_enabled: true # default true   是否启用move_base恢复行为以尝试清除空间。
clearing_rotation_allowed: true # default true   确定在尝试清理空间时机器人是否将尝试就地旋转。 注意：仅当使用默认恢复行为时才使用此参数，这意味着用户尚未将recovery_behaviors参数设置为任何自定义设置。
max_planning_retries: -1        # default -1     执行恢复行为之前允许多少次计划重试。值-1.0对应于无限次重试。

base_global_planner和base_local_planner的插件详细内容可以查看ROS的官方介绍。

后面的参数上面都有注释，更详细的可以查看ROS wiki 关于move_base的介绍。

amcl配置

简单介绍

amcl是一种机器人在2D中移动的概率定位系统。 它实现了自适应（或KLD采样）蒙特卡罗定位方法（如Dieter Fox所述），该方法使用粒子滤波器来针对已知地图跟踪机器人的位姿。

  • 蒙特卡罗定位算法
  • 二维环境定位
  • 针对已有地图使用粒子滤波器跟踪一个机器人的姿态

V3Cva4.png

AMCL是如何实现对机器人进行定位的呢？

本段转载自ROS小课堂——4.在STDR中使用amcl进行仿真机器人定位

AMCL使用的是粒子滤波的方式来实现对机器人的定位，下面我使用一个简单形象的例子来介绍这个定位过程是如何实现的。你可以想象这样一个场景，你站在一个商业广场上，这个广场就相当于一张全局地图。我在广场上随机的分布了100个妹子，这些妹子就是粒子用来对你进行跟踪定位的，当然现在这些妹子都不知道要跟踪定位的目标就是你啊。你也不认识妹子们，你现在漫无目的的往前移动5步，你突然发现你面前有一个火锅店，你就饿了想吃火锅，就顺便在广场上吼了一嗓子，“我往前走了5步就发现前面有个火锅店啊，我要去吃火锅“。妹子们根据你的信息也来同样的移动，然后就开始观察自己的前面有没有火锅店啊，那肯定是前面有火锅店的妹子附近才有要跟踪的目标啊。此时，由于开始的时候妹子们是随机分布在广场上，面朝的方向肯定也是各部相同，有的肯定就不能发现自己前面有火锅店，那这个妹子就肯定很伤心，自己就退出这个任务了。那些可以发现了火锅店的妹子就很开心，距离目标很近啊，所以她就开始再生出一个妹子跟自己一起来完成这个任务，因为自己找到目标的概率很大啊，可以想象现在这群妹子们肯定有很多就在要跟踪的目标附近了。

那接下来你吃了火锅出来后，继续移动，你向前走了10步，做转弯后发现一个兰州拉面，你就又吼了一嗓子，”我往前走了10步，左拐个弯就发现个兰州拉面啊“。那妹子们又开始根据这个信息来也做同样的移动，此时有的妹子移动后就发现前面没有兰州拉面啊，很伤心就退出跟踪定位任务。那些移动后发现自己前面有兰州拉面的就更开心了啊，距离目标又近了一点啊，因为两次概率都在目标的附近，那她就再生出一个妹子跟着自己一起来定位目标。那可以想象一下，现在肯定广场上有更多的妹子都聚集在一起，她们都在目标的附近。

那你吃完面后出来继续往前走20步，突然发现前面有个珠宝店，你就又吼一嗓子，”我往前走了20步，发现一个珠宝店啊“。妹子们又根据这个信息开始移动，并查看自己的前面有没有珠宝店，那些没有看到珠宝店的就给淘汰了，那发现又珠宝店的妹子就再生出一个妹子，可以想象现在基本上妹子差不多都聚集在目标的附近了，那经过多次这样的循环，即使妹子们不知道要跟踪的目标在哪里，但是可以确定自己现在所处的位置基本上跟目标位置也差不到哪里去了。所以以后我要想定位你的位置那就容易多了，即使我不知道你在哪里，我只要知道我的那群妹子在哪里就行，你基本上也就在那附近了，说实话我感觉这个过程都有点像遗传算法的过程了。

在这里我只是简单介绍了一下例子滤波定位的原理，当然以上这个例子仅供玩乐，不必拘泥是否科学严谨，重在理解这个定位的过程，在ROS下的AMCL只实现了在laser扫描的创建的地图上，使用laser来进行定位，其实该定位方法还可以修改使用其他类型的传感器来定位，例如可以使用超声波或红外这些测距的传感器也可以实现定位。

amcl功能包中的话题和服务

V3COqU.png

坐标转换

V3CqMV.png

  • 里程计定位：只通过里程计的数据来处理/base和/odom之间的TF转换；
  • AMCL定位：可以估算机器人在地图坐标系/map下的位姿信息，提供/base、/odom、/map之间的TF变换。

amcl参数配置

在 agv_car 功能包launch目录下新建amcl.launch文件：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
<launch>
    <node pkg="amcl" type="amcl" name="amcl" output="screen">
        <remap from="scan" to="scan_filtered"/>

        <!-- Overall filter parameters -->
        <param name="min_particles" value="500"/>
        <param name="max_particles" value="2000"/>
        <param name="kld_err" value="0.05"/>
        <param name="kld_z" value="0.99"/>
        <param name="update_min_d" value="0.2"/> <!-- Translational movement required before performing a filter update. -->
        <param name="update_min_a" value="0.2"/> <!-- Rotational movement required before performing a filter update. 0.1 represents 5.7 degrees  -->
        <param name="resample_interval" value="1"/> <!-- Number of filter updates required before resampling. -->
        <param name="transform_tolerance" value="1.25"/> <!-- Default 0.1; time with which to post-date the transform that is published, to indicate that this transform is valid into the future. -->
        <param name="recovery_alpha_slow" value="0.001"/> <!-- Exponential decay rate for the slow average weight filter, used in deciding when to recover by adding random poses. -->
        <param name="recovery_alpha_fast" value="0.1"/> <!-- Exponential decay rate for the fast average weight filter, used in deciding when to recover by adding random poses. -->
        <param name="gui_publish_rate" value="10.0"/> <!-- Maximum rate (Hz) at which scans and paths are published for visualization, -1.0 to disable. -->
        <param name="save_pose_rate"   value="0.5"/>
        <param name="use_map_topic"    value="false"/>
        <param name="first_map_only"   value="false"/>
        <!-- set particles init pose to robot pose -->
        <param name="initial_pose_x" value="0.0"/>
        <param name="initial_pose_y" value="0.0"/>
        <param name="initial_pose_a" value="0.0"/>
        <param name="initial_cov_xx" value="0.25"/>
        <param name="initial_cov_yy" value="0.25"/>
        <param name="initial_cov_aa" value="0.068538917"/> <!-- pi/12 * pi/12 -->

        <!-- Laser model parameters -->
        <param name="laser_max_beams" value="60"/>	
        <param name="laser_z_hit" value="0.5"/>
        <param name="laser_z_short" value="0.05"/>
        <param name="laser_z_max" value="0.05"/>
        <param name="laser_z_rand" value="0.5"/>
        <param name="laser_sigma_hit" value="0.2"/>
        <param name="laser_lambda_short" value="0.1"/>
        <param name="laser_likelihood_max_dist" value="2.0"/>
        <param name="laser_model_type" value="likelihood_field"/>

        <!-- Odometry model parameters -->
        <param name="odom_model_type" value="diff"/>
        <param name="odom_alpha1" value="0.25"/> <!-- Specifies the expected noise in odometry's rotation estimate from the rotational component of the robot's motion. -->
        <param name="odom_alpha2" value="0.25"/> <!-- Specifies the expected noise in odometry's rotation estimate from translational component of the robot's motion. -->
        <param name="odom_alpha3" value="0.25"/> <!-- Specifies the expected noise in odometry's translation estimate from the translational component of the robot's motion. -->
        <param name="odom_alpha4" value="0.25"/> <!-- Specifies the expected noise in odometry's translation estimate from the rotational component of the robot's motion. -->
        <param name="odom_alpha5" value="0.0"/> <!--only used if model is "omni"-->
        <param name="odom_frame_id" value="odom"/>
        <param name="base_frame_id" value="base_footprint"/> <!-- Change this if you want to change your base frame id. -->
        <param name="global_frame_id" value="map"/>
    </node>
</launch>

关于参数的详细介绍：

（1）滤波器可以设置的所有参数

  • ~min_particles (int, default: 100)：滤波器中的最少粒子数，值越大定位效果越好，但是相应的会增加主控平台的计算资源消耗。
  • ~max_particles (int, default: 5000)：滤波器中最多粒子数，是一个上限值，因为太多的粒子数会导致系统资源消耗过多。
  • ~kld_err (double, default: 0.01)：真实分布与估计分布之间的最大误差。
  • ~kld_z (double, default: 0.99)：上标准分位数（1-p），其中p是估计分布上误差小于kld_err的概率，默认0.99。
  • ~update_min_d (double, default: 0.2 meters)：在执行滤波更新前平移运动的距离，默认0.2m(对于里程计模型有影响，模型中根据运动和地图求最终位姿的似然时丢弃了路径中的相关所有信息，已知的只有最终位姿，为了规避不合理的穿过障碍物后的非零似然，这个值建议不大于机器人半径，否则因更新频率的不同可能产生完全不同的结果)。
  • ~update_min_a (double, default: π/6.0 radians)：执行滤波更新前旋转的角度。
  • ~resample_interval (int, default: 2)：在重采样前需要滤波更新的次数。
  • ~transform_tolerance (double, default: 0.1 seconds)：tf变换发布推迟的时间，为了说明tf变换在未来时间内是可用的。
  • ~recovery_alpha_slow (double, default: 0.0 (disabled))：慢速的平均权重滤波的指数衰减频率，用作决定什么时候通过增加随机位姿来recover，默认0（disable），可能0.001是一个不错的值。
  • ~recovery_alpha_fast (double, default: 0.0 (disabled))：快速的平均权重滤波的指数衰减频率，用作决定什么时候通过增加随机位姿来recover，默认0（disable），可能0.1是个不错的值。
  • ~initial_pose_x (double, default: 0.0 meters)：初始位姿均值（x），用于初始化高斯分布滤波器。（initial_pose_参数决定撒出去的初始位姿粒子集范围中心）。
  • ~initial_pose_y (double, default: 0.0 meters)：初始位姿均值（y），用于初始化高斯分布滤波器。（同上）
  • ~initial_pose_a (double, default: 0.0 radians)：初始位姿均值（yaw），用于初始化高斯分布滤波器。（粒子朝向）
  • ~initial_cov_xx (double, default: 0.50.5 meters)：初始位姿协方差（xx），用于初始化高斯分布滤波器。（initial_cov_参数决定初始粒子集的范围）
  • ~initial_cov_yy (double, default: 0.50.5 meters)：初始位姿协方差（yy），用于初始化高斯分布滤波器。（同上）
  • ~initial_cov_aa (double, default: (π/12)(π/12) radian)：初始位姿协方差（yawyaw），用于初始化高斯分布滤波器。（粒子朝向的偏差）
  • ~gui_publish_rate (double, default: -1.0 Hz)：扫描和路径发布到可视化软件的最大频率，设置参数为-1.0意为失能此功能，默认-1.0。
  • ~save_pose_rate (double, default: 0.5 Hz)：存储上一次估计的位姿和协方差到参数服务器的最大速率。被保存的位姿将会用在连续的运动上来初始化滤波器。-1.0失能。
  • ~use_map_topic (bool, default: false)：当设置为true时，AMCL将会订阅map话题，而不是调用服务返回地图。也就是说当设置为true时，有另外一个节点实时的发布map话题，也就是机器人在实时的进行地图构建，并供给amcl话题使用；当设置为false时，通过map server，也就是调用已经构建完成的地图。
  • ~first_map_only (bool, default: false)：当设置为true时，AMCL将仅仅使用订阅的第一个地图，而不是每次接收到新的时更新为一个新的地图。

（2）可以设置的所有激光模型参数

请注意无论使用哪种混合权重都应该等于1，对于laser_model_type是beam时会用到4个参数z_hit，z_short，z_max和z_rand，如果是likelihood_field模型仅使用2个：z_hit和z_rand。这4个laser_z_参数，是在动态环境下的定位时用于异常值去除技术的参数。

  • ~laser_min_range (double, default: -1.0)：最小扫描范围，参数设置为-1.0时，将会使用激光上报的最小扫描范围。

  • ~laser_max_range (double, default: -1.0)：最大扫描范围，参数设置为-1.0时，将会使用激光上报的最大扫描范围。

  • ~laser_max_beams (int, default: 30)：更新滤波器时，每次扫描中多少个等间距的光束被使用（减小计算量，测距扫描中相邻波束往往不是独立的可以减小噪声影响，太小也会造成信息量少定位不准）。

  • ~laser_z_hit (double, default: 0.95)：模型的z_hit部分的混合权值，默认0.95(混合权重1.具有局部测量噪声的正确范围–以测量距离近似真实距离为均值，其后laser_sigma_hit为标准偏差的高斯分布的权重)。

  • ~laser_z_short (double, default: 0.1)：模型的z_short部分的混合权值，默认0.1（混合权重2.意外对象权重（类似于一元指数关于y轴对称0～测量距离（非最大距离）的部分：–ηλe^(-λz)，其余部分为0，其中η为归一化参数，λ为laser_lambda_short,z为t时刻的一个独立测量值（一个测距值，测距传感器一次测量通常产生一系列的测量值）），动态的环境，如人或移动物体）。

  • ~laser_z_max (double, default: 0.05)：模型的z_max部分的混合权值，默认0.05（混合权重3.测量失败权重（最大距离时为1，其余为0），如声呐镜面反射，激光黑色吸光对象或强光下的测量，最典型的是超出最大距离）。

  • ~laser_z_rand (double, default: 0.05)：模型的z_rand部分的混合权值，默认0.05（混合权重4.随机测量权重–均匀分布（1平均分布到0～最大测量范围），完全无法解释的测量，如声呐的多次反射，传感器串扰）。

  • ~laser_sigma_hit (double, default: 0.2 meters)：被用在模型的z_hit部分的高斯模型的标准差，默认0.2m。

  • ~laser_lambda_short (double, default: 0.1)：模型z_short部分的指数衰减参数，默认0.1（根据ηλe^(-λz)，λ越大随距离增大意外对象概率衰减越快）。

  • ~laser_likelihood_max_dist (double, default: 2.0 meters)：地图上做障碍物膨胀的最大距离，用作likelihood_field模型（likelihood_field_range_finder_model只描述了最近障碍物的距离，（目前理解应该是在这个距离内的障碍物膨胀处理,但是算法里又没有提到膨胀，不明确是什么意思）.这里算法用到上面的laser_sigma_hit。似然域计算测量概率的算法是将t时刻的各个测量（舍去达到最大测量范围的测量值）的概率相乘，单个测量概率：Zh * prob(dist,σ) +avg，Zh为laser_z_hit,avg为均匀分布概率，dist最近障碍物的距离，prob为0为中心标准方差为σ（laser_sigma_hit）的高斯分布的距离概率。

  • ~laser_model_type (string, default: "likelihood_field")：激光模型类型定义，可以是beam, likehood_field, likehood_field_prob（和likehood_field一样但是融合了beamskip特征–官网的注释），默认是“likehood_field” 。

（3）里程计模型参数

  • ~odom_model_type (string, default: "diff")：odom模型定义，可以是”diff”, “omni”, “diff-corrected”, “omni-corrected”，后面两个是对老版本里程计模型的矫正，相应的里程计参数需要做一定的减小。
  • ~odom_alpha1 (double, default: 0.2)：指定由机器人运动部分的旋转分量估计的里程计旋转的期望噪声，默认0.2（旋转存在旋转噪声）。
  • ~odom_alpha2 (double, default: 0.2)：机器人运动部分的平移分量估计的里程计旋转的期望噪声，默认0.2（旋转中可能出现平移噪声）。
  • ~odom_alpha3 (double, default: 0.2)：机器人运动部分的平移分量估计的里程计平移的期望噪声，如果你自认为自己机器人的里程计信息比较准确那么就可以将该值设置的很小。
  • ~odom_alpha4 (double, default: 0.2)：机器人运动部分的旋转分量估计的里程计平移的期望噪声，你设置的这4个alpha值越大说明里程计的误差越大。
  • ~odom_alpha5 (double, default: 0.2)：平移相关的噪声参数（仅用于模型是“omni”的情况，就是当你的机器人是全向移动时才需要设置该参数，否则就设置其为0.0）
  • ~odom_frame_id (string, default: "odom")：里程计默认使用的坐标系。
  • ~base_frame_id (string, default: "base_link")：机器人的基坐标系。
  • ~global_frame_id (string, default: "map")：由定位系统发布的坐标系名称。
  • ~tf_broadcast (bool, default: true)：设置为false阻止amcl发布全局坐标系和里程计坐标系之间的tf变换。

参考资料：

[1] ROS小课堂——4.在STDR中使用amcl进行仿真机器人定位

[2] ROS wiki–amcl

开始导航

前面已经配置好了导航需要的相关参数，现在我们就可以开始实现机器人的导航了。

创建launch文件

在 agv_car 功能包launch目录下新建启动文件，命名为navigation.launch。主要完成下面几个操作：

  • 加载地图配置
  • 启动move_base节点
  • 启动AMCL节点
  • 发布tf坐标变换
  • 启动rviz可视化窗口
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
<launch>

  <!-- Run the map server -->
  <node name="map_server" pkg="map_server" type="map_server" args="$(find agv_car)/maps/map.yaml" output="screen"/>

  <!-- move_base -->
  <include file="$(find agv_car)/launch/move_base.launch" />

  <!-- AMCL -->
  <include file="$(find agv_car)/launch/amcl.launch" />

  <!-- Set a static coordinate transformation from /odom to /map -->
  <node pkg="tf" type="static_transform_publisher" name="map_to_odom" args="0 0 0 0 0 0 /map /odom 40" />

  <!-- rviz -->
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find agv_car)/rviz/nav.rviz" />

</launch>

启动机器人开始导航

因为需要使用到dwa-local-planner、global_planner、move_base和amcl功能包，可以使用下面的命令进行安装：

1
2
3
4
sudo apt-get install ros-kinetic-dwa-local-planner
sudo apt-get install ros-kinetic-global-planner
sudo apt-get install ros-kinetic-move-base
sudo apt-get install ros-kinetic-amcl

现在，可以使用下面的命令就可以开始机器人导航了：

1
2
3
roslaunch agv_car laser4.launch
rosrun agv_car base_control
roslaunch agv_car navigation.launch

启动完成后你会看到类似下面的界面，点击菜单栏的”2D Nav Goal”按钮，然后在地图上任意选择一个目标点，这是机器人就会开始向目标点移动。

3Adb1U.png

ROS > 两轮差速底盘SLAM
#ROS #SLAM #Navigation
两轮差速底盘SLAM系列(10)--导航配置
https://kevinloongc.github.io/posts/25677.html
作者
Kevin Loongc
发布于
2020年2月18日
许可协议