ROS中MPC局部路径规划器使用方法及源码流程解读

最新推荐文章于 2024-06-05 10:18:27 发布

古月居GYH

最新推荐文章于 2024-06-05 10:18:27 发布

阅读量1.3k

点赞数 26

文章标签：其他

本文链接：https://blog.csdn.net/weixin_68094467/article/details/137380005

版权

本文主要介绍ROS中Navigation导航框架中MPC局部路径规划器mpc_local_planner的使用方法，并对源码进行解读，梳理其规划流程等，具体包含MPC模型预测控制算法简介、mpc_local_planner使用方法、mpc_local_planner源码解读与规划流程梳理三部分内容。

一、MPC模型预测控制算法简介

MPC的设计和实施包含三个步骤，在k时刻第一步要做的是，估计/测量出系统当前状态。

MPC的优点包括处理多变量、多约束系统，适应动态环境和提供优化性能。然而，它的计算复杂度较高，适用于需要高精度控制的应用。

MPC模型预测控制算法的详细介绍可见：

《MPC模型预测控制器学习笔记》

二、mpc_local_planner使用方法

ROS现有开源MPC模型预测控制算法的局部路径规划器插件中，最受欢迎的是mpc_local_planner功能包，它与我们比较熟悉的teb_local_planner出自同一研究所（多特蒙德大学-控制理论与系统工程研究所），所以在流程及上有很多相似之处，mpc_local_planner使用步骤如下：

1、下载并将mpc_local_planner功能包放到ROS工作空间的src文件夹下

2、环境配置，在终端执行以下指令安装所需依赖和环境

<span style="background-color:#f5f2f0"><span style="color:#000000"><span style="color:black"><span style="color:blue"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:blue"><span style="color:black"><span style="color:black"><span style="color:black"><code class="language-cpp">rosdep install mpc_local_planner
</code></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span>

若rosdep不能正常使用，可以参考我之前写的博客，链接如下：

《ROS Noetic版本 rosdep找不到命令不能使用的解决方法》

3、使用catkin_make对mpc_local_planner功能包进行编译

4、可根据需要执行以下语句中的一个或多个（同一时间执行一个即可），来使用功能包自带的示例，对功能包是否能够正常工作，并可对其性能进行测试

可以执行以下语句进行阿克曼模型小车的demo的测试

<span style="background-color:#f5f2f0"><span style="color:#000000"><span style="color:black"><span style="color:blue"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:blue"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><code class="language-cpp">roslaunch mpc_local_planner_examples carlike_minimum_time<span style="color:#999999">.</span>launch
</code></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span>

动态效果演示如下：

注：↓↓↓↓↓点击以下图片可查看动态演示图↓↓↓↓↓

可以执行以下三条语句中任意一条进行差速模型小车的demo的测试

<span style="background-color:#f5f2f0"><span style="color:#000000"><span style="color:black"><span style="color:blue"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:blue"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:green"><span style="color:green"><span style="color:black"><code class="language-cpp">roslaunch mpc_local_planner_examples diff_drive_minimum_time<span style="color:#999999">.</span>launch
</code></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span>

<span style="background-color:#f5f2f0"><span style="color:#000000"><span style="color:black"><span style="color:blue"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:blue"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:green"><span style="color:green"><span style="color:black"><code class="language-cpp">roslaunch mpc_local_planner_examples diff_drive_quadratic_form<span style="color:#999999">.</span>launch
</code></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span>

<span style="background-color:#f5f2f0"><span style="color:#000000"><span style="color:black"><span style="color:blue"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:blue"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:green"><span style="color:green"><span style="color:black"><code class="language-cpp">roslaunch mpc_local_planner_examples diff_drive_minimum_time_costmap_conversion<span style="color:#999999">.</span>launch
</code></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span>

动态效果演示如下：

注：↓↓↓↓↓点击以下图片可查看动态演示图↓↓↓↓↓
-

执行任意上述launch文件对功能包是否能够正常工作进行测试后，若能正常进行局部路径规划，则可以进一步将该功能包运用在我们自己的机器人上，继续进行以下操作

