MPC算法学习(1)

北白川家的氷菓

已于 2022-08-16 20:32:11 修改

阅读量1.6w

点赞数 43

分类专栏：笔记文章标签：学习算法自动驾驶

于 2022-07-19 02:30:23 首次发布

本文链接：https://blog.csdn.net/aliezaliezaliez/article/details/125824161

版权

笔记专栏收录该内容

5 篇文章 4 订阅

订阅专栏

一、了解MPC

1.MPC → Model PredictIve Control

1.非线性MPC（nonlinear MPC\NMPC）

2.线性时变MPC（LTV MPC）

一、了解MPC

1.MPC → Model PredictIve Control

（1）处理对象：输入输出之间有交互作用的多输入多输出系统（MIMO System）

（2）PID 对于该类型系统的局限性：

① 如上图所示，两个回路之间没有交叉，就像二者是相互独立一样

② 如果系统（Plant）足够大，因为调参十分困难

MPC的优势：

① 可以处理多变量问题

② 可以添加约束

e.g.在驾驶时，汽车有期望的轨迹，同时有加速约束

③ 有预览功能（类似前馈），以改善控制器性能

（3）MPC硬件条件

需要功能强大的，内存大的处理器，因为每个时间步骤都需要在线优化问题

2.MPC基本工作原理

以如图的过程为例，期望汽车沿中线运动：

MPC在该过程中的作用如下：
（1）首先，预测一条p步之后可以到达中线的路径（这里有个问题，这个p如何选择呢？选择过少会不会导致无解？过多会不会降低效率？）该过程涉及了汽车模型本身的使用。

显然这种路径有无穷多条，我们选择“代价（cost）"最小的一条，该过程由优化器（optimizer）完成。对于这种情况，对应的cost包括：

① 汽车在此p步中，每一步的位置都会与中线有一个相对应的偏差值，我们想要这个偏差值之和最小

② 汽车每一次的打舵量要尽可能的小，否则会让乘客十分不舒服，即打舵量之和应该也最小

综上，应该让二者加权和最小，即：

$J=\sum_{i=1}^{p}{w}_{e}{e}_{k+i}^{2}+\sum_{i=0}^{p-1}{w}_{\Delta u}{\Delta u}_{k+i}^{2}$

我们总会找到一条使J最小的路径

一般地，有：

（2）进行条件约束，例如车必须在车道里，同时方向盘的转角应该有一个范围

（3）进行该路径中p步中的第一步，即进行响应角度的方向盘转动，而将后面的步骤舍弃

（4）在新的起点重复以上过程，下一次预测的结果可能与上次的预测不同，因为可能有干扰的存在（风，湿滑的路面）

根据上述过程，我们可知，我们每次运动后都在预测未来p步的路径，因此我们所谓的”预测视界“是向前运动的，因此MPC也被称为后退视界控制。

操纵变量与输出变量

二、MPC设计

1.MPC参数设计

（1）基本参数

① 采样时间 ${T}_{s}$ → 决定控制器执行控制算法的速率

太大：无法快速处理扰动

太小：计算负荷过大

因此一般情况下选择： $\tfrac{{T}{r}}{20}\leq {T}_{s}\leq \tfrac{{T}{r}}{10}$

② 预测范围：即预测未来事件步长的数量

太短：假如刹车需要5s，但是我只能预测到两秒外的视界，距离交通灯2s路程时开始制动，此时已经来不及了，我们必定会冲过头

太长：可以涵盖之后很长一段时间内的范围，但是如果发生突发情况，比如行人过马路，此时我们必须停车，那么之后就要重新预测，假如我们预测了未来20s的事件，在5s时发生了行人过马路事件，那么后面15s的内容我们就白预测了，即浪费了算力。当然，任何长度的预测范围都可能发生突发情况，但是预测范围越长，我们进行无效预测的时间越长，出现无效预测的概率越大。

因此，我们必须选择一个涵盖整个系统动态的预测范围，但是也不宜太长

建议选择步长20-30

③ 控制范围：控制移动到时间步长m的次数

步长m较大时，可以获得更好的预测，但是会增加复杂性，我们甚至可以是控制范围与预测范围相同，但是一般只有前几个控制动作才对输出行为有影响，因此控制范围最好是预测范围的10%到20%，至少2-3步

进一步理解：

假如”我“是一个学生，“我”的能力对于导师而言是完全可知的，在工程上，就是”我“的模型是完全可知的，导师会给我安排学习任务，假如导师在知道我能力的情况下做好了未来七天的计划（预测范围），但是只给我安排了五天的学习任务（控制范围），但在我完成一天学习后，我可能超额完成任务，也可能不达标（外界干扰导致没有达到预测结果），因此我只完成被安排的学习任务的第一天的任务，在此之后，导师再次制定七天之后的计划，并给我安排下五天的计划。长此以往，导师发现，制定七天的任务太多了，会浪费他宝贵的时间，因此之后改为制定五天计划，安排三天计划。

