自动驾驶-控制算法（三）

前言

本文主要记录模型预测控制(MPC)的原理，以及基于MPC实现自动驾驶车辆路径跟踪。

一、模型预测控制

1.1 基本概念

模型预测控制（MPC）是一种控制方案，它使用一个模型来预测系统在有限时间窗口（视界）内的未来行为。根据这些预测和系统当前的测量/估计状态，计算出与确定的控制目标相关的最佳控制输入，并遵守系统约束条件。经过一定的时间间隔后，测量、估计和计算过程将以移动的视界重复进行。

MPC的主要优势：

1. 主动控制行动： 控制器预测未来的干扰、设定点等；
2. 非线性控制： MPC 可明确考虑非线性系统，而无需线性化；
3. 任意控制目标： 传统的设定点跟踪和调节或经济型 MPC；
4. 受约束的表述： 明确考虑物理、安全或运行系统约束。

1.2 整体流程

1 预测区间与控制区间
对于一般的离散系统，在$k$时刻，可以测量出系统当前状态$g(k)$，通过计算可得到$u(k),u(k+1),u(k+2),...,u(k+j)$，并得到系统未来状态的估计值$y(k),y(k+1),y(k+2),...,y(k+j)$。

预测区间为一次优化后预测未来输出的时间步个数，如下图中的$[k,k+P]$区间；
控制区间为进行控制估计的部分，如下图中$[k,k+M-1]$区间。

过小的控制区间，可能无法做到较好的控制；而较大的控制区间，会导致只有控制范围的前一部分才会有较好的效果，后一部分的效果甚微，且带来大量的计算开销。

2 约束
约束包括Hard约束和Soft约束，Hard约束是不可违背必须遵守的，如车轮转角；Soft约束是可以违反的（会带来一定的惩罚代价），如道路限速。
在控制系统中，输入输出都可能会有约束，但是在设计时不建议将输入输出都设为Hard约束，因为这两部分的约束有可能存在重叠，导致优化器产生不可行解。
建议输出采用Soft约束，输入中的输入参数、输入参数变化率建议设置一个Hard约束和一个Soft约束。

1.3 MPC设计

当模型时线性的时候，MPC的设计求解一般使用二次规划方法。

设线性模型为以下形式：
$$\begin{array}{r}{x}_{k+1}=A{x}_k+B{u}_k+C\end{array}$$
假定未来$m$步的控制输入已知，为$u(k),u(k+1),u(k+2),...,u(k+m)$，根据以上模型，可以计算未来$m$步的状态：
$x_{k+1}=A{x}_k+B{u}_k+C$
$x_{k+2}=A{x}_{k+1}+B{u}_{k+1}+C=A(A{x}_k+B{u}_k+C)+B{u}_{k+1}+C=A^2{x}_k+AB{u}_k+B{u}_{k+1}+AC+C$
$x_{k+3}=A{x}_{k+2}+B{u}_{k+2}+C=A^3{x}_k+A^{2}B{u}_k+AB{u}_{k+1}+B{u}_{k+2}+A^{2}C+AC+C$
$...$
$x_{k+m}=A^m{x}_k+A^{m-1}B{u}_k+...A^{m-i}B{u}_{k+i-1}+...+B{u}_{k+m-1}+A^{m-1}C+A^{m-2}C+...+C$

将上述方程组写成矩阵向量形式可得：
$$\begin{array}{r}X=\mathcal{A}{x}_k+\mathcal{B}\mathbf{u}+\mathcal{C}\end{array}$$
其中：
$X=\left[\begin{array}{c}{x}_{k+1}\\ x_{k+2}\\ x_{k+3}\\ ⋮\\ x_{k+m}\end{array}\right];\mathbf{u}=\left[\begin{array}{c}{u}_k\\ u_{k+1}\\ u_{k+2}\\ ⋮\\ u_{k+m-1}\end{array}\right];\mathcal{A}=\left[\begin{array}{c}A\\ A^2\\ A^3\\ ⋮\\ A^m\end{array}\right]$

$\mathcal{B}=\left[\begin{array}{ccccc}B& 0& 0& ...& 0\\ AB& B& 0& ...& 0\\ A^{2}B& AB& B& ...& 0\\ ⋮& ⋮& ⋮& \ddots & ⋮\\ A^{m-1}B& A^{m-2}B& A^{m-3}B& ...& B\end{array}\right]$

