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

最近，作者参加了关于RMUS 高校 SimReal在挑战赛中，我第一次接触到机器人导航领域，记录了这段时间的收货情况。sim2real的全称是simulation to reality，它是强化学习的一个分支，也属于transfer learning的一种。解决的主要问题是，在机器人领域，机器人或机械臂在模拟过程中对物理环境存在误差，以及如何将模拟结果应用到实践中。

根据环境信息获取程度，机器人导航的路径规划主要分为全局路径规划和局部路径规划。

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

在一定程度上，动态窗口法采用粒子滤波的理念，在速度空间（v,w）采样多组速度，模拟这些速度在一定时间内的运动轨迹，通过评估函数评估这些轨迹，选择与最佳轨迹对应的速度驱动机器人运动。

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

• 计算简单
• 适用于差异和全向模型

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

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

t i m e e l a s t i c b a n d = t i m e d e l a t i c s b a n d time elastic band = timed elatics band time elasticband=timedelaticsband

其目标函数的定义：

虽然有优化的橡胶筋有很多状态点和时间段，但似乎有很多目标函数。然而，每个目标函数只与橡胶筋中的某些状态有关，而不是整个橡胶筋。将其描述为图片，然后用图片进行优化。

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

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

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

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

m i n C F ( x ( t f ) ) + f t = t 0 t f C R ( x , u ) d t x ˙ = f ( x , u ) g ( x , u ) < 0 h ( x , u ) = 0 minC_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\\ minCF​(x(tf​))+ft=t0​tf​​CR​(x,u)dtx˙=f(x,u)g(x,u)<0h(x,u)=0

m i n C F ( x ( t f ) ) + f t = t 0 t f C R ( x , u ) d t minC_F(x(t_f))+f_{t=t_0}^{t_f}C_R(x, u)dt\\ minCF​(x(tf​))+ft=t0​tf​​CR​(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学习笔记-局部路径规划算法对比

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