5、在启动move_base的launch文件中，配置局部路径规划器插件为mpc_local_planner/MpcLocalPlannerROS，并根据机器人的实际情况，设定参数clearing_rotation_allowed的值来设定在规划时是否允许机器人旋转，比如对于阿克曼结构的类车型机器人，不具备原地旋转能力，该参数设为flase，如下所示

<span style="background-color:#f5f2f0"><span style="color:#000000"><span style="color:black"><span style="color:blue"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:blue"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:green"><span style="color:green"><span style="color:black"><span style="color:black"><span style="color:green"><span style="color:black"><span style="color:black"><code class="language-cpp"> <span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a"><</span></span>param name<span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">=</span></span><span style="color:#669900">"base_local_planner"</span> value<span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">=</span></span><span style="color:#669900">"mpc_local_planner/MpcLocalPlannerROS"</span> <span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">/</span></span><span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">></span></span>
 <span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a"><</span></span>param name<span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">=</span></span><span style="color:#669900">"controller_frequency"</span> value<span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">=</span></span><span style="color:#669900">"5"</span> <span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">/</span></span><span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">></span></span>
 <span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a"><</span></span>param name<span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">=</span></span><span style="color:#669900">"controller_patiente"</span> value<span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">=</span></span><span style="color:#669900">"15.0"</span><span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">/</span></span><span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">></span></span>
 <span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a"><</span></span>param name<span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">=</span></span><span style="color:#669900">"clearing_rotation_allowed"</span> value<span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">=</span></span><span style="color:#669900">"false"</span> <span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">/</span></span><span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">></span></span> <span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a"><</span></span><span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">!</span></span><span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">--</span></span> carlike robot is <span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">not</span></span> able to rotate in place <span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">--</span></span><span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">></span></span>
</code></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span>

6、在上述move_base节点配置中调用mpc_local_planner的参数配置文件mpc_local_planner_params.yaml，如下图所示，需要注意的是文件路径可能要根据实际情况进行修改。

7、进行效果测试，并根据测试效果对参数进行调节

注：↓↓↓↓↓点击以下图片可查看动态演示图↓↓↓↓↓

三、mpc_local_planner源码解读与规划流程梳理

在我之前的文章《ROS导航包Navigation中的 Movebase节点路径规划相关流程梳理》中已经介绍过Move_base节点调用局部路径规划器插件的接口函数是computeVelocityCommands，接下来，我们就从这个函数入手梳理一下mpc_local_planner功能包的工作流程。

文章链接：

《ROS导航包Navigation中的 Movebase节点路径规划相关流程梳理》

1、computeVelocityCommands函数

从局部路径规划器的接口函数computeVelocityCommands开始：

<span style="background-color:#f5f2f0"><span style="color:#000000"><span style="color:black"><span style="color:blue"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:blue"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:green"><span style="color:green"><span style="color:black"><span style="color:black"><span style="color:green"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:green"><span style="color:blue"><span style="color:green"><span style="color:black"><span style="color:black"><span style="color:blue"><code class="language-cpp"><span style="color:#0077aa">uint32_t</span> <span style="color:#dd4a68">MpcLocalPlannerROS</span><span style="color:#999999">::</span><span style="color:#dd4a68">computeVelocityCommands</span><span style="color:#999999">(</span><span style="color:#0077aa">const</span> geometry_msgs<span style="color:#999999">::</span>PoseStamped<span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">&</span></span> pose<span style="color:#999999">,</span> <span style="color:#0077aa">const</span> geometry_msgs<span style="color:#999999">::</span>TwistStamped<span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">&</span></span> velocity<span style="color:#999999">,</span>geometry_msgs<span style="color:#999999">::</span>TwistStamped<span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">&</span></span> cmd_vel<span style="color:#999999">,</span> std<span style="color:#999999">::</span>string<span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">&</span></span> message<span style="color:#999999">)</span>
</code></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span>

