六、基于MPC的车辆控制及轨迹规划

6.1、MPC简介

MPC的基本思想为：在每一个采样时刻，根据获得的当前测量信息，在线求解一个有限时间开环优化问题，并将得到的控制序列的第一个元素作用于被控对象。在下一个采样时刻，重复上述过程，用新的测量值作为此时预测系统未来动态的初始条件，刷新优化问题并重新求解 。
即MPC算法包括三个步骤：

（1）预测系统未来动态；

（2）求解开环优化问题；

（3）将优化解的第一个元素（或者说第一部分）作用于系统；

这三步是在每个采样时刻重复进行的，且无论采用什么样的模型，每个采样时刻得到的测量值都作为当前时刻预测系统未来动态的初始条件。

6.2、MPC原理

现在有一个线性系统
$$\begin{gathered} x_{k+1}=A{x}_k+B{u}_k \\ {y}_k=C{x}_k \end{gathered}$$
则输入量$u$与状态量$x$之间的关系可以表示为
$$x_k=A^k{x}_0+\sum _{j=0}^{k-1}{A}^{j}B{u}_{k-1-j}$$
将上式写成矩阵的形式
$$\left[\begin{array}{c}{x}_1\\ x_2\\ ⋮\\ x_{N-1}\\ x_N\end{array}\right]=\left[\begin{array}{cccc}B& 0& \dots & 0\\ AB& B& \dots & 0\\ ⋮& ⋮& \ddots & ⋮\\ A^{N-1}B& A^{N-2}B& \dots & B\end{array}\right]\left[\begin{array}{c}{u}_0\\ u_1\\ ⋮\\ u_{N-1}\end{array}\right]+\left[\begin{array}{c}A\\ A^2\\ ⋮\\ A^N\end{array}\right]x_0$$
则代价函数可以写作
$$J=x_N^{T}P{X}_N+\sum _{k=0}^{N-1}(x_k^{T}Q{x}_k+u_k^{T}R{u}_k)$$
其中
$$\begin{gathered} P=P^T>0 \\ Q=Q^T>0 \\ R=R^T>0 \end{gathered}$$
我们的目标是寻找一个最优的控制序列$u_{0:N-1}^{\ast }$，从而使代价函数$j$最小
$$J(z,x_0)=x_0^{T}Q{x}_0+{\left[\begin{array}{c}{x}_1\\ x_2\\ ⋮\\ x_{N-1}\\ x_N\end{array}\right]}^T\left[\begin{array}{ccccc}Q& 0& 0& \dots & 0\\ 0& Q& 0& \dots & 0\\ ⋮& ⋮& \ddots & ⋮& ⋮\\ 0& \dots & 0& Q& 0\\ 0& 0& \dots & 0& P\end{array}\right]+{\left[\begin{array}{c}{u}_0\\ u_1\\ ⋮\\ u_{N-1}\end{array}\right]}^T\left[\begin{array}{cccc}R& 0& \dots & 0\\ 0& R& \dots & 0\\ ⋮& ⋮& \ddots & ⋮\\ 0& \dots & 0& R\end{array}\right]\left[\begin{array}{c}{u}_0\\ u_1\\ ⋮\\ u_{N-1}\end{array}\right]$$
令$\bar{S}=\left[\begin{array}{cccc}B& 0& \dots & 0\\ AB& B& \dots & 0\\ ⋮& ⋮& \ddots & ⋮\\ A^{N-1}B& A^{N-2}B& \dots & B\end{array}\right]$，$z=\left[\begin{array}{c}{u}_0\\ u_1\\ ⋮\\ u_{N-1}\end{array}\right]$，$\bar{T}=\left[\begin{array}{c}A\\ A^2\\ ⋮\\ A^N\end{array}\right]$，$\bar{Q}=\left[\begin{array}{ccccc}Q& 0& 0& \dots & 0\\ 0& Q& 0& \dots & 0\\ ⋮& ⋮& \ddots & ⋮& ⋮\\ 0& \dots & 0& Q& 0\\ 0& 0& \dots & 0& P\end{array}\right]$，$\bar{R}=\left[\begin{array}{cccc}R& 0& \dots & 0\\ 0& R& \dots & 0\\ ⋮& ⋮& \ddots & ⋮\\ 0& \dots & 0& R\end{array}\right]$,

