【硬核】 ROS Navigation 局部路径规划常见算法

简介

​ 最近，作者参加了关于RMUS 高校 SimReal挑战赛，首次接触到了机器人导航领域，这里记录一下这段时间的收货。sim2real的全称是simulation to reality，是强化学习的一个分支，同时也属于transfer learning的一种。主要解决的问题是机器人领域中，直接让机器人或者机械臂在仿真中对于物理环境存在误差，如何将仿真上取得的成果应用到实际中的问题。

​ 机器人导航的路径规划问题主要分为全局路径规划和局部路径规划，这两者是根据对环境信息获取程度划分的。

• 全局规划通常需要在已知环境中进行，属于一种事前规划，可以找到最优解，一旦环境发生变化，未及时更新地图时，该方法就不能达到预期效果。
• 局部路径规划通常用在未知或部分已知的环境中，系统根据传感器实时获取到环境障碍物的信息，并做出相应规划，这对系统的实时计算处理能力有着较高的要求。由于缺乏全局环境信息，结果很可能不是最优的。

​ 全局路径规划和局部路径规划并没有本质的区别，很多适用于全局路径规划的方法经过改进也可以用于局部路径规划，而适用于局部路径规划的方法同样经过改进后也可适用于全局路径规划。两者协同工作，机器人可更好的规划从起始点到终点的行走路径。

​ 本文主要针对局部路径规划作介绍。

局部路径规划

​ 机器人在获得目的地信息后，首先经过全局路径规划规划出一条大致可行的路线，然后调用局部路径规划器根据这条路线及costmap的信息规划出机器人在局部时做出具体行动策略。常用的局部路径规划算法有动态窗口法(DWA)、时间弹性带(TEB)和模型预测控制(MPC)。

动态窗口法(DWA)

​ 动态窗口法在一定程度上采用了粒子滤波的思想，在速度空间（v,w）中采样多组速度，并模拟出这些速度在一定时间内的运动轨迹，并通过评价函数对这些轨迹进行评价，选取最优轨迹对应的速度驱动机器人运动。

基本思想

1. 在机器人的控制空间中离散采样 (dx,dy,dtheta)
2. 对于每个采样速度，从机器人的当前状态执行前向模拟，以预测如果采样速度应用于某个（短）时间段会发生什么。
3. 使用包含以下特征的度量来评估前向模拟产生的每个轨迹：与障碍物的距离、与目标的距离、与全局路径的距离和速度，排除非法轨迹（与障碍物相撞的轨迹）。
4. 选择得分最高的轨迹并将相关的速度发送到移动基地。
5. 重复执行上述步骤。

优点

• 计算简单
• 适用于差分和全向车模

缺点

• 前瞻性不足
• 动态效果差
• 不适用于阿克曼模型车模

时间弹性带(TEB)

​ 时间弹性带(TEB)简而言之，就是连接起始、目标点，并让这个路径可以变形，变形的条件就是将所有约束当做橡皮筋的外力。起始点、目标点状态由用户/全局规划器指定，中间插入N个控制橡皮筋形状的控制点（机器人姿态），当然，为了显示轨迹的运动学信息，我们在点与点之间定义运动时间Time。

$$time+elasticband=timedelaticsband$$

其目标函数的定义：

​ 虽然待优化的橡皮筋有不少状态点与时间段，目标函数也好像很多。但是，每个目标函数只与橡皮筋中的某几个状态有关，而非整条橡皮筋。将它描述成图，然后用图优化。

优点

• 前瞻性好
• 适用于各种车模

缺点

• 计算复杂
• 速度和角速度波动大，控制不稳定

模型预测控制(MPC)

​ MPC其实是一种基于对受控对象进行预测的控制方法。MPC最大的特点在于，相对于LQR控制而言，MPC可以考虑空间状态变量的各种约束，而LQR，PID等控制只能够考虑输入输出变量的各种约束。

​ MPC的作用机理可以表述为：在每一个采样时刻，根据当前的测量信息，在线求解一个有限时间开环优化问题，并将得到的控制序列的第一个元素用于被控对象；在下一个采样时刻，用新的测量值作为此时预测系统未来动态的初试条件，刷新优化问题求解。应用于机器人的典型的模型预测控制方法：

问题模型

$$\begin{gathered} min{C}_F(x(t_f))+f_{t=t_0}^{t_f}{C}_R(x,u)dt \\ \dot{x}=f(x,u) \\ g(x,u)<0 \\ h(x,u)=0 \end{gathered}$$

参数空间

$$min{C}_F(x(t_f))+f_{t=t_0}^{t_f}{C}_R(x,u)dt$$

控制

传统MPC控制框图为

1. 设置优化问题
2. 使用测量模块告诉我们当前的initial state
3. 求解优化问题得到参数，这些参数构成系统的最优输入 u*
4. 使用 u*驱动系统，由于系统受到干扰无法保证求解得到的 u*我们想要的，仅此旨在很小一段时间中使用，然后利用观测的状态重新求解问题，转回步骤(2)

局部规划器的应用

​ 这里以mpc_local_planner为例，首先，在本地终端中键入

sudo apt install ros-melodic-mpc-local-planner

​ 由于apt安装有可能导致我们运行失败，上面的指令只是用来安装依赖。同时，克隆远程仓库mpc_local_planner到ros的工作空间中，执行catkin_make编译。对应ros navigation中的move_base.launch，修改其中的局部路径规划器类型

<?xml version="1.0"?>
<launch>

  <!-- 运行move_base节点  -->
  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
    
    <!--move_base参数配置http://wiki.ros.org/move_base -->
    <param name="base_global_planner" value="global_planner/GlobalPlanner" /><!-- 选择全局规划器类型 -->
    <rosparam file="$(find mpc_navigation)/config/base_global_planner_params.yaml" command="load" />
    
    <rosparam file="$(find mpc_navigation)/config/mpc_local_planner_params_minimum_time.yaml" command="load" />
    <param name="base_local_planner" value="mpc_local_planner/MpcLocalPlannerROS" />

    <rosparam file="$(find mpc_navigation)/config/move_base_params.yaml" command="load" /><!-- 其它参数 -->

    <param name="controller_frequency" value="1" /><!-- 选择全局规划器类型 -->

    <!-- 全局代价地图和局部代价地图参数配置http://wiki.ros.org/costmap_2d -->
    <rosparam file="$(find mpc_navigation)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find mpc_navigation)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find mpc_navigation)/config/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find mpc_navigation)/config/global_costmap_params.yaml" command="load" />
    
  </node>

</launch>

​ 对应mpc_local_planner的参数文件可以在克隆仓库中的mpc_local_planner_examples中获得，将cfg/diff_drive中的参数yaml文件复制到对应的config文件夹中即可，base_ local_planner参数设置为mpc_local_planner中定义的类型MpcLocalPlannerROS。

​ mpc_local_planner提供了两个参数文件，一个是以最短时间为目的，另一个是二次规划模型，可以根据需求自行选择。

​ 其他的局部规划器同样是修改base_local_planner的类型和提供相应的功能包以及yaml参数文件实现。

参考

ros-dwa_local_planner

ros-teb_local_planner

ros-mpc_local_planner

ROS学习笔记-局部路径规划算法对比

后续

 喜欢的话可以关注一下我的公众号技术开发小圈，尤其是对深度学习以及计算机视觉有兴趣的朋友，我会把相关的源码以及更多资料发在上面，希望可以帮助到新入门的大家！