（1）该函数中首先检查了该路径规划器是否初始化、并定义了一些变量等，然后读取了机器人当前的位姿和速度信息。

（2）调用pruneGlobalPlan函数，对全局路径进行修剪

由于机器人是实时运动的，所以需要对已经走过的全局路径裁减处理，其实现思路是从全局路径规划容器的起点开始一个个遍历，计算其与机器人当前点的距离差，直到找到距离小于我们设定的阈值的点，将该点之前的全局路径点全部删除，这样就可以将位于机器人后方距离超过一定阈值的点删除掉。该过程涉及到参数配置文件中的参数 global_plan_prune_distance

<span style="background-color:#f5f2f0"><span style="color:#000000"><span style="color:black"><span style="color:blue"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:blue"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:green"><span style="color:green"><span style="color:black"><span style="color:black"><span style="color:green"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:green"><span style="color:blue"><span style="color:green"><span style="color:black"><span style="color:black"><span style="color:blue"><span style="color:purple"><span style="color:purple"><span style="color:black"><span style="color:#FF7F00"><span style="color:purple"><code class="language-cpp"> <span style="color:#dd4a68">pruneGlobalPlan</span><span style="color:#999999">(</span><span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">*</span></span>_tf<span style="color:#999999">,</span> robot_pose<span style="color:#999999">,</span> _global_plan<span style="color:#999999">,</span> _params<span style="color:#999999">.</span>global_plan_prune_distance<span style="color:#999999">)</span><span style="color:#999999">;</span>
</code></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span>

（3）将全局路径转换到局部代价地图的坐标系下，并从中选择合适的点作为局部路径点

调用transformGlobalPlan函数，通过tf 树将全局路径信息转换到局部代价地图的坐标系下，值得注意的是，在这个函数中并不仅仅进行了坐标系的变换，还对全局路径信息点进行了一些处理，从中选出合适的点作为局部路径规划点。其具体过程是依次对全局路径进行坐标变换，同时计算其与当前位置的距离，并统计已转换的全局路径点之间的距离和（即已转换的全局路径的长度），直到超过设定的长度阈值或者与当前点的距离大于设定的距离阈值，则丢弃剩余的路径点。其中，长度阈值由配置文件中参数max_global_plan_lookahead_dist 决定，距离阈值由局部代价地图的大小width和height以及系数0.85有关

<span style="background-color:#f5f2f0"><span style="color:#000000"><span style="color:black"><span style="color:blue"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:blue"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:green"><span style="color:green"><span style="color:black"><span style="color:black"><span style="color:green"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:green"><span style="color:blue"><span style="color:green"><span style="color:black"><span style="color:black"><span style="color:blue"><span style="color:purple"><span style="color:purple"><span style="color:black"><span style="color:#FF7F00"><span style="color:purple"><span style="color:purple"><span style="color:black"><span style="color:#FF7F00"><span style="color:black"><code class="language-cpp"><span style="color:#0077aa">if</span> <span style="color:#999999">(</span><span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">!</span></span><span style="color:#dd4a68">transformGlobalPlan</span><span style="color:#999999">(</span><span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">*</span></span>_tf<span style="color:#999999">,</span> _global_plan<span style="color:#999999">,</span> robot_pose<span style="color:#999999">,</span> <span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">*</span></span>_costmap<span style="color:#999999">,</span> _global_frame<span style="color:#999999">,</span> _params<span style="color:#999999">.</span>max_global_plan_lookahead_dist<span style="color:#999999">,</span> transformed_plan<span style="color:#999999">,</span> <span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">&</span></span>goal_idx<span style="color:#999999">,</span> <span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">&</span></span>tf_plan_to_global<span style="color:#999999">)</span><span style="color:#999999">)</span>
<span style="color:#999999">{</span>
  <span style="color:#dd4a68">ROS_WARN</span><span style="color:#999999">(</span><span style="color:#669900">"Could not transform the global plan to the frame of the controller"</span><span style="color:#999999">)</span><span style="color:#999999">;</span>
  message <span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">=</span></span> <span style="color:#669900">"Could not transform the global plan to the frame of the controller"</span><span style="color:#999999">;</span>
  <span style="color:#0077aa">return</span> mbf_msgs<span style="color:#999999">::</span>ExePathResult<span style="color:#999999">::</span>INTERNAL_ERROR<span style="color:#999999">;</span>
<span style="color:#999999">}</span>
</code></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span>