则代价函数改写为
$$\begin{array}{rl}J(z,x_0)& =(\bar{S}z+\bar{T}{x}_0{)}^T\bar{Q}(\bar{S}z+\bar{T}{x}_0)+z^T\bar{R}z+x_0^{T}Q{x}_0\\ & =\frac{1}{2}{z}^{T}2(\bar{R}+{\bar{S}}^T\bar{Q}\bar{S})z+x_0^{T}2{\bar{T}}^T\bar{Q}\bar{S}z+\frac{1}{2}{x}_0^{T}2(Q+{\bar{T}}^T\bar{Q}\bar{T})x_0\\ & =\frac{1}{2}{z}^{T}Hz+x_0^{T}Fz+\frac{1}{2}{x}_0^{T}E{x}_0\end{array}$$
令$H=2(\bar{R}+{\bar{S}}^T\bar{Q}\bar{S})$，$F=2{\bar{T}}^T\bar{Q}\bar{S}$，$E=2(Q+{\bar{T}}^T\bar{Q}\bar{T})$

我们的目标是选择一个合适的控制序列$z$，使得代价函数的值最小。

等式两边同时对$z$求偏导，得
$$\begin{gathered} {▽}_{z}J(z,x_0)=Hz+F^T{x}_0=0 \\ {z}^{\ast }=-H^{-1}{F}^T{x}_0 \end{gathered}$$
这样就得到最优控制序列$z^{\ast }$，它与初始状态$x_0$呈线性关系。

在这里，$z^{\ast }$的计算量主要体现在$H$矩阵的求逆上，由于$H$是一个稠密矩阵（非0元素占所有元素比例较大的矩阵），该矩阵的求逆运算计算量很大，如何加速计算呢？

一个比较直观的方法是将稠密矩阵稀疏化，将系统未来时刻的输入和状态拼成一个控制序列$z=\left[\begin{array}{c}{u}_0\\ u_1\\ ⋮\\ u_{N-1}\\ x_1\\ ⋮\\ x_N\end{array}\right]$，则系统代价函数就转化为
$$J(z,x_0)=x_0^{T}Q{x}_0+z^T\left[\begin{array}{ccccc}R& 0& 0& \dots & 0\\ 0& R& 0& \dots & 0\\ ⋮& ⋮& \ddots & ⋮& ⋮\\ 0& \dots & 0& Q& 0\\ 0& 0& \dots & 0& P\end{array}\right]z$$
代价函数就变成了一个对角矩阵，$H$矩阵就变成了一个稀疏矩阵，其求逆操作计算量比较小。但是这样引入了新约束，在无约束情况下不会使用。

另一种方法运用了动态规划的思想，动态规划算法与分治法类似，其基本思想也是将待求解问题分解成若干个子问题，先求解子问题，然后从这些子问题的解得到原问题的解。

对于代价函数$J=x_N^{T}P{X}_N+{\sum }_{k=0}^{N-1}(x_k^{T}Q{x}_k+u_k^{T}R{u}_k)$求解最小值的问题，假设我们已知$x_k$，则$u_k^{\ast }$是$x_K$到$x_N$这个子问题的最优解。

假设我们已知$x_N$，则第$N$个时刻的代价函数$J_N^{\ast }(x_N)=x_N^{T}P{x}_N$；

假设我们已知$x_{N-1}$，则第$N-1$个时刻的代价函数为$J_{N-1}^{\ast }=x_{N-1}^{T}Q{x}_{N-1}+u_{N-1}^{T}R{u}_{N-1}+J_N^{\ast }(x_N)$；

由于$x_N=A{x}_{N-1}+B{u}_{N-1}$，则第$N-1$个时刻的控制量
$$\begin{array}{rl}{u}_{N-1}(x_{N-1})& =argmin{x}_{N-1}^{T}Q{x}_{N-1}+u_{N-1}^{T}R{u}_{N-1}+J_N^{\ast }(A{x}_{N-1}+B{u}_{N-1})\\ & =argmin{x}_{N-1}^T(A^{T}PA+Q)x_{N-1}+U_{N-1}^T(B^{T}PB+R)u_{N-1}+2x_{N-1}^T{A}^{T}PB{u}_{N-1}\end{array}$$
求导得，$2(B^{T}PB+R)u_{N-1}^{\ast }+2B^{T}PA{x}_{N-1}=0$；