④ 硬约束和软约束：硬约束必须服从，软约束不一定服从，如果全是硬约束，比如输入输出全部采用硬约束，那么可能会导致无法解决优化问题

软约束可以通过优化问题保证违反程度较小

⑤ 权重：

通过权衡来衡量竞争目标之间的权重（如输入和输出有不同权重）

同时，对于同一个组，我们可以对不同的成员分配权重（如第一输出和第二输出有不同权重）

2.MPC使用范围

（1）线性系统，线性约束条件，有二次cost函数→ 可以使用线性MPC

（2）非线性系统，同样可以使用线性MPC，原理是线性化

如：在自适应MPC中，随着运行条件变化，工作点也变化，因此可以动态地在工作点附近进行线性化，从而动态获得线性模型

注：对于自适应MPC，在不同运行条件下，状态数和约束数不会改变

如果随着操作条件变化，状态数改变：使用增益预定MPC，在感兴趣的工作点进行离线显性化，针对每个工作点设计线性

（3）如果非线性系统难以进行线性化，可以使用非线性MPC，此时做出的预测更加准确，但同时，优化问题变得不凸，即可能有很多局部最优解，如图：

3.MPC速度优化

I.（1）模型简化，可以减少状态数量

（2）预测范围适当减小

（3）控制范围适当减小

（4）减少约束数量

（5）降低数据表示精度

II.当采样时间特别小时，可以采用显式MPC（Implicit MPC）

显式MPC特点：离线解决问题，而不是在线优化，对于范围内每个x值，都预先计算最佳解，

III. 采用次优解决方案

如果想在采样时间内找到解决方案，同时还有多余时间执行其他任务，可以确定迭代次数最大值，当达到最大次数时，停止最优化

三、MPC推导

1.基于单位脉冲响应的公式推导

步骤：

（1）确定模型

（2）进行预测

（3）滚动优化

（4）误差补偿

对于模型已知的系统，其单位脉冲响应在每个采样时刻都是可知的，根据离散卷积定理，此时输出可以用输入和单位脉冲响应的卷积计算

假设在k+1时刻开始输入信号，对等式两侧进行差分，则在k+1时刻及其后，有：

$\Delta {y}_{k+1}={a}_{1}\Delta {u}_{k}$

$\Delta {y}_{k+2}={a}_{1}\Delta {u}_{k+1}+{a}_{2}\Delta {u}_{k}$

$\cdot$

$\Delta {y}_{k+m}={a}_{1}\Delta {u}_{k+m-1}+...+{a}_{m}{u}_{k}$

$\Delta {y}_{k+p}={a}_{p-m+1}\Delta {u}_{k+m-1}+...+{a}_{p}{u}_{k}$

这里需要注意一点，由于控制范围是m，预测范围为p，一般情况下，控制范围比预测范围要小一点，因此m会小于p，而控制范围反应的是输入信号，因此u只有m个序号，即从k到k+m-1，在第m步预测之后，后面的累加项一直都是m个，因为在第m个采样时间后，由于此时已经超出控制范围，在此之后，输入量保持上一时刻不变，因此取差分后为0

整理为矩阵形式，如下：

$\begin{bmatrix} \widehat{Y}_{k+1} & \\ .& \\ .& \\ .& \\ \widehat{Y}_{k+p} & \end{bmatrix} = \begin{bmatrix} {a}_{1}& 0 && ...& 0\\ {a}_{2}&{a}_{1} & & ...&0 \\ .& .& .& & .\\ .& .& & . &. \\ .& .& & &. \\ {a}_{p} & {a}_{p-1} & & ...& {a}_{p-m+1}\\ & & & & \end{bmatrix} \begin{bmatrix}{\Delta u}_{k}\\.\\.\\.\\{\Delta u}_{k+m} \end{bmatrix}+\begin{bmatrix}{Y}^{0}_{k+1} & \\ .& \\ .& \\ .& \\ {Y}^{0}_{k+p} & \end{bmatrix}$

接下来进行滚动优化，优化原则是代价函数的值最小，代价函数分为以下几个部分：

首先，我们希望真实输出与期望轨迹之间的偏差平方和最小，即：

${J}_{1}=\sum_{i=1}^{P}({y}_{k+i}-{\omega}_{k+i})^2$

其中 ${\omega}_{k+i}$ 是预测的轨迹，可以通过真实输出与参考输出表示，即：

$\omega_i=\alpha^i{y}_k+(1-\alpha^i)y_r$

这相当于是一个滤波， $\alpha$ 在[0,1]之间，在i足够大时， $\omega_i$ 趋近于参考值

另外地，我们希望每次输入的偏差之和小一点，以让用户有更好的体验，则有：

$J_2=\sum_{i=1}^{m} \Delta u_{k+i-1}^2$

则整体的代价函数为：

$J=QJ_1+RJ_2$

其中Q、R分别为两项代价的权重

$Q=\begin{bmatrix} q_1 & & & & & \\& q_2 & & & & \\& & .&&&\\&&&.&&\\&&&&.&\\&&&&&q_n \end{bmatrix}$ $R=\begin{bmatrix} r_1 & & & & & \\& r_2 & & & & \\& & .&&&\\&&&.&&\\&&&&.&\\&&&&&r_n \end{bmatrix}$