（4）对得到的局部路径规划点进行进一步的筛选

调用updateViaPointsContainer函数，对上一步得到的局部路径规划点进行进一步的筛选，其实现思路是，循环遍历局部路径规划点，从中选出距离上一个被选中的点的距离大于设定的阈值的点（局部路径规划点中的第一个点为本步筛选中第一个被选中的点）

<span style="background-color:#f5f2f0"><span style="color:#000000"><span style="color:black"><span style="color:blue"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:blue"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:green"><span style="color:green"><span style="color:black"><span style="color:black"><span style="color:green"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:green"><span style="color:blue"><span style="color:green"><span style="color:black"><span style="color:black"><span style="color:blue"><span style="color:purple"><span style="color:purple"><span style="color:black"><span style="color:#FF7F00"><span style="color:purple"><span style="color:purple"><span style="color:black"><span style="color:#FF7F00"><span style="color:black"><span style="color:purple"><span style="color:black"><code class="language-cpp"><span style="color:#0077aa">if</span> <span style="color:#999999">(</span><span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">!</span></span>_custom_via_points_active<span style="color:#999999">)</span> <span style="color:#dd4a68">updateViaPointsContainer</span><span style="color:#999999">(</span>transformed_plan<span style="color:#999999">,</span>_params<span style="color:#999999">.</span>global_plan_viapoint_sep<span style="color:#999999">)</span><span style="color:#999999">;</span>
</code></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span>

这样处理可以将上一步得到的局部路径点稀疏化，从而减少局部路径点的个数，阈值由配置文件中的参数global_plan_viapoint_sep 决定，若该参数设置为负数，则表示跳过本步处理，不进行筛选

第（2）~（4）步的动态演示如下（与TEB的处理过程相同）：

全局路径处理过程动态演示【点击可跳转】

（5）判断机器人是否到达目标点

得到局部路径的状态点后，先检查是否已经到达了全局目标点，具体要检查以下内容：

（1）当前点距离目标点的距离是否小于由参数配置文件设定的阈值xy_goal_tolerance

（2）当前点的姿态角与目标姿态角是否小于由外参数配置文件设定的阈值yaw_goal_tolerance

<span style="background-color:#f5f2f0"><span style="color:#000000"><span style="color:black"><span style="color:blue"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:blue"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:green"><span style="color:green"><span style="color:black"><span style="color:black"><span style="color:green"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:green"><span style="color:blue"><span style="color:green"><span style="color:black"><span style="color:black"><span style="color:blue"><span style="color:purple"><span style="color:purple"><span style="color:black"><span style="color:#FF7F00"><span style="color:purple"><span style="color:purple"><span style="color:black"><span style="color:#FF7F00"><span style="color:black"><span style="color:purple"><span style="color:black"><span style="color:black"><span style="color:#FF7F00"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:purple"><span style="color:black"><span style="color:black"><span style="color:#FF7F00"><span style="color:black"><span style="color:black"><span style="color:#FF7F00"><span style="color:black"><code class="language-cpp">    <span style="color:#0077aa">if</span> <span style="color:#999999">(</span>std<span style="color:#999999">::</span><span style="color:#dd4a68">abs</span><span style="color:#999999">(</span>std<span style="color:#999999">::</span><span style="color:#dd4a68">sqrt</span><span style="color:#999999">(</span>dx <span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">*</span></span> dx <span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">+</span></span> dy <span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">*</span></span> dy<span style="color:#999999">)</span><span style="color:#999999">)</span> <span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a"><</span></span> _params<span style="color:#999999">.</span>xy_goal_tolerance <span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">&&</span></span> std<span style="color:#999999">::</span><span style="color:#dd4a68">abs</span><span style="color:#999999">(</span>delta_orient<span style="color:#999999">)</span> <span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a"><</span></span> _params<span style="color:#999999">.</span>yaw_goal_tolerance<span style="color:#999999">)</span>
    <span style="color:#999999">{</span>
        _goal_reached <span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">=</span></span> <span style="color:#990055">true</span><span style="color:#999999">;</span>
        <span style="color:#0077aa">return</span> mbf_msgs<span style="color:#999999">::</span>ExePathResult<span style="color:#999999">::</span>SUCCESS<span style="color:#999999">;</span>
    <span style="color:#999999">}</span>