解得$u_{N-1}^{\ast }(x_{N-1})=K{x}_{N-1}$，其中$K=-(B^{T}PB+R{)}^{-1}{B}^{T}PA$；

在每次计算中，矩阵$P$不断更新，$P=A^{\prime }PA+Q-A^{\prime }PB(B^{T}PB+R{)}^{-1}{B}^{T}PA$；

不断重复上述过程，就可求得第0时刻的控制量$u_0^{\ast }(x_0)=K{x}_0$。

6.3、车辆运动模型构建

​ 在本节中介绍另一种车辆模型，$CurvilinearCoordinatesVehicleModel$，该模型能够很好地描述车辆与参考轨迹之间的关系。

如图所示，选取状态量$x=\left[\begin{array}{c}s\\ n\\ \mu \\ v\\ a\\ \delta \\ \dot{\delta }\end{array}\right]$，其中

$s$代表车辆沿轨迹行驶过的距离；

$n$代表车辆与参考轨迹的横向距离；

$\mu$代表轨迹上参考点切线方向与车辆行驶方向的夹角；

$v$代表车辆速度；

$a$代表车辆加速度；

$\delta$代表车辆转向角；

$\dot{\delta }$则代表车辆转向角的变化率；

选取控制量$u=\left[\begin{array}{c}{u}_{jerk}\\ u_{\ddot{\delta }}\end{array}\right]$，其中

$u_{jerk}$是速度的二阶导数；

$u_{\ddot{\delta }}$是转向角度的二阶导数；

车辆沿着轨迹运动的速度可以表示为
$$v_s=v_x\cos \mu -v_y\sin \mu =(R-n){\dot{\varphi }}_S$$

因为$\dot{s}=R{\dot{\varphi }}_s$，进而可以推出
$$\dot{s}=\frac{R}{R-n}(v_x\cos \mu -v_y\sin \mu )$$
因为曲率$k=\frac{1}{R}$，继续可以推出
$$\dot{s}=\frac{v_x\cos \mu -v_y\sin \beta j\sin \mu }{1-nk}$$
因为$v_x=v\cos \beta$，$v_y=v\sin \beta$，继续推出
$$\dot{s}=\frac{v\cos \beta \cos \mu -v\sin \beta \sin \mu }{1-nk}=\frac{v\cos (\beta +\mu )}{1-nk}$$

横向距离的变化率可以表示为
$$\dot{n}=v_x\sin \mu +v_y\cos \mu =v\cos \beta \sin \mu +v\sin \beta \cos \mu =v\sin (\beta +\mu )$$
同样的，航向偏差的变化率可以表示为
$$\dot{\mu }=r-{\dot{\varphi }}_s=\frac{v}{R_v}-k\frac{v\cos (\beta +\mu )}{1-nk}=\frac{v\sin \beta }{l_r}-k\frac{v\cos (\beta +\mu )}{1-nk}$$
其中，$r$代表车辆自身的转速；

整理以上的推导，得
$$\dot{x}=\left[\begin{array}{c}\dot{s}\\ \dot{n}\\ \dot{\mu }\\ \dot{v}\\ \dot{v}\\ \dot{a}\\ \dot{\delta }\\ \ddot{\delta }\end{array}\right]=\left[\begin{array}{c}\frac{v\cos (\mu +\beta )}{1-nk}\\ v\sin (\mu +\beta )\\ \frac{v}{l_r}\sin (\beta )-k\frac{v\cos (\mu +\beta )}{1-nk}\\ a\\ u_{jerk}\\ \dot{\delta }\\ u_{\ddot{\delta }}\end{array}\right]$$
其中，$\beta ={\tan }^{-1}(\frac{l_r}{l_r+l_f}\tan ({\delta }_f))$；

6.4、代价函数构建

代价函数主要由三个方面构成，一是保持轨迹跟踪精确性的代价，二是保持行驶中乘员舒适性的代价，三是保持行驶安全性能的代价。即
$$J=w_t\cdot J_{tracking}+w_C\cdot J_{comfort}+w_s\cdot J_{safety}$$
其中