每个元素可以不一样，这样可以分配各个状态变量内部的权重

写成矩阵形式为：

$J=(r-\omega)^TQ(r-\omega)+\Delta u^TR\Delta u$

一般地，Q和R都是对角矩阵

对 $\Delta u$ 求偏导数，有：

$\frac{\partial J}{\partial \Delta u}=0$

得：

最后进行误差补偿：

假如在k时刻，我们获得了之后p个时刻的预测值： $\widehat{Y}_{k+1},\widehat{Y}_{k+2},...,\widehat{Y}_{k+p}$

在k+1时刻，我们获得了 $y_{k+1}$ 的真实值

则我们可以评估预测的误差：

$e_{k+1}=y{k+1}-\widehat{Y}_{k+1}$

设补偿系数为h，h一般为0.5，这里注意下面的公式没有打错，一直都是用k+1点的误差补偿，所以才需要一个补偿系数，因为其他点的误差不见得等于k+1点的误差

则补偿后，有：

$y_{cor}^{k+1}=\widehat{Y}_{k+1}+h_1*e_{k+1}$

$y_{cor}^{k+2}=\widehat{Y}_{k+2}+h_2*e_{k+1}$

$y_{cor}^{k+p}=\widehat{Y}_{k+p}+h_p*e_{k+1}$

用矩阵表示，为：

$Y_{cor}=\widehat {Y}+H*e_{k+1}$

本质上这是一个状态反馈

则根据滚动预测原则：

$\widehat{Y}_0=S*Y_{cor}$

其中S为移位矩阵（p*p）

可能看到这里有点乱，我其实一开始也乱了，但是这里捋顺一下思路就好了，乱的原因主要是变量太多，难以区分。

总结：整体的思路是这样的：

我们首先对于这个模型的单位脉冲响应已经得知，那么我们可以获得矩阵A，根据卷积定理，我们可以获得输入变化量，输出变化量，输出初态的关系，即：

$\widehat{Y}=A\Delta u +Y_0$

其实根据我个人理解，这个也可以认为是：响应=零输入响应+零状态响应

这个式子只是一个理论基础，用处是告诉我们：只要你知道一个输入变化量，我们就可以预测这个系统的输出

那么这个输入变化量他可不是乱取的，他是有备而来，这就是所谓的优化问题，根据上述推导的公式我们可以得知， $\Delta u$ 取何值时可以得到最优解

由于模型有误差，再加上可能存在扰动之类的问题，因此我们需要加一个反馈校正，在这里选择的是状态反馈，我们的目的是在目前的时间点，进行预测之后，对这个预测进行合理的矫正，给他加个buff，那么这个buff是怎么算出来的呢，我们不是知道当前时间点的真实值吗，并且我们肯定有之前预测的此刻的预测值数据，我们做一个差，此时我们就知道偏差是多少，乘以一个合适的H矩阵，加上原来的预测值，可以让预测更合理。

当然，我们说这是一个滚动预测过程，那么滚动体现在何处呢？我们预测完这个时刻后，一定要来到下一个时刻，那么到达“下一个”时刻(k+2)时，k+1就成了过去时，k+1的预测值又可以用来和k+2真实值作差，评估误差，以此类推，以完成滚动，这时S矩阵便派上用场。

2.基于状态空间的推导

基本的原理和冲激响应模型是一样滴，只不过使用的模型不一样

我们不妨设E=Y-R=X，即用状态变量 $X_k$ 表示

设在k时刻，初始误差为 $x_k$ ，其中 $x_k$ 是一个向量，因为他可能有很多输出

则根据现代控制理论的知识：

若我们设 $x_{k+1}=Ax_k+Bu_k$

则有：

$\quad x_k=x_k$

$x_{k+1}=Ax_k+Bu_k$

$x_{k+2}=Ax_{k+1}+Bu_{k+1}=A^2x_k+ABu_k+Bu_{k+1}$

$x_{k+N}=A^Nx_k+\sum_{i=0}^{N-1}A^iBu_{N-1-i}$

将上面~~一坨公式~~整理为矩阵形式：

$X_k=\begin{bmatrix} I\\ A\\ A^2\\ .\\ .\\ .\\ A^N\\ \end{bmatrix}x_k+\begin{bmatrix} 0 & 0 &. &. & . & 0 & \\ B& & & & & & \\ AB& B & & & & & \\ .& .&& . & & & \\ .& .& & & .& & \\ A^{N-1}B& A^{N-1}B& & & &B& \end{bmatrix} \begin{bmatrix} u_{k}\\ u_{k+1}\\ .\\ .\\ .\\ u_{k+N-1} \end{bmatrix}$

其中，

$X_k$ 为 $x_k$ 到 $x_N$ 向量组合，值得注意的是， $x_k$ 到 $x_N$ 在这里是预测值，即在k点对未来做的预测，我懒得加帽子了。N为预测范围