</code></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span>

（6）调整局部目标点的角度

根据外部参数 global_plan_overwrite_orientation决定是否调整局部目标点的角度（也就是经过上述处理后的路径的最后一个点的角度），调整是通过调用estimateLocalGoalOrientation函数来实现的

<span style="background-color:#f5f2f0"><span style="color:#000000"><span style="color:black"><span style="color:blue"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:blue"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:green"><span style="color:green"><span style="color:black"><span style="color:black"><span style="color:green"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:green"><span style="color:blue"><span style="color:green"><span style="color:black"><span style="color:black"><span style="color:blue"><span style="color:purple"><span style="color:purple"><span style="color:black"><span style="color:#FF7F00"><span style="color:purple"><span style="color:purple"><span style="color:black"><span style="color:#FF7F00"><span style="color:black"><span style="color:purple"><span style="color:black"><span style="color:black"><span style="color:#FF7F00"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:purple"><span style="color:black"><span style="color:black"><span style="color:#FF7F00"><span style="color:black"><span style="color:black"><span style="color:#FF7F00"><span style="color:black"><span style="color:purple"><span style="color:black"><span style="color:#FF7F00"><span style="color:black"><code class="language-cpp">    <span style="color:#0077aa">if</span> <span style="color:#999999">(</span>_params<span style="color:#999999">.</span>global_plan_overwrite_orientation<span style="color:#999999">)</span>
    <span style="color:#999999">{</span>
        _robot_goal<span style="color:#999999">.</span><span style="color:#dd4a68">theta</span><span style="color:#999999">(</span><span style="color:#999999">)</span> <span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">=</span></span> <span style="color:#dd4a68">estimateLocalGoalOrientation</span><span style="color:#999999">(</span>_global_plan<span style="color:#999999">,</span> transformed_plan<span style="color:#999999">.</span><span style="color:#dd4a68">back</span><span style="color:#999999">(</span><span style="color:#999999">)</span><span style="color:#999999">,</span> goal_idx<span style="color:#999999">,</span> tf_plan_to_global<span style="color:#999999">)</span><span style="color:#999999">;</span>
        <span style="color:#708090">// overwrite/update goal orientation of the transformed plan with the actual goal (enable using the plan as initialization)</span>
        tf2<span style="color:#999999">::</span>Quaternion q<span style="color:#999999">;</span>
        q<span style="color:#999999">.</span><span style="color:#dd4a68">setRPY</span><span style="color:#999999">(</span><span style="color:#990055">0</span><span style="color:#999999">,</span> <span style="color:#990055">0</span><span style="color:#999999">,</span> _robot_goal<span style="color:#999999">.</span><span style="color:#dd4a68">theta</span><span style="color:#999999">(</span><span style="color:#999999">)</span><span style="color:#999999">)</span><span style="color:#999999">;</span>
        tf2<span style="color:#999999">::</span><span style="color:#dd4a68">convert</span><span style="color:#999999">(</span>q<span style="color:#999999">,</span> transformed_plan<span style="color:#999999">.</span><span style="color:#dd4a68">back</span><span style="color:#999999">(</span><span style="color:#999999">)</span><span style="color:#999999">.</span>pose<span style="color:#999999">.</span>orientation<span style="color:#999999">)</span><span style="color:#999999">;</span>
    <span style="color:#999999">}</span>
    <span style="color:#0077aa">else</span>
    <span style="color:#999999">{</span>
        _robot_goal<span style="color:#999999">.</span><span style="color:#dd4a68">theta</span><span style="color:#999999">(</span><span style="color:#999999">)</span> <span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">=</span></span> tf2<span style="color:#999999">::</span><span style="color:#dd4a68">getYaw</span><span style="color:#999999">(</span>transformed_plan<span style="color:#999999">.</span><span style="color:#dd4a68">back</span><span style="color:#999999">(</span><span style="color:#999999">)</span><span style="color:#999999">.</span>pose<span style="color:#999999">.</span>orientation<span style="color:#999999">)</span><span style="color:#999999">;</span>
    <span style="color:#999999">}</span>