$w_t$是轨迹追踪性能的权重；

$w_c$是舒适性能的权重；

$w_s$是安全性能的权重；

如图所示，对于轨迹追踪性能的代价函数
$$J_{tracking}=w_{\dot{s}}(\dot{s}-{\dot{s}}_{ref}{)}^2+w_n\cdot n^2+w_{\mu }(\mu +\beta {)}^2$$
其中

$(\dot{s}-{\dot{s}}_{ref}{)}^2$代表沿着参考轨迹的速度偏差；

$n^2$代表侧向位移偏差；

$(\mu +\beta {)}^2$代表角度偏差；

而$w_{\dot{s}}$、$w_n$、$w_{\mu }$则代表相应的权重；

对于舒适性的代价函数
$$J_{comfort}=w_{jx}\cdot j_x^2+w_{a_x}\cdot a_x^2+w_{a_y}\cdot a_y^2+w_{\dot{\delta }}\cdot \dot{\delta }+w_{\ddot{\delta }}\cdot {\ddot{\delta }}^2$$
其中，$j_x$代表纵向速度；

$a_x$代表横向加速度；

$a_y$代表纵向加速度；

$\dot{\delta }$代表转向角的变化率；

$\ddot{\delta }$代表转向角的加速度；

而$w_{jx}$、$w_{a_x}$、$w_{a_y}$、$w_{\dot{\delta }}$、$w_{\ddot{\delta }}$则代表相应的权重；

对于安全性能的代价函数，我们需要定义一个约束，在约束以内的行为，看作是安全的行为，而在约束以外的行为，则看作不安全的行为。

约束又可以分为硬约束和软约束，硬约束是指车辆在行驶过程中绝对达不到的某种状态而作出的约束，在优化过程中，这种约束不能违反；而软约束指车辆在行驶过程中可以达到的但是不安全的状态，这类约束在特殊情况下可以违反，但是其相应的代价很大。

在工程中，一般把输入量设置为硬约束，而将状态量设置为软约束。

在这里需要引入一个新的变量——松弛因子${\lambda }_k$，它将系统对约束的违反程度转化为代价函数，从而实现优化求解。

所以，安全性能的代价函数可以表示为
$$J_{safety}={\lambda }_{soft}^{T}E{\lambda }_{soft}+\bar{H}{\lambda }_{hard}$$
其中，$E$为代表软约束权重的对角矩阵，$E=diag(w_{{\lambda }_{n,soft}},w_{{\lambda }_{v,soft}},w_{{\lambda }_{a,soft}})$；

$\bar{H}$为代表硬约束权重的行向量，$\bar{H}=[w_{{\lambda }_n},w_{{\lambda }_a},w_{{\lambda }_s}]$

${\lambda }_{soft}$为软约束的松弛因子，${\lambda }_{soft}=\left[\begin{array}{c}{\lambda }_{n,soft}\\ {\lambda }_{v,soft}\\ {\lambda }_{q,soft}\end{array}\right]$，${\lambda }_{n,soft}$代表违反横向偏差软约束的代价，${\lambda }_{v,soft}$代表违反速度偏差软约束的代价，${\lambda }_{a,soft}$代表违反加速度偏差软约束的代价；

${\lambda }_{hard}$为硬约束的松弛因子，${\lambda }_{hard}=\left[\begin{array}{c}{\lambda }_n\\ {\lambda }_v\\ {\lambda }_q\end{array}\right]$，${\lambda }_{n,soft}$代表违反横向偏差硬约束的代价，${\lambda }_{v,soft}$代表违反速度偏差硬约束的代价，${\lambda }_{a,soft}$代表违反加速度偏差硬约束的代价；