$\mathcal{C}=\left[\begin{array}{c}C\\ AC+C\\ A^{2}C+AC+C\\ ...\\ A^{m-1}C+...+C\end{array}\right]$

上式$\mathcal{B}$中的下三角形式，直接反应了系统在时间上的因果关系，即$k+1$时刻的输入对$k$时刻的输出没有影响，$k+2$时刻的输入对$k$和$k+1$时刻没有影响。
假定参考轨迹为$\bar{X}=[{\bar{x}}_{k+1},{\bar{x}}_{k+2},...,{\bar{x}}_{k+m}{]}^T$，则MPC的一个简单的目标代价函数如下：

$$\begin{array}{r}\min \mathcal{J}={\mathcal{E}}^{T}Q\mathcal{E}+{\mathbf{u}}^{T}R\mathbf{u}\\ s.t.u_{min}\le \mathbf{u}\le u_{max}\end{array}$$

其中，$\mathcal{E}=X-\bar{X}=[x_{k+1}-{\bar{x}}_{k+1},x_{k+2}-{\bar{x}}_{k+2},...,x_{k+m}-{\bar{x}}_{k+m}{]}^T$

以上最优化问题可通过二次规划求解，得到满足目标代价函数最小的最优控制序列$\mathbf{u}={\left[\begin{array}{ccccc}{u}_k& u_{k+1}& u_{k+2}& ...& u_{k+m-1}\end{array}\right]}^T$

二、基于MPC的自动驾驶车辆轨迹跟踪

基于运动学模型的离散状态空间方程如下：
$$\begin{array}{r}\tilde{X}(k+1)=\tilde{A}\tilde{X}(k)+\tilde{B}\tilde{\mathrm{u}}(k)\end{array}$$
其中：
$\tilde{X}(k)=\left[\begin{array}{c}x(k)-x_r\\ y(k)-y_r\\ \psi (k)-{\psi }_r\end{array}\right]$

$\tilde{\mathrm{u}}(k)=\left[\begin{array}{c}v(k)-v_r\\ \delta (k)-{\delta }_r\end{array}\right]$
$\tilde{A}=A\cdot T+I=\left[\begin{array}{ccc}1& 0& -v_r\cdot T\cdot \sin {\psi }_r\\ 0& 1& v_r\cdot T\cdot \cos {\psi }_r\\ 0& 0& 1\end{array}\right]$

$\tilde{B}=B\cdot T=\left[\begin{array}{cc}T\cos {\psi }_r& 0\\ T\sin {\psi }_r& 0\\ \frac{T\cdot \tan {\delta }_r}{L}& \frac{v_r\cdot T}{L\cdot {\cos }^2{\delta }_r}\end{array}\right]$

式中，$X_r=[x_r,y_r,{\psi }_r]$为参考点处的状态，${\mathrm{u}}_r=[v_r,{\delta }_r]$为参考点处的控制量；$T$为采样步长，$I$为单位矩阵，维度与矩阵$A$一致。
将上述状态方程进行改写可得：
$$\begin{array}{r}X(k+1)=\tilde{A}X(k)+\tilde{B}\mathrm{u}(k)+X_r-\tilde{A}{X}_r-\tilde{B}{\mathrm{u}}_r\end{array}$$
其中$X(k)=[x(k),y(k),\psi (k)]$，$\mathrm{u}(k)=[v(k),\delta (k)]$表示$k$点处的状态量和控制量。
令$\tilde{C}=X_r-\tilde{A}{X}_r-\tilde{B}{\mathrm{u}}_r$，可得：
$$\begin{array}{r}X(k+1)=\tilde{A}X(k)+\tilde{B}\mathrm{u}(k)+\tilde{C}\end{array}$$

MPC控制的代价函数定义如下：
$$\begin{array}{r}J={\left(X(N)-\bar{X}(N)\right)}^T{Q}_f\left(X(N)-\bar{X}(N)\right)+\sum _{k=0}^{N-1}{\left(X(k)-\bar{X}(k)\right)}^{T}Q\left(X(k)-\bar{X}(k)\right)+\mathrm{u}(k{)}^{T}R\mathrm{u}(k)\end{array}$$