</code></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span>

当需要修改时，会判断当前局部路径规划的终点与全局路径规划的终点是否接近，如果两个点比较接近(就是同一个点或者下一个点就是终点)则直接使用当前局部路径规划中终点的方向作为规划的方向。如果当前局部路径规划的终点跟全局目标点之间还间隔很多个点的话，会使用当前局部路径的终点以及向后两个点的角度进行平滑处理，得到一个新的角度值。

当不需要修改时，采用局部目标点的原有角度。

（7）更新局部路径的起点

上一步完成了局部规划的目标点的确认，这一步要完成局部规划的起始点的确认，，前面虽然根据全局路径规划中截取了局部路径规划的点，但是这个容器的起点不一定是是机器人当前时刻所处的点。

<span style="background-color:#f5f2f0"><span style="color:#000000"><span style="color:black"><span style="color:blue"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:blue"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:green"><span style="color:green"><span style="color:black"><span style="color:black"><span style="color:green"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:green"><span style="color:blue"><span style="color:green"><span style="color:black"><span style="color:black"><span style="color:blue"><span style="color:purple"><span style="color:purple"><span style="color:black"><span style="color:#FF7F00"><span style="color:purple"><span style="color:purple"><span style="color:black"><span style="color:#FF7F00"><span style="color:black"><span style="color:purple"><span style="color:black"><span style="color:black"><span style="color:#FF7F00"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:purple"><span style="color:black"><span style="color:black"><span style="color:#FF7F00"><span style="color:black"><span style="color:black"><span style="color:#FF7F00"><span style="color:black"><span style="color:purple"><span style="color:black"><span style="color:#FF7F00"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:purple"><span style="color:black"><code class="language-cpp">    <span style="color:#0077aa">if</span> <span style="color:#999999">(</span>transformed_plan<span style="color:#999999">.</span><span style="color:#dd4a68">size</span><span style="color:#999999">(</span><span style="color:#999999">)</span> <span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">==</span></span> <span style="color:#990055">1</span><span style="color:#999999">)</span>  <span style="color:#708090">// plan only contains the goal</span>
    <span style="color:#999999">{</span>
        transformed_plan<span style="color:#999999">.</span><span style="color:#dd4a68">insert</span><span style="color:#999999">(</span>transformed_plan<span style="color:#999999">.</span><span style="color:#dd4a68">begin</span><span style="color:#999999">(</span><span style="color:#999999">)</span><span style="color:#999999">,</span> geometry_msgs<span style="color:#999999">::</span><span style="color:#dd4a68">PoseStamped</span><span style="color:#999999">(</span><span style="color:#999999">)</span><span style="color:#999999">)</span><span style="color:#999999">;</span>  <span style="color:#708090">// insert start (not yet initialized)</span>
    <span style="color:#999999">}</span>
    transformed_plan<span style="color:#999999">.</span><span style="color:#dd4a68">front</span><span style="color:#999999">(</span><span style="color:#999999">)</span> <span style="background-color:rgba(255, 255, 255, 0.5)"><span style="color:#9a6e3a">=</span></span> robot_pose<span style="color:#999999">;</span>  <span style="color:#708090">// update start</span>
</code></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span>