以上，我们就得到了三个性能指标的代价函数。
$$\begin{gathered} J_{tracking}=w_{\dot{s}}(\dot{s}-{\dot{s}}_{ref})+w_n\cdot n^2+w_{\mu }\cdot (\mu +\beta {)}^2 \\ {J}_{comfort}=w_{jx}\cdot j_x^2+w_{a_x}\cdot a_x^2+w_{a_y}\cdot a_y^2+w_{\dot{\delta }}\cdot \dot{\delta }+w_{\ddot{\delta }}\cdot {\ddot{\delta }}^2 \\ {J}_{safety}={\lambda }_{soft}^{T}E{\lambda }_{soft}+\bar{H}{\lambda }_{hard} \end{gathered}$$
接着，为了使$MPC$有解，我们还需要对有限时域内的终端状态进行约束。

综上，总的代价函数可以表示为
$$J(x_k,U)=\sum _{k=0}^{N-1}{J}_{stage}(x_k,u_k,{\lambda }_k)+J_{terminal}(x_N)$$

6.5、约束构建

在自动驾驶控制问题中，除了要满足输入输出以及系统状态的约束，还将考虑以下约束
$$\begin{gathered} c^{station}(x,{\lambda }_s)\le 0 \\ {c}^{vel}(x,{\lambda }_{v,soft})\le 0 \\ {c}^{tube,hard}(x,{\lambda }_n)\le 0 \\ {c}^{tube,soft}(x,{\lambda }_{n,soft})\le 0 \\ {c}^{a,hard}(x,{\lambda }_a)\le 0 \\ {c}^{a,soft}(x,{\lambda }_{a,soft})\le 0 \\ {c}^{\ddot{\delta }}(x)\le 0 \end{gathered}$$
其中包括了对安全距离的约束、对车辆速度的软约束、空间上的软硬约束、加速度的软硬约束以及转向角加速度的约束，下面将一一讨论。

首先是纵向安全距离的约束
c k s t a t i o n ( x k , λ s ) = { s k − s k m a x − λ s , k   ≤ 0 − s k + s k m i n − λ s , k   ≤ 0 , ∀ k ∈ { 0 , … , N } c_k^{station}(x_k,\lambda_s)= \begin{cases} s_k-s_k^{max}-\lambda_{s,k}\ \le0\\ -s_k+s_k^{min}-\lambda_{s,k}\ \le0 \end{cases},\qquad \forall k\in\{0,\dots,N\} ckstation​(xk​,λs​)={sk​−skmax​−λs,k​ ≤0−sk​+skmin​−λs,k​ ≤0​,∀k∈{0,…,N}
其次是速度约束
$$v-v_{max}-\lambda \le 0$$
接着是横向空间约束，对于硬约束，有
KaTeX parse error: Undefined control sequence: \forallk at position 350: …d{cases},\quad \̲f̲o̲r̲a̲l̲l̲k̲\in\{0,\dots,N\…
其中，$n_k$是横向偏差，$d_{car,left,front}({\mu }_k)$是左前方由于转角引起的横向偏差，$d_{road,left,front,k}$是车辆左前方偏差极限，${\lambda }_{n,k}$是松弛因子。

接着是加速度的硬约束，有
c k a , h a r d ( x k , λ a ) = a y 2 a y , m a x m k 2 + a x 2 a x , m a x , k 2 − 1 − λ a ≤ 0 , ∀ k ∈ { 0 , … , N } c_k^{a,hard}(x_k,\lambda_a)=\sqrt{\frac{a_y^2}{a_{y,maxmk}^2}+\frac{a_x^2}{a_{x,max,k}^2}}-1-\lambda_a\le0,\quad\forall k\in\{0,\dots,N\} cka,hard​(xk​,λa​)=ay,maxmk2​ay2​​+ax,max,k2​ax2​​ ​−1−λa​≤0,∀k∈{0,…,N}
对于加速度的软约束，有
$$c_k^{a,soft}(x_k,{\lambda }_{a,soft})=\{\begin{array}{l}{a}_k-a_{comfort}-{\lambda }_{a,soft}\le 0\\ -a_k-dece{l}_{comfort}-{\lambda }_{a,soft}\le 0\end{array}$$

6.5、主流MPC求解器及MPC加速

主流$MPC$求解器如下：

使用$MPC$进行车辆控制时有一个很现实的问题，如何加快求解器的求解速度？

工程中一般有如下几个方法：

（1）简化模型的复杂度，去除影响比较小的状态参数；

（2）减小预测时长；

（3）减少约束；

（4）降低精确度；

（5）使用离线$MPC$控制；