【路径规划】move_base、路径规划算法、局部避障算法介绍_move_base导入地图进行路径规划

资料整理供个人学习使用。

一、move_base

1、move_base 包内容

需要安装 navigation 包，才可以运行 move_base。

move_base 包默认算法：

1. 全局路径规划：Dijkstra；
2. 局部路径规划：航迹推算；

ROS 系统的路径规划由 move_base 统筹，在 move_base 中集成了代价地图模块 costmap_2d，全局规划器（此处使用 global_planner/GlobalPlanner）和局部规划器（dwa_local_planner/DWAPlannerROS），其中 costmap_2d 分别衍生了全局代价地图和局部代价地图，此外还有恢复模块 recovery_behaviors，采用默认值即可。

2、move_base 参数解析

move_base 需要设置六个部分的参数

1. move_base 自身参数
2. 全局代价地图（global_costmap）
3. 局部代价地图（local_costmap）
4. 全局规划器（global_panner）
5. 局部规划器（local_planner）
6. recovery_behaviors（使用默认值先不讲述）

以上参数在 move_base 节点加载时全部加载，用以下参数文件格式

<launch>
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
	<rosparam file="$(find xxx)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
	<rosparam file="$(find xxx)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find xxx)/param/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find xxx)/param/global_costmap_params.yaml" command="load" />
	<rosparam file="$(find xxx)/param/base_local_planner_params.yaml" command="load" />
	<rosparam file="$(find xxx)/param/base_global_planner_params.yaml" command="load" />
    <rosparam file="$(find xxx)/param/move_base_params.yaml" command="load" />
	<remap from="/cmd_vel" to="/cmd_vel"/>
  </node>
</launch>
1
2
3
4
5
6
7
8
9
10
11
12

1. move_base 参数

move_base 自身的参数是为了控制其他模块，有着相应的关联性

• ~base_global_planner
  (string, default: “navfn/NavfnROS” For 1.1+ series)
  全局规划器，用于规划最优路径，但是默认值中没有没有实现 A* 算法的全局规划，在版本迭代优化后通常设置为 global_planner/GlobalPlanner，同时可以设置 A* 或者 D*

• ~base_local_planner
  (string, default: “base_local_planner/TrajectoryPlannerROS” For 1.1+ series)
  局部规划器，用于运动过程中的动态避障，默认值现在通常设置为 dwa_local_planner/DWAPlannerROS

• ~recovery_behaviors
  (list, default:
  [{name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery},
  {name: rotate_recovery, type: rotate_recovery/RotateRecovery},
  {name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery}]
  For 1.1+ series)
  恢复模块，一个是恢复行为 rotate_recovery，一个是地图清理 clear_costmap_recovery，通常使用默认值即可

注意：当全局路径规划无法到达目标点时，才会执行这个恢复模块

• ~controller_frequency
  (double, default: 20.0)
  向机器人底盘发送的控制速度的频率，这个速度由base_local_planner计算

• ~planner_patience
  (double, default: 5.0)
  进行全局规划的时间间隔，如果超时则认为规划失败

• ~controller_patience
  (double, default: 15.0)
  等待控制速度的时间间隔，如果控制速度的发布超过设置时间，则认为局部路径规划失败

• ~conservative_reset_dist
  (double, default: 3.0)
  在恢复模块执行后，重置的代价地图范围

• ~recovery_behavior_enabled
  (bool, default: true)
  是否使用恢复模块，默认值为true

• ~clearing_rotation_allowed
  (bool, default: true)
  是否允许旋转恢复行为，默认为true

• ~shutdown_costmaps
  (bool, default: false)
  当move_base不活动时，是否关闭代价地图的加载

• ~oscillation_timeout
  (double, default: 0.0)
  在执行恢复行为之前允许振荡的时间（秒）。

• ~oscillation_distance
  (double, default: 0.5)
  机器人必须移动多远（以米计）才能被视为不摆动。如果出现摆动则说明全局规划失败，那么将在超时后执行恢复模块。

• ~planner_frequency
  (double, default: 0.0)
  全局规划器的执行频率，如果为 0 则只有出现新的目标点，才会重新规划

• ~max_planning_retries
  (int32_t, default: -1)