如果路径中只包含一个目标点，即没有中间的路径点，程序会在路径的起始点处插入机器人当前的姿态。这是为了将路径用作初始化轨迹，以确保路径规划从机器人当前位置开始。如果路径包含多个点，则使用机器人当前位姿点覆盖更新容器中的第一个路径点。

（8）更新障碍物信息

根据配置文件中参数 costmap_converter决定如何更新障碍物信息，若该参数不为空，则调用costmap_converter插件进行地图信息更新，若为空直接使用costmap的信息进行更新。

分别对应下面那个函数：

<span style="background-color:#f5f2f0"><span style="color:#000000"><span style="color:black"><span style="color:blue"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:blue"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:green"><span style="color:green"><span style="color:black"><span style="color:black"><span style="color:green"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:green"><span style="color:blue"><span style="color:green"><span style="color:black"><span style="color:black"><span style="color:blue"><span style="color:purple"><span style="color:purple"><span style="color:black"><span style="color:#FF7F00"><span style="color:purple"><span style="color:purple"><span style="color:black"><span style="color:#FF7F00"><span style="color:black"><span style="color:purple"><span style="color:black"><span style="color:black"><span style="color:#FF7F00"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:purple"><span style="color:black"><span style="color:black"><span style="color:#FF7F00"><span style="color:black"><span style="color:black"><span style="color:#FF7F00"><span style="color:black"><span style="color:purple"><span style="color:black"><span style="color:#FF7F00"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:purple"><span style="color:black"><span style="color:black"><span style="color:purple"><span style="color:black"><span style="color:#FF7F00"><span style="color:black"><span style="color:black"><code class="language-cpp"><span style="color:#dd4a68">updateObstacleContainerWithCostmapConverter</span><span style="color:#999999">(</span><span style="color:#999999">)</span>
</code></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span>

<span style="background-color:#f5f2f0"><span style="color:#000000"><span style="color:black"><span style="color:blue"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:blue"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:green"><span style="color:green"><span style="color:black"><span style="color:black"><span style="color:green"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:green"><span style="color:blue"><span style="color:green"><span style="color:black"><span style="color:black"><span style="color:blue"><span style="color:purple"><span style="color:purple"><span style="color:black"><span style="color:#FF7F00"><span style="color:purple"><span style="color:purple"><span style="color:black"><span style="color:#FF7F00"><span style="color:black"><span style="color:purple"><span style="color:black"><span style="color:black"><span style="color:#FF7F00"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:purple"><span style="color:black"><span style="color:black"><span style="color:#FF7F00"><span style="color:black"><span style="color:black"><span style="color:#FF7F00"><span style="color:black"><span style="color:purple"><span style="color:black"><span style="color:#FF7F00"><span style="color:black"><span style="color:black"><span style="color:black"><span style="color:purple"><span style="color:black"><span style="color:black"><span style="color:purple"><span style="color:black"><span style="color:#FF7F00"><span style="color:black"><span style="color:black"><code class="language-cpp"><span style="color:#dd4a68">updateObstacleContainerWithCostmap</span><span style="color:#999999">(</span><span style="color:#999999">)</span></code></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span></span>

点击 ROS中MPC局部路径规划器使用方法及源码流程解读 - 古月居可查看全文

古月居GYH

关注

26
点赞
踩
41

收藏

觉得还不错? 一键收藏
0
评论
ROS中MPC局部路径规划器使用方法及源码流程解读

其具体过程是依次对全局路径进行坐标变换，同时计算其与当前位置的距离，并统计已转换的全局路径点之间的距离和（即已转换的全局路径的长度），直到超过设定的长度阈值或者与当前点的距离大于设定的距离阈值，则丢弃剩余的路径点。由于机器人是实时运动的，所以需要对已经走过的全局路径裁减处理，其实现思路是从全局路径规划容器的起点开始一个个遍历，计算其与机器人当前点的距离差，直到找到距离小于我们设定的阈值的点，将该点之前的全局路径点全部删除，这样就可以将位于机器人后方距离超过一定阈值的点删除掉。
复制链接

扫一扫