模型预测控制在路径规划中的应用

1.模型预测控制(Model Predictive Control)

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

• 问题模型

• 参数空间
  
  上述问题的目的是找到最优的$u$使得上式最小化。工程问题中系统输入是随意变化的量，因此需要将无限维度的系统输入转化为有限维度的参数，常用的方法有：Zero order hold(离散方法)；多项式；B-spline；数值映射
• 优化
  常用优化方法包括：搜索、凸优化和非凸优化；本文中，关于线性系统主要使用凸优化中的二次规划方法(Quadratic programming)求解；关于非线性系统主要使用PSO(Particle swarm optimization)求解。
• 控制
  
  传统MPC控制框图为：(1)设置优化问题 (2)使用测量模块告诉我们当前的initial
  state (3)求解优化问题得到参数，这些参数构成系统的最优输入$u^{\ast }$ (4)使用$u^{\ast }$驱动系统，由于系统受到干扰无法保证求解得到的$u^{\ast }$是我们想要的，仅此旨在很小一段时间中使用，然后利用观测的状态重新求解问题，转回步骤(2)

无人机应用中，使用传统MPC对控制频率的要求较高，要求较快的求解速度，考虑到无人机实际的计算资源，其使用受限，因此提出了Tube based MPC，控制框图如下：

上述框架分为Nominal System和Real System两部分，Nominal System部分不考虑不确定性，求解优化问题得到参考信号$x^{\ast }$，系统Real System部分的目的是使系统的真实状态与$x^{\ast }$保持一致，Associate Controller的目的是通过处理模型不确定性和外界干扰来镇定系统使其可以跟踪系统。

Matlan中有Matlab MPC toolbox工具包，可以一键生成C/C++代码。

2.线性MPC

下面以一个简单的模型为例，首先将连续模型通过时间离散化为离散模型：

将离散模型转化为矩阵形式，即构造预测模型：

根据要解决的问题构造问题模型，假设存在两个目标，目标1为将位置、速度、加速度带回原点，目标2为系统的轨迹尽量平滑，指标函数可以定义如下：

整体的目标函数可以构建为：

将前述预测模型的结果代入，我们有：

该问题可由二次规划算子(如matlab中的quadprog)求解，将上述方程表示如下：

求解结果为$J=-H^{-1}F$

线性MPC的硬约束

加入一个简单的速度和加速度的硬约束，首先将其改写为矩阵形式

代入前述的预测模型，将有关$v,a$的不等式修改为关于$J$的不等式，然后修改为不等式形式(工具箱求解时需要)：

软约束

一般是将约束直接添加到目标函数中，如下：

但是上述修改后的目标函数存在不可导的情况，无法使用二次规划求解，大部分MPC算子是二次规划的算子，为了解决上述问题，可以通过修改之前硬约束条件实现，以下式为例：
$-T_{v}J<=1_{20x1}+B_v$
首先加入slack variable：

修改目标函数为：

$L$的权重$w_5$一般取较大的值

3.非线性MPC

JLT
Jerk limitied trajectory(JLT)算法为BVP问题的一种解法。该问题主要对三阶积分器模型进行递推，从而得到从初始状态平滑过度到目标状态的轨迹，包括这个过程钟每一时刻的位置$p$，速度$v$、加速度$a$和jerk $u_j$。

首先考虑二阶积分器的情况（给定速度）：

该问题的求解本质上是一个分类讨论的过程，jerk的取值一般为三种，不变，某一正值或某一负值，由此得到的满足边界条件的加速度曲线为三角形，则三角形下面积即为速度，根据速度的边界条件，存在以下几种情况：

• 一是加速度曲线为三角形，且达到最大值，如上图中最上方的子图所示情况，此时从初始状态到达最大加速度的时间$△t_1$和从最大加速度降为0的时间$△t_3$可以直接求解得到；
• 二是速度超过了边界条件，此时需要将最大加速度(即三角形的顶点)下调，此时$△t_1$和$△t_3$也可以直接求解出来
• 三是，速度不够大，因此需要将顶点上调，但是由于存在加速度的边界条件，只能通过转变为梯形(即达到最大加速度后维持一段时间$△t_2$)解决。

JLT本身是对应三阶积分器数学模型的运动规划问题(给定位置)，问题表述如下：

该问题与给定速度的问题相似，也可以通过分类讨论求解，但是给定位置的情况下JLT需要求解的时间从3段$△t_1$、$△t_2$、$△t_3$扩展为7段，如下图所示：

从图中可以看出，类三角形的上升阶段对应时间段$△t_1$、$△t_2$、$△t_3$；巡航阶段对应$△t_4$；下降阶段对应$△t_5$、$△t_6$、$△t_7$；在确定这些时间参数之前，需要通过位置设定值、初始位置、初始速度、初始加速度等参数，进行上升阶段终止速度的搜索。

JLT与非线性MPC的结合
主要解决局部路径规划问题，步骤如下：

• 首先将地图建立为cost map供机器人观测：

Cost map中，地图中的点距离障碍物越近，cost越高；

• 然后进行全局规划，可以使用A*或JPS算法实现；
• 生成符合动力学的轨迹然后评估；

JLT与非线性MPC相结合主要用于求解最优轨迹，一个典型的MPC优化问题如下所示：

模型中约束条件共有4个，其中JLT的构建中满足了前三个，因此上述问题可以转变为：

障碍物约束有硬约束和软约束两种表达方式，对于软约束，我们可以将其直接添加到待优化的目标函数中：

上述问题转变为一个求解无约束的优化问题，其中$C_R$和$C_F$的第一项用于控制路径到达目标点的距离，$C_R$第二项用于使轨迹更平滑，二者第三项用于避障。上述问题由于加入了障碍物约束，问题不可直接求导，无法直接通过梯度求解。求解的一个方法是利用PSO(粒子群优化)求解，算法步骤如下：

算法具体思想是通过初始化一群随机粒子(随机解),然后通过迭代找到最优解，每次迭代中，粒子通过跟踪两个极值(全局最优解和历史最优解)来更新自己的速度和位置。

此外还可以与神经网络相结合求解问题，具体可见论文Model Predictive Local Motion Planning With Boundary State Constrained Primitives。