ROS中局部导航算法介绍及部分算法配置

概述

概述参考

在ROS中，进行导航需要使用到的三个包是：

（1） move_base：根据参照的消息进行路径规划，使移动机器人到达指定的位置；
（2） gmapping：根据激光数据（或者深度数据模拟的激光数据）建立地图；
（3） amcl：根据已经有的地图进行定位。

在总体框架图中可以看到，move_base提供了ROS导航的配置、运行、交互接口，它主要包括两个部分：
（1） 全局路径规划（global planner）：根据给定的目标位置进行总体路径的规划.

在ROS的导航中，首先会通过全局路径规划，计算出机器人到目标位置的全局路线。

这一功能是navfn这个包实现的。

navfn通过Dijkstra或A*最优路径的算法，计算costmap上的最小花费路径，作为机器人的全局路线。

常用的global_planner:

A*、Dijstra、prm、人工势场、单元分解、快速搜索树（RRT）等

（2） 本地实时规划（local planner）：根据附近的障碍物进行躲避路线规划。

本地的实时规划是利用base_local_planner包实现的。

该包使用Trajectory Rollout 和Dynamic Window approaches算法计算机器人每个周期内应该行驶的速度和角度（dx，dy，dtheta velocities）。

base_local_planner这个包通过地图数据，通过算法搜索到达目标的多条路经，利用一些评价标准（是否会撞击障碍物，所需要的时间等等）选取最优的路径，并且计算所需要的实时速度和角度。

其中，Trajectory Rollout 和Dynamic Window approaches算法的主要思路如下：
（1） 采样机器人当前的状态（dx,dy,dtheta）；
（2） 针对每个采样的速度，计算机器人以该速度行驶一段时间后的状态，得出一条行驶的路线。
（3） 利用一些评价标准为多条路线打分。
（4） 根据打分，选择最优路径。
（5） 重复上面过程。

常用的local_planner:

base_local_planner
dwa_local_planner
teb_local_planner
eband_local_planner
asr_ftc_local_planner
dwb_local_planner

base_local_planner(或者TrajectoryPlannerROS)

base_local_planner::TrajectoryPlannerROS对象是base_local_planner::TrajectoryPlanner对象的ROS封装，在初始化时指定的ROS命名空间使用，继承了nav_core::BaseLocalPlanner接口。它是move_base默认的局部规划包。该软件包提供了对平面上本地机器人导航的轨迹展开和动态窗口方法的实现。根据计划遵循和成本图，控制器生成速度命令以发送到移动基站。该软件包支持完整和非完整机器人，可以表示为凸多边形或圆形的任何机器人足迹，并将其配置公开为可在启动文件中设置的ROS参数。此包的ROS包装器遵循nav_core包中指定的BaseLocalPlanner接口。与dwa_local_planner思路接近。

唯一区别是DWA与“TrajectoryPlanner”的不同之处在于如何对机器人的控制空间进行采样。在给定机器人的加速度极限的情况下，TrajectoryPlanner在整个前向模拟周期内从可实现的速度集合中进行采样，而DWA在给定机器人的加速度极限的情况下仅针对一个模拟步骤从可实现的速度集合中进行采样。在实践中，我们发现DWA和轨迹展示在我们的所有测试中都具有相同的性能，并建议使用DWA来提高效率，因为其样本空间更少。

dwa_local_planner

dwa算法介绍
DWA算法全称为：Dynamic Window Approach，中文为动态窗口法，其原理主要是在速度空间（v,w）中采样多组速度，并模拟这些速度在一定时间内的运动轨迹，再通过一个评价函数对这些轨迹打分，最优的速度被选择出来发送给下位机。

动态窗口的意思是根据移动机器人的加减速性能限定速度采样空间在一个可行的动态范围呢。

DWA算法基本思路如下:

1. 在机器人控制空间进行速度离散采样(dx,dy,dtheta)
2. 对每一个采样速度执行前向模拟，看看使用该采样速度移动一小段段时间后会发生什么
3. 评价前向模拟中每个轨迹，评价准则如: 靠近障碍物，靠近目标，贴近全局路径和速度；丢弃非法轨迹（如哪些靠近障碍物的轨迹）
4. 挑出得分最高的轨迹并发送相应速度给移动底座
5. 重复上面步骤.

dwa算法介绍
ros中dwa使用
dwa原理推导

• dwa_local_planner配置方法

1. 可输入下面的命令进行安装

sudo apt-get install ros-melodic-dwa-local-planner

2. 检测是否安装成功

roscd dwa_local_planner

3. 创建launch文件 如下

<launch>

    <!-- 设置地图的配置文件 -->
    <arg name="map" default="cloister_gmapping.yaml" />

    <!-- 运行地图服务器，并且加载设置的地图-->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find ares_navigation)/maps/$(arg map)"/>

    <!-- 运行move_base节点 -->
    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
    <!--开启dwa局部导航节点-->
    <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
    <!--加载设置参数-->
    <rosparam file="$(find ares_navigation)/config/ares/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find ares_navigation)/config/ares/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find ares_navigation)/config/ares/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find ares_navigation)/config/ares/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find ares_navigation)/config/ares/move_base_params.yaml" command="load" />
    <rosparam file="$(find ares_navigation)/config/ares/dwa_local_planner_params.yaml" command="load" />
    </node>

    <!-- 启动AMCL节点 -->
    <include file="$(find amcl)/examples/amcl_omni.launch" />

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

</launch>

4. 运行该文件
   
   由上图我们可以看到绿色的线是全局规划的路线，蓝色的短线表示局部规划的路线

teb_local_planner（本质是插件）