在执行恢复行为之前允许计划重试的次数默认为 -1，表示全局规划失败后立即执行恢复模块。

2. 全局代价地图参数

全局代价地图和局部代价地图都衍生于 costmap_2d 功能包，参数设置也是一样的，但是考虑到路径规划和避障的职能不同，可以采用不同的参数策略。

全局代价地图可以将膨胀半径设置更大，障碍物系数设置更高，从而使得路径规划中，能够得到远离障碍物的规划路径。
而在局部规划中，为了避免避障陷入困境，可以设置小一点的膨胀半径和障碍物系数，使得机器人能够绕过障碍物。

• ~<name>/plugins
  (sequence, default: pre-Hydro behavior)
  地图序列，每层一个。每个地图都是一个包含名称和类型字段的字典。

• ~<name>/global_frame
  (string, default: “/map”)
  代价地图的世界坐标系

• ~<name>/robot_base_frame
  (string, default: “base_link”)
  机器人底座的局部坐标系

• ~<name>/transform_tolerance
  (double, default: 0.2)
  等待坐标变换发布信息的超时时间

• ~<name>/update_frequency
  (double, default: 5.0)
  代价地图的更新频率

• ~<name>/publish_frequency
  (double, default: 0.0)
  代价地图的发布频率

• ~<name>/rolling_window
  (bool, default: false)
  是否使用动态窗口，默认为 false，在静态的全局地图中，地图不会变化

• ~<name>/always_send_full_costmap
  (bool, default: false)
  是否发布全部代价地图，默认 false，只会发布部分的代价地图，加载全图耗费计算资源

下面为设置代价地图的尺寸和精度参数

• ~<name>/width (int, default: 10)
  代价地图宽度

• ~<name>/height (int, default: 10)
  代价地图高度

• ~<name>/resolution (double, default: 0.05)
  代价地图精度，即每个网格的尺寸

• ~<name>/origin_x (double, default: 0.0)
  代价地图原点x坐标在世界坐标系中的位置

• ~<name>/origin_y (double, default: 0.0)
  代价地图原点 y 坐标在世界坐标系中的位置

全局坐标到机器人底座的变换信息，可由AMCL等发布

代价地图包含多个地图层

1. Static Map Layer
2. Obstacle Map Layer
3. Inflation Layer
4. Other Layers

下面举例说明，在Inflation Layer中需要设置两个计算障碍物代价值的参数

• ~<name>/inflation_radius (double, default: 0.55)
  膨胀半径，扩展在碰撞区域以外的代价区域，使得机器人规划路径避开障碍物

• ~<name>/cost_scaling_factor (double, default: 10.0)
  代价比例系数，越大则代价值越小

公式如下 exp(-1.0 * cost_scaling_factor * (distance_from_obstacle - inscribed_radius)) * (costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1)
其中 costmap_2d::INSCRIBED_INFLATED_OBSTACLE=254 也就是说代价值最大为 253

3. 局部代价地图参数

参数设置与全局相似，只是参数的策略上可能会不同

4. 全局规划器参数

• ~<name>/allow_unknown
  (bool, default: true)
  是否允许规划路径穿越未知区域

• ~<name>/default_tolerance
  (double, default: 0.0)
  路径规划的终点与目标点的偏差距离

• ~<name>/visualize_potential
  (bool, default: false)
  是否允许发布点云表达的代价趋势

• ~<name>/use_dijkstra
  (bool, default: true)
  是否使用 D* 全局规划算法，默认为 true，如果为 false，则使用 A* 算法

• ~<name>/use_quadratic
  (bool, default: true)
  是否使用二次逼近的趋势，默认为 true，否则使用简单的线性方法

• ~<name>/use_grid_path
  (bool, default: false)
  是否沿着网格规划路径，默认为 false，false 时使用最速下降法计算路径

• ~<name>/old_navfn_behavior
  (bool, default: false)
  是否使用旧版本导航功能，默认 false

• ~<name>/lethal_cost
  (int, default: 253)
  最高的代价值，默认 253

• ~<name>/neutral_cost
  (int, default: 50)
  中性代价系数

• ~<name>/cost_factor
  (double, default: 3.)
  占用网格的系数

• ~<name>/publish_potential
  (bool, default: True)
  是否发布趋势，默认为 true

