ROS中MPC局部路径规划器使用方法及源码流程解读
本文主要介绍ROS中Navigation导航框架中MPC局部路径规划器mpc_local_planner的使用方法，并对源码进行解读，梳理其规划流程等，具体包含MPC模型预测控制算法简介、mpc_local_planner使用方法、mpc_local_planner源码解读与规划流程梳理三部分内容。
注：本文首发于古月居，原文链接如下：
https://www.guyuehome.com/45097
本篇文章我首发在古月居，因版权原因，在CSDN不能放全文，只能放一小部分，欢迎大家前往古月居查看完整文章！！！，链接如上↑↑↑
一、MPC模型预测控制算法简介
MPC（Model Predictive Control）是一种先进的控制策略，其核心思想是使用系统数学模型来预测系统在某一未来时间段内的表现来进行优化控制，并通过求解优化问题来计算最优的控制输入，多用于数位控制，所以分析时常采用离散型的状态空间表达形式 X k + 1 = A X k + B U k X_{k+1}=AX_{k}+BU_{k} Xk+1=AXk+BUk
MPC的设计和实施包含三个步骤，在k时刻第一步要做的是，估计/测量出系统当前状态。
第二步是基于 u k u_{k} uk、 u k + 1 u_{k+1} uk+1、 u k + 2 u_{k+2} uk+2、 u k + 3 u_{k+3} uk+3 … u k + n u_{k+n} uk+n来做最优化控制，即在k时刻施加一个输入 u k u_{k} uk，根据我们的模型来预测系统k ~ k+1时间段内的表现，然后基于预测的k+1时刻的表现，再施加一个输入 u k + 1 u_{k+1} uk+1、再根据我们的模型来预测系统k+1 ~ k+2时间段内的表现，以此类推，直至预测完我们设定的预测区间，比如k~k+3时间段。则输入 u k u_{k} uk、 u k + 1 u_{k+1} uk+1、 u k + 2 u_{k+2} uk+2称为控制区间，在这个控制区间， u k u_{k} uk、 u k + 1 u_{k+1} uk+1、 u k + 2 u_{k+2} uk+2的选择就是一个最优化的问题。来使得设定的目标函数最小。
第三步是非常重要的一步，也是MPC当中非常有技巧性的一步，通过上面的计算。我们其实得到了 u k u_{k} uk、 u k + 1 u_{k+1} uk+1、 u k + 2 u_{k+2} uk+2这三个输入，但是在实施的时候，并不是把这三个数值输入都实施下去，而是在K时刻只实施 u k u_{k} uk这一个在k时刻计算出来的最优化结果。
在实施的时候只取 u k u_{k} uk这一个输入非常重要，这是因为我们预测的模型很难完美的描述现实的系统，而且，在现实的系统当中可能会有扰动，就是说我们预测出来的实施 u k u_{k} uk后，它的输出 Y k Y_{k} Yk与现实当中真正执行完 u k u_{k} uk后的 Y k Y_{k} Yk是有偏差的，这就要求当我们实施 u k u_{k} uk之后，在k+1时刻要把上面的三步重新执行一遍，即把整个的预测区间(control horizon)和控制区间(predictive horizon)向右移动一个时刻。这个过程就称为滚动优化控制(receiving horizon control)。
MPC的优点包括处理多变量、多约束系统，适应动态环境和提供优化性能。然而，它的计算复杂度较高，适用于需要高精度控制的应用。
MPC模型预测控制算法的详细介绍可见 ：《MPC模型预测控制器学习笔记》
二、mpc_local_planner使用方法
ROS现有开源MPC模型预测控制算法的局部路径规划器插件中，最受欢迎的是mpc_local_planner功能包，它与我们比较熟悉的teb_local_planner出自同一研究所（多特蒙德大学-控制理论与系统工程研究所），所以在流程及上有很多相似之处，mpc_local_planner使用步骤如下：
1、下载并将mpc_local_planner功能包放到ROS工作空间的src文件夹下
2、环境配置，在终端执行以下指令安装所需依赖和环境
rosdep install mpc_local_planner
若rosdep不能正常使用，可以参考我之前写的博客，链接如下：
《ROS Noetic版本 rosdep找不到命令 不能使用的解决方法》
3、使用catkin_make对mpc_local_planner功能包进行编译
4、可根据需要执行以下语句中的一个或多个（同一时间执行一个即可），来使用功能包自带的示例，对功能包是否能够正常工作，并可对其性能进行测试
可以执行以下语句进行阿克曼模型小车的demo的测试
roslaunch mpc_local_planner_examples carlike_minimum_time.launch
效果演示如下：
可以执行以下三条语句中任意一条进行差速模型小车的demo的测试
roslaunch mpc_local_planner_examples diff_drive_minimum_time.launch
roslaunch mpc_local_planner_examples diff_drive_quadratic_form.launch
roslaunch mpc_local_planner_examples diff_drive_minimum_time_costmap_conversion.launch
效果演示如下：
执行任意上述launch文件对功能包是否能够正常工作进行测试后，若能正常进行局部路径规划，则可以进一步将该功能包运用在我们自己的机器人上，继续进行以下操作
5、在启动move_base的launch文件中，配置局部路径规划器插件为mpc_local_planner/MpcLocalPlannerROS，并根据机器人的实际情况，设定参数clearing_rotation_allowed的值来设定在规划时是否允许机器人旋转，比如对于阿克曼结构的类车型机器人，不具备原地旋转能力，该参数设为flase，如下所示
<param name="base_local_planner" value="mpc_local_planner/MpcLocalPlannerROS" />
<param name="controller_frequency" value="5" />
<param name="controller_patiente" value="15.0"/>
<param name="clearing_rotation_allowed" value="false" /> <!-- carlike robot is not able to rotate in place -->
6、在上述move_base节点配置中调用mpc_local_planner的参数配置文件mpc_local_planner_params.yaml，如下图所示，需要注意的是文件路径可能要根据实际情况进行修改。
7、进行效果测试，并根据测试效果对参数进行调节
三、mpc_local_planner源码解读与规划流程梳理
本文的第三部分是本文核心部分，详细的对mpc_local_planner源码进行了解读，并对mpc_local_planner规划流程进行了梳理，本部分约1.5w字，预览图如下：
欢迎对以上内容感兴趣的小伙伴前往古月居，查看全文，链接如下：
https://www.guyuehome.com/45097
本篇文章我首发在古月居，因版权原因，在CSDN不能放全文，只能放一小部分，欢迎大家前往古月居查看完整文章！！！，链接如上↑↑↑