teb_local_planner则是2D导航堆栈的base_local_planner的插件。实现了一个在线优化的本地轨迹规划器，用于导航和控制移动机器人，作为ROS 导航包的插件。全局规划器生成的初始轨迹在运行时期间进行优化，最小化轨迹执行时间（时间最优目标）， 与障碍物分离并符合动力学约束，例如满足最大速度和加速度。当前的实施符合非完整机器人（差动驱动和类似汽车的机器人）的运动学。自Kinetic以来，包括对完整机器人的支持。

通过求解稀疏的标量化多目标优化问题，可以有效地获得最优轨迹。用户可以为优化问题提供权重，以便在目标冲突的情况下指定行为。

teb_local_planner配置方法
安装步骤

• 源码安装

需要安装teb_local_planner，前往teb_local_planner，将分支切换到melodic-devel然后下载源码到本地，并解压源码包到~/catkin_ws_nav/src/后，就可以编译安装了。

#安装依赖
source ~/catkin_ws_nav/devel/setup.bash 
rosdep install teb_local_planner

cd  ~/catkin_ws_nav/
catkin_make  -DCATKIN_WHITELIST_PACKAGES="teb_local_planner"

teb_local_planner源码中关于plugin的配置文件均已写好，直接编译源码就能完成plugin的注册及插入，非常方便

• 二进制安装方法

1. 打开teminal，输入

sudo apt-get install ros-melodic-teb-local-planner

2. 验证是否安装成功

roscd teb_local_planner

若能进入teb_local_planner的目录，则表明安装成功

• 使用方法
  示例设置
  您可以在teb_local_planner_tutorials软件包中找到使用舞台模拟器的示例设置。

1. 安装teb_local_planner_tutorials软件包

2. 检查参数/配置文件

3. 启动diff_drive设置：

roslaunch teb_local_planner_tutorials robot_diff_drive_in_stage.launch

4. 使用rosrun rqt_reconfigure rqt_reconfigure或修改文件来修改参数。
   使用效果
   
   如上图所示绿色线段表示全局规划路径，红色线段表示teb_local_planner的局部规划路径

eband_local_planner（本质是插件）

eband_local_planner实现了base_local_planner的插件。它在SE2流形上实现了弹性带方法。

此ROS move_base本地规划器的原始实现仅支持全向（完整）机器人。

修改了当前版本以与差分驱动器一起使用。将differential_drive设置为false以启用横向/完整运动，但该模式已经过很长时间的测试，应该被认为是实验性的。

在到达目标位置和目标之前计算速度时，差动驱动机器人可以处于三种动作之一：

1：机器人在目标位置的横向公差（xy_goal_tolerance）内，并且在适当位置旋转以达到目标方向。

2：机器人在目标位置的横向公差之外，则必须朝向目标移动。给定机器人的方向以及弹性带中下一个点的方向，机器人首先计算这两个位置之间的差异。如果该差值小于参数rotation_threshold_multiplier，则机器人执行线性和角速度分量以朝向频带中的下一个点弧。一旦机器人靠近目标位置，使得到目标的x和y距离小于0.6 * xy_goal_tolerance，然后进入步骤1.这里选择0.6，因为机器人在开始之前接近目标而不是公差要求最后一个转弯，因为最后一个转弯可能会导致机器人稍微偏离位置。

 3：如果在前一步骤中计算的差值大于参数rotation_threshold_multiplier，则机器人就位，直到差值变小，并且机器人可以执行动作2。

asr_ftc_local_planner

该软件包提供了 “Follow the Carrot” 算法的实现，以便在平面上进行本地机器人导航。根据全局规划器和成本图，局部规划生成速度命令以发送到移动平台。此规划器的参数也可动态重新配置。此包实现本地计划程序的asr_nav_core接口（而不是nav_core）。为此，必须调整标准的move_base和nav_core（查看kapitel 3.1 Needed Packages）。

“追随胡萝卜”刨床试图尽可能准确地遵循全球计划。根据全球计划，它计算速度命令以达到该计划中的某个点。因为它使用全球计划来绕过计划需要不断更新的障碍。

相位分解，在设定新目标后，规划器将其分解为三个相位：

1.当场旋转到全局规划方向。

2.驶向目标。

3.当场旋转到目标方向。

优点
快速驾驶：机器人尽可能快地达到最大速度。
参数很少：只能设置11个参数。
几个混蛋：机器人试图以最大速度不断开车。
对于小房间：机器人密切关注全局计划，没有循环（如dwa_local_planner）。因此它可以在小房间内无碰撞地行驶。
通过全球计划驱动障碍：保证找到解决障碍的方法。
处理时间短
限制
仅适用于圆形机器人。
只有差动驱动机器人。
仅向前驱动（而不是向后）。

源码安装后不能正常运行

dwb_local_planner

这个本地规划器实现重写并扩展了dwa_local_planner它的功能，因此它在逻辑上被称为dwb_local_planner。目标是通过pluginlib或直接扩展实现类，使尽可能多的部分功能可自定义。

当地规划人员的目标是采用全球计划和本地成本图，并产生命令速度，这可能会使机器人移动到目标。dwa和dwb都通过采样来做到这一点，即生成合理的速度命令并在各种度量上评估它们并选择具有最佳得分的命令，直到机器人达到其目标。

评估的形式至关重要。假设我们正在评估给定的命令，看它是否与costmap中的任何障碍物发生碰撞。关键问题是机器人将使用命令驱动的位置。为此，您需要知道机器人的位置和速度，此外还需要考虑机器人的运动学。为此，我们不是单独评估速度，而是评估不仅包含速度的轨迹，而且包含一些我们预期机器人可以驱动的样本姿势的数组。

导航堆栈仅能够在2.5尺寸导航（即X，Y和θ），因此大多数的接口处理的geometry_msgs/Pose2D和nav_2d_msgs/Twist2D，而不是更普通的Pose和Twist。