• ~<name>/orientation_mode
  (int, default: 0)
  方向设置，默认 0，采用默认坐标系

• ~<name>/orientation_window_size
  (int, default: 1)
  方向窗口大小，默认为 1

5. 局部规划器参数

局部规划器使用局部代价地图，实现在全局规划路径上的避障功能

速度参数配置

• ~<name>/acc_lim_x (double, default: 2.5)
  x轴方向最高加速度

• ~<name>/acc_lim_y (double, default: 2.5)
  y轴方向最高加速度

• ~<name>/acc_lim_th (double, default: 3.2)
  最高旋转加速度

• ~<name>/max_trans_vel (double, default: 0.55)
  最大线速度

• ~<name>/min_trans_vel (double, default: 0.1)
  最小线速度

• ~<name>/max_vel_x (double, default: 0.55)
  最大x轴方向线速度

• ~<name>/min_vel_x (double, default: 0.0)
  最小x轴方向线速度

• ~<name>/max_vel_y (double, default: 0.1)
  最大y轴方向线速度

• ~<name>/min_vel_y (double, default: -0.1)
  最小y轴方向线速度

• ~<name>/max_rot_vel (double, default: 1.0)
  最大旋转角速度

• ~<name>/min_rot_vel (double, default: 0.4)
  最小旋转角速度

目标误差参数

• ~<name>/yaw_goal_tolerance (double, default: 0.05)
  路径规划到达点与目标点的角度允许偏差

• ~<name>/xy_goal_tolerance (double, default: 0.10)
  路径规划到达点与目标点的距离允许偏差

• ~<name>/latch_xy_goal_tolerance (bool, default: false)
  通常不使用

前向仿真系数

• ~<name>/sim_time (double, default: 1.7)
  仿真时间，仿真距离=sim_time*vel

• ~<name>/sim_granularity (double, default: 0.025)
  轨迹的碰撞检测距离

• ~<name>/vx_samples (integer, default: 3)
  x轴方向速度采样数量

• ~<name>/vy_samples (integer, default: 10)
  y轴方向速度采样数量

• ~<name>/vth_samples (integer, default: 20)
  角速度采样数量

• ~<name>/controller_frequency (double, default: 20.0)
  发布控制速度的频率

轨迹评分参数

• ~<name>/path_distance_bias (double, default: 32.0)
  局部规划路径与全局路径保持一致的权重系数

• ~<name>/goal_distance_bias (double, default: 24.0)
  无论从什么路径以多大权重尝试到达目标点，增大后于全局规划路径一致性降低

• ~<name>/occdist_scale (double, default: 0.01)
  权衡机器人以多大的权重躲避障碍物。该值过大会导致机器人陷入困境

• ~<name>/forward_point_distance (double, default: 0.325)
  将机器人与目标点连线并延长forward_point_distance距离作为一个评分点

• ~<name>/stop_time_buffer (double, default: 0.2)
  为防止碰撞,机器人必须提前停止的时间长度

• ~<name>/scaling_speed (double, default: 0.25)
  开始缩放机器人足迹时的速度的绝对值，单位为m/s

• ~<name>/max_scaling_factor (double, default: 0.2)
  最大缩放因子

• ~<name>/publish_cost_grid (bool, default: false)
  是否发布规划器在规划路径时的代价网格.如果设置为 true，那么就会在 ~/cost_cloud 话题上发布 sensor_msgs/PointCloud2 类型消息

避免振荡参数

• ~<name>/oscillation_reset_dist (double, default: 0.05)
  机器人运动多远距离才会重置振荡标记

二、路径规划

1、Dijkstra

• 用来寻找图形中节点之间的最短路径
• 优先级队列以每个节点距离起点的总移动代价排序
• 每次从优先级队列中选取代价最小的作为下一个遍历的节点
• 不考虑节点移动代价差异时，退化为BFS

2、最佳优先搜索

• 优先级队列以每个节点到终点的距离为优先级
• 每次选取到终点移动代价最小的节点作为下一个遍历的节点

缺点：如果起点和终点之间存在障碍物，则最佳优先算法找到的很可能不是最短路径

3、A*

f(n) 是节点 n 的综合优先级。当我们选择下一个要遍历的节点时，我们总会选取综合优先级最高（值最小）的节点。