记

$X_k=Mx_k+CU_k$

对于MIMO系统，由于有多个输入，则u为p维向量，所以B为n×p矩阵，且C矩阵上面应该有n行0，

M：（N+1）n×n C：（N+1）n×Np $X_k$ :(N+1)n×1 $U_k$ :Np×1

对于代价函数：

$J=\sum_{i=0}^{N-1}x_{k+i}^TQx_{k+i}+u_{k+i}^TRu_{k+i}+x_{k+N}^TFx_{k+N}$

其中最后一项是final cost，由于其比过程误差更重要，所以要单独拿出来

整理代价函数：

$J=X_k^T\overline{Q}X_k+U_k^T\overline{R}U_k$

其中：

$\overline{Q}=\begin{bmatrix} Q & & & & & & \\ & Q& & & & & \\ & & Q& & & & \\ & & & .& & & \\ & & & & . & & \\ & & & & & . & \\ & & & & & & F \end{bmatrix}$ $\overline{R}=R$

代入：

$X_k=Mx_k+CU_k$

得：

$J=(Mx_k+CU_k)^T\overline{Q}(Mx_k+CU_k)+U_k^T\overline{R}U_k$

$=x_k^TM^T\overline{Q}Mx_k+U_k^TC^T\overline{Q}Mx_k+x_k^TM^T\overline{Q}CU_k+U_k^TC^T\overline{Q}CU_k+U_k^TRU_k$

由于J是一个标量，因此内部任何一个式子都是标量，同时：

$U_k^TC^T\overline{Q}Mx_k \quad x_k^TM^T\overline{Q}CU_k$ 互相成转置关系，可以合并

$J=x_k^TM^T\overline{Q}Mx_k+2x_k^TM^T\overline{Q}CU_k+U_k^T(C^T\overline{Q}C+R)U_k$

令： $G=M^T\overline{Q}M \quad E=M^T\overline{Q}C \quad H=C^T\overline{R}C+R$

则：

$J=x_k^TGx_k+2x_k^TEU_k+U_k^THU_k$

最后是约束的添加

对于matlab中的quadprog函数，调用方法如下:

[x,fval,exitflag]=quadprog(H,f,A,b,Aeq,beq,lb,ub,x0,options)

$Ax\leq b\\ Aeq*x=beq\\ lb\leq x\leq ub$

应用时，首先对于 $U_k$ 的限制可以用lb和ub表示

对于状态变量，若有：

$MIN\leq X_k\leq MAX$

代入模型：

$MIN \leq Mx_k+CU_k \leq MAX$