$$\begin{gathered} subjectto:X(k+1)=\tilde{A}X(k)+\tilde{B}\mathrm{u}(k)+\tilde{C} \\ {\mathrm{u}}_{min}\le \mathrm{u}(k)\le {\mathrm{u}}_{max} \\ X(0)=X_0 \end{gathered}$$
其中，矩阵$Q$，$Q_f$，$R$为正定对称矩阵，$X(k),\mathrm{u}(k)$分别表示状态量和控制量，其中控制量受 $[{\mathrm{u}}_{min},{\mathrm{u}}_{max}]$约束。$X_0$表示初始状态，$X(N)$表示终端状态。$\bar{X}(k)$表示参考路径在$k$时刻的状态。

求解上述MPC问题需要将其改写为二次规划的形式，然后用求解器进行求解。
二次规划的标准形式为：
$$\begin{array}{r}\mathrm{minimize}\frac{1}{2}{x}^{T}Px+q^{T}x\\ subjecttol\le A_{c}x\le u\end{array}$$
其中hessian矩阵$P$为：
$$\begin{array}{r}P=diag(Q,Q,...,Q_f,R,...,R)\end{array}$$
矩阵维度为：$P\in R^{(3\ast (N+1)+2\ast N)\times (3\ast (N+1)+2\ast N)}$。（状态变量个数为3，控制变量个数为2）
梯度向量$q$为：
$$\begin{array}{r}q=\left[\begin{array}{c}-Q\bar{X}(0)\\ -Q\bar{X}(1)\\ ⋮\\ -Q\bar{X}(N)\\ 0\\ 0\\ ⋮\\ 0\end{array}\right]\end{array}$$
矩阵维度为：$Q\in R^{(3\ast (N+1)+2\ast N)\times 1}$

优化变量$x$为：
$$\begin{array}{r}x=\left[\begin{array}{c}X(0)\\ X(1)\\ ⋮\\ X(N)\\ \mathrm{u}(0)\\ \mathrm{u}(1)\\ ⋮\\ \mathrm{u}(N-1)\end{array}\right]\end{array}$$
矩阵维度为：$x\in R^{(3\ast (N+1)+2\ast N)\times 1}$

线性约束矩阵$A_c$为：
$$\begin{array}{r}{A}_c=\left[\begin{array}{ccccccccc}-I& 0& 0& \cdots & 0& 0& 0& \cdots & 0\\ A& -I& 0& \cdots & 0& B& 0& \cdots & 0\\ 0& A& -I& \cdots & 0& 0& B& \cdots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮& ⋮& ⋮& \ddots & ⋮\\ 0& 0& 0& \cdots & -I& 0& 0& \cdots & B\\ 0& 0& 0& \cdots & 0& I& 0& \cdots & 0\\ 0& 0& 0& \cdots & 0& 0& I& \cdots & 0\\ ⋮& ⋮& ⋮& \ddots & ⋮& ⋮& ⋮& \ddots & ⋮\\ 0& 0& 0& \cdots & 0& 0& 0& \cdots & I\end{array}\right]\end{array}$$
矩阵维度为：$A_c\in R^{(3\ast (N+1)+2\ast N)\times (3\ast (N+1)+2\ast N)}$
约束条件的上界 $u$ 和下界 $l$ 分别为：
$$\begin{array}{r}l=\left[\begin{array}{c}-X(0)\\ -\tilde{C}\\ ⋮\\ -\tilde{C}\\ {\mathrm{u}}_{min}\\ {\mathrm{u}}_{min}\\ ⋮\\ {\mathrm{u}}_{min}\end{array}\right]u=\left[\begin{array}{c}-X(0)\\ -\tilde{C}\\ ⋮\\ -\tilde{C}\\ {\mathrm{u}}_{max}\\ {\mathrm{u}}_{max}\\ ⋮\\ {\mathrm{u}}_{max}\end{array}\right]\end{array}$$
矩阵维度为：$l\in R^{(3\ast (N+1)+2\ast N)\times 1}$， $u\in R^{(3\ast (N+1)+2\ast N)\times 1}$

参考文献

1、Basics of model predictive control
2、An Introduction to Model-based PredictiveControl (MPC)
3、【自动驾驶】模型预测控制(MPC)实现轨迹跟踪
4、Using osqp-eigen in MPC fashion