• g(n) 是节点 n 距离起点的代价
• h(n) 是节点 n 距离终点的预计代价，这也就是 A* 算法的启发函数。

启发函数：控制算法的速度和精确度

• 在极端情况下，当启发函数 h(n) 始终为 0，则将由 g(n) 决定节点的优先级，此时算法就退化成了 Dijkstra 算法。
• 如果 h(n) 始终小于等于节点 n 到终点的代价，则 A* 算法保证一定能够找到最短路径。但是当 h(n) 的值越小，算法将遍历越多的节点，也就导致算法越慢。
• 如果 h(n) 完全等于节点 n 到终点的代价，则 A* 算法将找到最佳路径，并且速度很快。可惜的是，并非所有场景下都能做到这一点。因为在没有达到终点之前，我们很难确切算出距离终点还有多远。
• 如果 h(n) 的值比节点 n 到终点的代价要大，则 A* 算法不能保证找到最短路径，不过此时会很快。
• 在另外一个极端情况下，如果 h(n) 相较于 g(n) 大很多，则此时只有 h(n) 产生效果，这也就变成了最佳优先搜索。
• 对于网格形式的图，有以下这些启发函数可以使用
  • 如果图形中只允许朝上下左右四个方向移动，则可以使用曼哈顿距离（Manhattan distance）。
  • 如果图形中允许朝八个方向移动，则可以使用对角距离。
  • 如果图形中允许朝任何方向移动，则可以使用欧几里得距离（Euclidean distance）。

4、A* 和 Dijkstra 比较

• Dijkstra 算法计算源点到其他所有点的最短路径长度，A* 关注点到点的最短路径（包括具体路径)。
• Dijkstra 算法建立在较为抽象的图论层面，A* 算法可以更轻松地用在诸如游戏地图寻路中。
• Dijkstra 算法的实质是广度优先搜索，是一种发散式的搜索，所以空间复杂度和时间复杂度都比较高。对路径上的当前点，A* 算法不但记录其到源点的代价，还计算当前点到目标点的期望代价，是一种启发式算法，也可以认为是一种深度优先的算法。
• 由第一点，当目标点很多时，A* 算法会带入大量重复数据和复杂的估价函数，所以如果不要求获得具体路径而只比较路径长度时，Dijkstra 算法会成为更好的选择。

三、局部避障

1、dwa（动态窗口法）

• 原理主要是在速度空间（v,w）中采样多组速度，并模拟出这些速度在一定时间内的运动轨迹，并通过评价函数对这些轨迹进行评价，选取最优轨迹对应的速度驱动机器人运动。
• 动态窗口法 DWA 的实现包含两个步骤
  • 对机器人速度进行约束限制，形成动态窗口进行速度采样；
  • 根据速度采样点求出待评价轨迹，最大化评价函数选取最优速度命令。
• 动态窗口是由一系列的约束构成，其中约束主要包括差动机器人的非完整约束、环境障碍物约束与受结构与电机影响的动力学约束。

优点

• 计算复杂度低：考虑到速度和加速度的限制，只有安全的轨迹会被考虑，且每次采样的时间较短，因此轨迹空间较小
• 可以实现避障：可以实时避障，但是避障效果一般
• 适用于差分和全向车模

缺点

• 前瞻性不足：只模拟并评价了下一步，如在机器人前段遇见 “C” 字形障碍时，不能很好的避障
• 动态避障效果差： 模拟运动轨迹断，动态避障效果差
• 非全局最优路径： 每次都选择下一步的最佳路径，而非全局最优路径不适用于阿克曼模型车模

2、teb（时间弹力带）

• TEB 全称为 Time Elastic Band，定义橡皮筋连接起始、目标点，并让这个路径可以变形，变形的条件就是将所有约束当做橡皮筋的外力。
• 关于 time eletic band 的简述：起始点、目标点状态由用户/全局规划器指定，中间插入 N 个控制橡皮筋形状的控制点（机器人姿态），当然，为了显示轨迹的运动学信息，我们在点与点之间定义运动时间 Time，即为 Timed-Elastic-Band 算法。通过此方法可以把问题描述为一个多目标优化问题，通过构建超图(hyper-graph)，使用 g2o 框架中的图优化来求解