$\left\{\begin{matrix} CU_k\leq MAX-Mx_k\\ -CU_k\leq -MIN+Mx_k \end{matrix}\right.$

$\begin{bmatrix} C\\ -C \end{bmatrix}U_k\leq \begin{bmatrix} MAX-Mx_k\\ -MIN+Mx_k \end{bmatrix}$

深入理解：
对代价函数求导后，可以得到一个 $\Delta U$ 和 $x_k$ 的关系，本质上就是得到一个输入和状态空间的关系，则输入和状态有： $u=kx$ 的关系，本质上，这是一个状态反馈。对于完全能控的系统，状态反馈可以任意配置极点，当然可以保证极点在单位圆内，则系统可以镇定。

这里可以进一步定量推导：

设：

$x_{k+1}=Ax_k+B_uu_k+B_dd_k$

其中 $d_k$ 为可测外部干扰变量

两边取微分：

$\Delta x_{k+1}=A\Delta x_k +B_u\Delta u_k+B_d\Delta d_k$

输出：

$y_k=Cx_k=C(\Delta x+x_{k-1})=C\Delta x_k+y_{k-1}^c$

进一步地：

$y_{k+1}^k=C\Delta x_{k+1}^k+y_k=CA\Delta x_k +CB_u\Delta u_k+CB_d\Delta d_k+y_k\\ y_{k+2}^k=(CA^2+CA)\Delta x_k +(CB_u+CAB_u)\Delta u_k+CB\Delta u_{k+1}+(CAB_d+CB_d)\Delta d_k+y_k\\$

$y_{k+p}^k=\sum_{i=1}^{p}CA^i\Delta x_k+\sum_{i=1}^{p}CA^{i-1}B_u\Delta u_k+\sum_{i=1}^{p-1}CA^{i-1}B_u\Delta u_{k+1}+...+\sum_{i=1}^{p-m+1}CA^{i-1}B_u\Delta u_{k+m-1}+\sum_{i=1}^{p}CA^{i-1}B_d\Delta d_k+y_k$

这个推法和前面差不多，不多说了

则有：

$Y_{k+1}^k=S_x\Delta x_k+\iota y_k^c+S_d\Delta d_k+S_u\Delta U_k$

其中：

$S_x=\begin{bmatrix} CA\\\sum_{i=1}^{2}CA^i\\.\\.\\.\\\sum_{i=1}^{p}CA^i\end{bmatrix}$

$I=\begin{bmatrix} I_{ne\times ne}\\I_{ne\times ne}\\.\\.\\.\\I_{ne\times ne}\end{amatrix}$

$S_u=\begin{bmatrix} CB_u & 0 &0 & .&.&. &\quad& 0\\ \sum_{i=1}^{2}CA^iB_u & CB_u & 0&.&.&.&\quad&0\\ .&.&.&.&&&\quad&.\\.&.&.&&.&&\quad&.\\.&.&.&&&.&\quad&.\\ \sum_{i=1}^{m}CA^{i-1}B_u &\sum_{i=1}^{m-1}CA^{i-1}B_u &...&.&.&.&\quad&CB_u\\ .&.&.&.&&&\quad&.\\.&.&.&&.&&\quad&.\\.&.&.&&&.&\quad&.\\ \sum_{i=1}^{p}CA^{i-1}B_u &\sum_{i=1}^{p-1}CA^{i-1}B_u &...&.&.&.&\quad&\sum_{i=1 }^{p-m+1}CA^{i-1}B_u \end{bmatrix}$

$S_d=\begin{bmatrix} CB_d\\ \sum_{i=1}^{2}CA^{i-1}B_d\\ .\\ .\\ .\\ \sum_{i=1}^{p}CA^{i-1}B_d \end{bmatrix}$

设参考输入为 $R_{k+1}$

定义：

$\rho=\begin{bmatrix} \overline{Q}(Y_{k+1}^k-R_{k+1}) \\\overline R \Delta U_k \end{bmatrix}$

代入 $Y_{k+1}^k=S_x\Delta x_k+\iota y_k^c+S_d\Delta d_k+S_u\Delta U_k$

$\rho =\begin{bmatrix} \overline{Q}S_u \\\overline{R} \end{bmatrix}\Delta U_k-\begin{bmatrix} \overline{Q}(R_{k+1}-S_x\Delta x_k-\iota y_k^c-S_d\Delta d_k )\\ 0 \end{bmatrix}=Kz-d$

令 $E_{k+1}^k=\overline{Q}(R_{k+1}-S_x\Delta x_k-\iota y_k^c-S_d\Delta d_k$

则：

$J=\rho^T\rho=(Kz-d)^T(Kz-d)$

$\frac{\partial J}{\partial z}=\frac{\partial (z^TK^TKz-2d^TKz+d^Td)}{\partial z}=2K^TKz-2d^TK=0$

极值为：

$z=(K^TK)^{-1}K^Td$

$\Delta U_k=(S_u^T\overline{Q}^T\overline{Q}S_u+\overline{R}^T\overline{R})^{-1}S_u^T\overline{Q}^T\overline{Q}E_{k+1}$

这里计算时注意，由于J是个标量，因此其中相加的几个量都是标量，则：

$d^TKz=z^TK^Td$

我们只需要第一个元素，即：

$\Delta u_k=\begin{bmatrix} I_{nu\times nu} &0 &. &. &. & 0 \end{bmatrix}\Delta U_k$

不妨设：

$K_{mpc}=\begin{bmatrix} I_{nu\times nu} &0 &. &. &. & 0 \end{bmatrix}(S_u^T\overline{Q}^T\overline{Q}S_u+\overline{R}^T\overline{R})^{-1}S_u^T\overline{Q}^T\overline{Q}$

$\Delta u_k=K_{mpc}E_{k+1}=K_{mpc}\overline{Q}(R_{k+1}-S_x\Delta x_k-\iota y_k^c-S_d\Delta d_k)$

即：

$K_{mpc}R_{k+1}$ 是对于未来参考输入的前馈补偿

$-K_{mpc}S_D\Delta d_k$ 是对于可预测扰动的前馈补偿

$-K_{mpc}(S_x+\iota C)\Delta x_k-K_{mpc}\iota y_{k-1}^c$ 是状态反馈

四、仿真实例

1.SISO系统

设系统状态空间方程为：

$\dot {x}=\begin{bmatrix} 1 &0.1 \\ 0&2 \end{bmatrix}x+\begin{bmatrix} 0\\ 0.5 \end{bmatrix}u$

$\begin{bmatrix} \dot{x_1}^0\\ \dot{x_2}^0 \end{bmatrix}=\begin{bmatrix} 20\\ -20 \end{bmatrix}$

对于该系统，零输入响应如图：

其实从系统A矩阵可以看出，该系统特征值在单位圆外面，所以肯定是发散的

对于单输入系统，加入MPC后，结果如下：

可见加入MPC控制后，系统收敛了，MPC的确起到了状态反馈的作用，使得新的闭环系统的极点处于单位圆内。

如果提高 $x_1$ 的权重，效果如下：

如果提高输入的权重，结果如下：

显然状态空间衰减变慢

如果期望值不是0，如期望 $x_1$ 终值为70，结果如下：

改变目标值的本质原理如下：

一般情况下，对于状态空间 $\dot x =Ax+Bu$ ，如果A的特征值在单位圆内（离散状态空间），那么该系统必能收敛至0

设期望值为 $x_{exp}$ ，那么偏差值： $x_d=x-x_{exp}$

两边取微分： $\dot x_d=\dot x=Ax+Bu$

因此偏差也可以衰减至0，且和原状态空间的A矩阵相同，因此可以套用原状态空间的控制方法

下图来自现代控制理论

综上，SISO系统MPC控制代码如下：

mpc_sim.m

clear;
clc;
A=[1 0.1;0 2];
B=[0;0.5];
%x=Ax0+Bu
Q=[2 0;0 1];%weight of error cost function
F=[1 0;0 1];%weight of error cost function(last step)
R=[0.1]; %weight of input cost function



n=size(A,1);%the dimension of the state space
p=size(B,2);%the dimension of input vector

k_step=100;%steps of my state space 

X_k=zeros(n,k_step);%set state space vector
X_d=zeros(n,k_step);



X_k(:,1)=[20;-20];%initialization of the state space 

U_k=zeros(p,k_step);%set input vector 

N=5; %prediction horizon

basic = eye(n);
[E,H]=Cal_matrix(A,B,Q,R,F,N);
X_expect=[70;0];








for k = 1:k_step
    X_d(:,k)=X_k(:,k)-X_expect;
    U_k(:,k)=Prediction(X_d(:,k),E,H,N,p);%with k adding,the initial value of
    %X_k also changes,so this is what is called recursive optimization 
    
    X_d(:,k+1)=A*X_d(:,k)+B*U_k(:,k);%recursion of state space
    X_k(:,k+1)=X_d(:,k+1)+X_expect;
end 


subplot(2,1,1);

for i= 1 :size(X_k,1)
    plot(X_k(i,:));
    hold on
end 
legend("x1 x2");
hold on;


subplot(2,1,2);



for i =1:size(U_k,1)
    plot (U_k(i,:));
end

legend("u");

Cal_matrix.m

function [E,H]=Cal_matrix(A,B,Q,R,F,N)

n=size(A,1);
p=size(B,2);


M=[eye(n);zeros(N*n,n)];
C=zeros((N+1)*n,N*p);



temp=eye(n);


for i =  1:N
    rows=i*n+(1:n); %this mean from which lines should we start at this step,
    %for each matrix has not a single row,we use 'i*n',it starts from n+1
    C(rows,:)=[temp*B,C(rows-n,1:end-p)]%"rows-n,1:end-p"means using the 
    %elements of last step,end -p equals to let out a place for 'temp*B'
    temp=A*temp;
    M(rows,:)=temp;
end

Q_bar=kron(eye(N),Q);%in order to make a diagonal matrix with Q
% as its diagonal elements

Q_bar=blkdiag(Q_bar,F);
R_bar=kron(eye(N),R);


G=M'*Q_bar*M;
E=C'*Q_bar*M;
H=C'*Q_bar*C+R_bar;
end

prediction.m

function u_k=Prediction(x_k,E,H,N,p)
U_k=zeros(N*p,1);
U_k=quadprog(H,E*x_k);
u_k=U_k(1:p,1);%only use the first step
end

另外，令该系统跟踪圆形轨迹，结果如下:

主函数对应如下：

clear;
clc;
A=[0.3 0;0 1];
B=[0;0.5];
%x=Ax0+Bu
Q=[1 0;0 1];%weight of error cost function
F=[1 0;0 1];%weight of error cost function(last step)
R=[0.1]; %weight of input cost function



n=size(A,1);%the dimension of the state space
p=size(B,2);%the dimension of input vector

k_step=1000;%steps of my state space 


X_k=zeros(n,k_step);%set state space vector
X_d=zeros(n,k_step);




X_k(:,1)=[20;-20];%initialization of the state space 

U_k=zeros(p,k_step);%set input vector 

N=5; %prediction horizon

basic = eye(n);
[E,H]=Cal_matrix(A,B,Q,R,F,N);
X_expect=zeros(n,k_step);








for k = 1:k_step
    X_expect=[10*cos(360/k_step*k)+10;10*sin(360/k_step*k)-5];
    X_d(:,k)=X_k(:,k)-X_expect;
    U_k(:,k)=Prediction(X_d(:,k),E,H,N,p);%with k adding,the initial value of
    %X_k also changes,so this is what is called recursive optimization 
    
    X_d(:,k+1)=A*X_d(:,k)+B*U_k(:,k);%recursion of state space
    X_k(:,k+1)=X_d(:,k+1)+X_expect;
end 

figure(1)
subplot(2,1,1);

for i= 1 :size(X_k,1)
    plot(X_k(i,:));
    hold on
end 
legend("x1 x2");
hold on;


subplot(2,1,2);



for i =1:size(U_k,1)
    plot (U_k(i,:));
end

legend("u");
hold on;
figure(2);

plot(X_k(1,:),X_k(2,:),'r')

hold on;

X_exp_vec=zeros(n,k_step);
for k = 1:k_step
    X_exp_vec(:,k)=[10*cos(360/k_step*k)+10;10*sin(360/k_step*k)-5];
end
plot(X_exp_vec(1,:),X_exp_vec(2,:),'g')
legend("track","reference");

2.MIMO系统

对于MIMO系统：

$\dot x=\begin{bmatrix} 1 &0.1 \\ -1& 2 \end{bmatrix} x+\begin{bmatrix} 0.2 & 1\\ 0.5& 2 \end{bmatrix} u$

仿真代码如下：

clear;
clc;
A=[1 0.1;-1 2];
B=[0.2 1;0.5 2];
%x=Ax0+Bu
Q=[1500 0;0 1500];%weight of error cost function
F=[1 0;0 1];%weight of error cost function(last step)
R=[0.1 0;0 0.1]; %weight of input cost function
lb=[-200;-200];
ub=[200;200];
MIN=[-20;-20;-20;-20;-20;-20;-20;-20;-20;-20;-20;-20];
MAX=[20;20;20;20;20;20;20;20;20;20;20;20];






n=size(A,1);%the dimension of the state space
p=size(B,2);%the dimension of input vector

k_step=1000;%steps of my state space 


X_k=zeros(n,k_step);%set state space vector
X_d=zeros(n,k_step);




X_k(:,1)=[20;-20];%initialization of the state space 

U_k=zeros(p,k_step);%set input vector 

N=5; %prediction horizon

basic = eye(n);
[E,H,M,C]=Cal_matrix(A,B,Q,R,F,N);

A_k=[C;-C];
b_k=[MAX-M*X_k(:,1);-MIN+M*X_k(:,1)];



X_expect=zeros(n,k_step);








for k = 1:k_step
    X_expect=[10*cos(360/k_step*k)+10;10*sin(360/k_step*k)-5];
    X_d(:,k)=X_k(:,k)-X_expect;
    U_k(:,k)=Prediction(X_d(:,k),E,H,N,p,A_k,b_k,lb,ub);%with k adding,the initial value of
    %X_k also changes,so this is what is called recursive optimization 
    
    X_d(:,k+1)=A*X_d(:,k)+B*U_k(:,k);%recursion of state space
    X_k(:,k+1)=X_d(:,k+1)+X_expect;
end 

figure(1)
subplot(2,1,1);

for i= 1 :size(X_k,1)
    plot(X_k(i,:));
    hold on
end 
legend("x1 x2");
hold on;


subplot(2,1,2);



for i =1:size(U_k,1)
    plot (U_k(i,:));
    hold on 
end

legend("u");
hold on;
figure(2);


X_exp_vec=zeros(n,k_step);
for k = 1:k_step
    X_exp_vec(:,k)=[10*cos(360/k_step*k)+10;10*sin(360/k_step*k)-5];
end
plot(X_exp_vec(1,:),X_exp_vec(2,:),'g')
hold on;

plot(X_k(1,:),X_k(2,:),'r')
legend("track","reference");

该代码在原基础上加入了约束条件，如果代码运行不了可以将约束去掉，仿真结果如下，其中 $x_1$ $x_2$ 权重提高可以提高跟踪的效果

3.基于simulink的仿真

在simulink中使用mpc controler搭建框图如下：

搭建教程：【Model Predictive Control】了解模型预测控制，第六部分：如何使用 Simulink 和模型预测控制工具箱设计 MPC 控制器_哔哩哔哩_bilibili

给定约束：

① 方向盘物理转角不得超过30°

② 为了保证舒适度，方向盘变化率不得超过15°/s

在此条件下，仿真结果如图：

在预测范围为10，控制范围为2时，结果如下：

如果将prediction horizon变大，结果如下：

超调此时减小了，但是预测范围太大可能会造成算力的浪费

将控制范围变大，可见跟踪效果更好，跟踪曲线更接近reference

然而，进一步提高控制范围，发现结果变化并不大，所以我们一般取预测范围的20%-30%就可以了

当速度提升至35时，结果如下：

可见跟踪效果并不好，所以我们最好添加自适应MPC（adaptive MPC）

4.车辆横向运动状态空间模型的推导

这里就必须要提及著名的自行车模型

自行车模型的条件如下：
① 车辆做平面运动

② 左右前轮的转向角近似相等

③ 忽略轮胎收到的侧向力

④ 忽略前后轴荷载的转移

⑤ 车身和悬架系统是刚性的

其中② ③限制了车体必须是低速运动的

我们选择状态变量：

x,y:车辆在绝对坐标系的位置

v：车辆的速度大小

$\psi$ : 车辆的转角大小

假如这是一辆车，则有：

$\delta f$ :前轮与车辆纵轴之间的夹角

$\delta r$ :后轮与车辆纵轴之间的夹角（如果后轮不能转就是0）

$\beta$ :质心侧偏角，即车辆速度与纵轴之间的偏角，低速行驶时认为特别小

A B为前后轮的速度质心，C为车体整体质心，下面以C为原点推导自行车模型，当然，我们可以以A，B为原点推导，过程相似，结果略有差异

设PC=R

则有：

$\Delta PCA : \frac{sin(\delta f -\beta)}{l_f}=\frac{sin(\frac{\pi}{2}-\delta f)}{R}$

$\Delta PCB : \frac{sin( \beta-\delta r)}{l_r}=\frac{sin(\frac{\pi}{2}+\delta f)}{R}$

整理得：

$tan(\delta f cos \beta )-sin(\beta)=\frac{cos(\delta f )}{R}$

$sin(\beta)-tan(\delta_r cos \beta )=\frac{cos(l_r )}{R}$

联立以上两式，同时在低速时，我们假设R变化缓慢，那么:

$\dot{\psi}=\frac{v}{R}$

与以上两式联立，得：

$\dot {\psi}= \frac {vcos(\beta)}{l_f+l_r}(tan\delta_f - tan \delta _r)$

在质心处：

$\dot x _c=vcos(\psi +\beta)\\$

$\dot y_c =vsin(\psi +\beta)$

$\dot v =a$

此时反解 $\beta$ :

$\beta = tan^{-1}(\frac{l_ftan \delta_r +l_r tan \delta_f}{l_f+l_r})$

在高速情况下，外力对于车辆的影响是比较大的，所以我们不光要考虑运动学模型，更要考虑动力学模型

在低速时，我们通常认为轮胎的朝向就是车的运动方向，但是在高速行驶时并不是这样，因为轮胎存在侧偏特性，即车轮的速度方向并不是轮胎的朝向

对于横向动力学模型，有以下假设：

① 忽略悬架运动、道路倾斜度、空气动力学等非线性效应

② 忽略轮胎力的纵横向耦合关系

以车体质心为原点，车体纵轴为x轴建立坐标系如下：

对于侧向，有：

$ma_y=F_{yf}+F_{yr}$

而侧向加速度包括运动加速度和向心加速度

$a_y=\ddot{y}+v_x \dot {\psi}$

代入：

$m(\ddot y + v_x \dot \psi)=F_{yf}+F_{yr}$

对于横摆运动（即绕z轴的转动）：

$I_z \ddot \psi =l_f F_{yf}-l_rF_{yr}$

对车轮进行受力分析，车轮受侧向力（风吹，提供离心力的摩擦）和纵向力（摩擦，牵引），二者可以合成为车体的横向力

$F_{yr}=F_{cr}$

$F_{yf}=F_{cf}cos(\delta_f)+F_{lf}sin(\delta_f)$

$\delta _f$ 很小，所以可以忽略

侧偏力和侧偏角关系如图：

在侧偏角较小时，可以认为： $F_c=C\alpha$

其中C为侧偏刚度

$\alpha_f=\delta _f -\theta_{vf}$

$\alpha_r = -\theta_{vr}$

因此：

$F_{yf}=2C_{af}\alpha_f=2C_{af}(\delta_f-\theta_{vf})$

$F_{yr}=2C_{af}\alpha_r=2C_{ar}(-\theta_{vr})$

对前后轮进行速度分解，得：

$v_{fx}=v{x} \quad v_{lx}=v{x} \quad v_{fy}=\dot y+l_f \dot\psi \quad v_{ly}=\dot y - l_f \dot \psi$

$tan(\theta_{vf})\approx \theta_{vf}=\frac{v_{fy}}{v_{fx}}=\frac{\dot y+l_{f}\dot \psi}{v_x}$

$tan(\theta_{vr})\approx \theta_{vr} =\frac{v_{ly}}{v_{lx}}=\frac{\dot y -l_r \dot \psi}{v_x}$

代入侧向力表达式：

$F_{vy}=2C_{af}(\delta_f -\frac{\dot y + l_f\dot \psi}{v_x})$

$F_{yr}=2C_{ar}(-\frac{\dot y -l_r \dot \psi}{v_x})$

代入横向动力学方程，并整理成矩阵形式

令：

$X=\begin{bmatrix} y\\ \dot y\\ \psi\\ \dot \psi \end{bmatrix}$ 为状态向量

$\delta _f$ 为控制量

$l_f \quad l_r\quad C_{af} \quad C_{ar} \quad m \quad I_z \quad v_x$ 为已知参数

得：

$\frac{\mathrm{d} }{\mathrm{d} t}\begin{bmatrix} y\\ \dot y\\ \psi\\ \dot \psi \end{bmatrix}=\begin{bmatrix} 0 &1 &0 &0 \\ 0&- \frac{2C_{af}+2C_{ar}}{mv_x} & 0 & -v_x-\frac{2C_{af}l_f-2C_{ar}l_r}{mv_x}\\ 0& 0& 0 &1 \\ 0& -\frac{2C_{af}l_f-2C_{ar}l_r}{I_zv_x} & 0 & -\frac{2C_{af}l_f^2+2C_{ar}l_r^2}{I_zv_x} \end{bmatrix}\begin{bmatrix} y\\ \dot y\\ \psi\\ \dot \psi \end{bmatrix}+\begin{bmatrix} 0\\ \frac{2C_{af}}{m}\\ 0\\ \frac{2l_fC_{af}}{I_z} \end{bmatrix} \delta_f$