优点

• 适用于各种常见车模：如差分、全向、阿克曼模型
• 有很强的前瞻性： 对前方一段轨迹进行优化
• 动态避障： 对动态障碍有较好的避障效果，可直接使用其封装好障碍类 Obstacle 如：静态障碍时 TEB 算法轨迹规划效果

缺点

• 计算复杂度较大：可通过牺牲预测距离来降低复杂度
• 速度和角度波动较大、控制不稳定：源码中是通过两状态之间的距离和角度差及时间差来计算该控制周期内的速度和角速度，使得在控制过程中速度和角度波动较大。在计算资源足够的情况下，提高控制频率可以改善此现象。

3、dwa 和 teb 比较

teb 在运动过程中会调整自己的位姿朝向，当到达目标点时，通常机器人的朝向也是目标朝向而不需要旋转。

dwa 则是先到达目标坐标点，然后原地旋转到目标朝向。

四、move_base 更换全局路径规划算法

move_base 更换全局路径规划算法（A*、Dijkstra、DWA）
A*、Dijkstra属于全局路径规划、DWA属于局部路径规划。

1、move_base.launch

move_base.launch 文件需要添加以下内容：

    <!--加载A star、Dijkstra算法-->
    <param name="base_global_planner" value="global_planner/GlobalPlanner"/>
    <param name="planner_frequency" value="1.0" />
    <param name="planner_patience" value="5.0" />
    <!--加载DWA算法-->
    <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
1
2
3
4
5
6

整体的 move_base.launch 文件内容如下（其中 xxx 内容需要修改为对应自己的包）：

<launch>
  <!-- Arguments -->
  <arg name="model" default="$(env TURTLEBOT3_MODEL)" doc="model type [burger, waffle, waffle_pi]"/>
  <arg name="cmd_vel_topic" default="/cmd_vel" />
  <arg name="odom_topic" default="/odom" />
  <arg name="move_forward_only" default="false"/>
 
  <!-- move_base -->
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <!--加载A star算法-->
    <param name="base_global_planner" value="global_planner/GlobalPlanner"/>
    <param name="planner_frequency" value="1.0" />
    <param name="planner_patience" value="5.0" />
    <!--加载DWA算法-->
    <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
    <!---->
    <rosparam file="$(find xxx)/param/costmap_common_params_$(arg model).yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find xxx)/param/costmap_common_params_$(arg model).yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find xxx)/param/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find xxx)/param/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find xxx)/param/move_base_params.yaml" command="load" />
    <rosparam file="$(find xxx)/param/dwa_local_planner_params_$(arg model).yaml" command="load" />
    <remap from="cmd_vel" to="$(arg cmd_vel_topic)"/>
    <remap from="odom" to="$(arg odom_topic)"/>
    <param name="DWAPlannerROS/min_vel_x" value="0.0" if="$(arg move_forward_only)" />
  </node>
</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

2、修改 planner_core.cpp 文件

planner_core.cpp 文件是 navigation 包中的文件，其具体目录如下（对应自己的路径）：

修改如下内容：

private_nh.param("use_dijkstra", use_dijkstra, false);
1

此句 true 为使用 dijkstra 算法，false 为使用 A star 算法。

ROS_INFO("use_dijkstra");
ROS_INFO("use_A_star");
1
2

此句可以在运行路径规划时，查看自己使用的是 dijkstra 还是 A star。

注意：修改完代码之后，需要删除 build 和 devel 文件，重新 catkin_make 编译。

3、问题

原因：膨胀半径设置问题，需要自己设置，最简单方法是将膨胀半径设置为机器人外切圆。

问题：机器人在运行时会出现倒着走的现象。
解决：将 base_local_planner_params.yaml 中 holonomic_robot 参数改为 false；
holonomic_robot参数意义：代表是否是全向机器人。

参考：

1. ROS：move_base路径规划介绍、更换全局路径规划算法（A star、Dijkstra、DWA，测试当前是哪种算法，效果展示图）
2. 路径规划参数解析（一）——move_base参数解析
3. 路径规划之 A* 算法
4. https://www.leiphone.com/category/transportation/0TCtaBOIcFOIBN69.html
5. ROS局部运动规划器Teb/DWA