算法详解+代码（无人驾驶车辆模型预测控制第四章）

Auerbach°

已于 2023-10-13 17:34:01 修改

阅读量3k

点赞数 29

文章标签：算法

于 2023-10-13 17:29:12 首次发布

本文链接：https://blog.csdn.net/qq_67958647/article/details/133760598

版权

一.MPC模型预测控制理论推导

首先对汽车运动学模型进行建模：

设后轴速度为 $\left ( \dot{X_{r}},\dot{Y_{r}} \right )$ ,前轴行速度为 $\left ( \dot{X_{f}},\dot{Y_{f}} \right )$ 考虑后轴轴心的速度约束，得到 $\left(\dot{X_{r}},\dot{Y_{r}} \right )$ 和后轴速度 $v_{r}$ 的关系如下：

$v_{r}=\dot{X_{r}}\cos\varphi+\dot{Y_r}\sin\varphi$

考虑到前后轴速度约束的运动学方程为：

$\left\{ \begin{aligned} &\dot{X_{r}}\sin (\delta _f +\varphi )-\dot{Y_{r}}\cos(\delta_{f}+\varphi)=0 \\ &\dot{X}_r\sin(\varphi)-\dot{Y}_{r}\cos{\varphi}=0\\ \end{aligned} \right.$

设后轴行驶中心为 $\left ( X_{r},Y_{r}\right )$ ，前轴行驶中心为 $\left ( X_{f},Y_{f} \right )$ ，根据前后轴之间的几何约束关系可得到:

$\left\{ \begin{aligned} &X_{f}=X_{r}+L\cos\varphi\\ &Y_{f}=Y_{r}+L\sin\varphi \end{aligned} \right.$

将上述三个公式联立可得到横摆角速度 $\omega$ 的表达式为：

$\dot{\varphi}=\omega =\boldsymbol{}\frac{V\mathbf{}_{r}}{L\tan{\delta_{f}}}$

由横摆角速度和车速 $v_{r}$ 可得到车辆的转向半径和车轮偏角 $\delta_{f}$ :

$\left\{ \begin{aligned} &R=\frac{v_{r}}{\omega }\\ &\delta_{f}=\arctan(\frac{L}{R}) \end{aligned} \right.$

即可得到矩阵形式的汽车运动学的表达方程：

$\begin{pmatrix} \dot{X_{r}}\\ \dot{Y_{r}}\\ \dot{\varphi} \end{pmatrix}= \begin{pmatrix} \cos\varphi\\ \sin\varphi\\ \tan\frac{\delta_{f} }{L} \end{pmatrix}v_{r}$

由此方程可知，这个动力学系统可以看成是由两个输入 $\left ( v,\delta_{f} \right )$ ,三个输出 $\left ( X,Y,\varphi \right )$ 构成的控制系统，抽象成函数为：

$\dot{X}=f(X,u)$

设我们期望的参考轨迹点为 $X_{ref}$ ,在参考点附近对该抽象映射函数进行泰勒展开以线性化，并且忽略高阶项：

$\dot{X}=f(X,u)+\frac{\partial f(X,u)}{\partial X}\left | (X-X_{ref})+\frac{\partial f(X,u)}{\partial u}(u-u_{ref}) \right |$

考虑到最终我们控制的是误差的增量，设 $\dot{\tilde{X}}$ 为误差：

$\large \dot{\tilde{X}}=\begin{pmatrix} \dot{X}-\dot{X_{ref}}\\ \dot{Y}-\dot{Y_{ref}}\\ \dot{\varphi}-\dot{\varphi_{ref}} \end{pmatrix}$

整理计算后得到：

$\large \dot{\tilde{X}}(k+1)=A_{k}{\tilde{X}}(k)+B_{k}\tilde{u}(k)$

$\large A_{k}=\begin{pmatrix} 1 & 0& -v_{ref}\sin\varphi_{ref}\\ 0 & 1 & v_{ref}\cos\varphi_{ref}\\ 0& 0& 1 \end{pmatrix}$

$\large B_{k}=\begin{pmatrix} \cos\varphi_{ref} &0 \\ \sin_{ref}&0 \\ \frac{tan\delta_{f}}{L}& \frac{v_{ref}}{L{\cos^{2}\delta_{ref}}} \end{pmatrix}$

MPC的基本原理，就是在每个计算时域内，求解一个二次规划问题，并且以求解得到的二次规划问题的最优解作为下一个步长的输入，从而对未来多个时间步长内的状态量进行预测控制。在无人驾驶汽车中，目标函数要保证能够快速且平稳的跟踪期望轨迹，就要求我们对系统状态量做出优化。此处我们对状态量进行优化控制，以便快速平稳的跟踪轨迹。

代价函数（或优化函数）如下：

$\large J(k)=\sum_{j=1}^{N}\tilde{X}^T(k+j|k)Q\tilde{X}(k+j)+\tilde{u}^T(k+j-1)R\tilde{u}(k+j-1)$

其中，Q，R为权重矩阵，反应了我们对状态量的侧重程度，等号左边的第一项是我们要控制的轨迹跟踪效果，反映了无人车对轨迹的跟随能力，第二项为对输出变化的约束。输出的u为前轮转角和速度。

由于我们最终控制的是误差的增量，对上述方程进行变形得到：

$\large J(k)=\sum_{j=1}^{N}||\eta (k+j|t)-\eta_{ref}(k+j|t)||^2+||\Delta U(k+j|k)||^2+\rho \varepsilon ^{2}$

其中 $\large \varepsilon$ 为松弛因子， $\large \rho$ 为松弛因子系数

MPC具体迭代过程如下：

为方便后续的理论分析和计算，将状态变量和输出的增量矩阵写成一个矩阵：

$\large \xi (k|t)=\begin{pmatrix} {\tilde{X}}(k|t)\\ {\tilde{u}}(k-1 |t) \end{pmatrix}$

由前面状态量的迭代关系可得到：

$\large \begin{aligned} \xi (k+1|t)&=\begin{pmatrix} \dot{\tilde{X}}(k+1|t)\\ {\tilde{u}}(k |t) \end{pmatrix}=\begin{pmatrix}A_{k}{\tilde{X}}(k|t)+B_{k}\tilde{u}(k|t) \\ {\tilde{u}}(k-1 |t)+ \Delta {\tilde{u}}(k |t)\end{pmatrix}\\ \\\hspace*{\fill}\\&=\begin{pmatrix}A_{k}{\tilde{X}}(k|t)+B_{k}\tilde{u}(k|t) +B_{k}\tilde{u}(k-1|t)\\ {\tilde{u}}(k-1 |t)+ \Delta {\tilde{u}}(k |t)\end{pmatrix}\\ \\\hspace*{\fill}\\&=\begin{pmatrix} A_{k}&B_{k} \\ 0& I_{{u}}\end{pmatrix} \begin{pmatrix} \dot{\tilde{X}}(k|t)\\ {\tilde{u}}(k-1|t) \end{pmatrix}+\begin{pmatrix} B_{k}\\ I_{u} \end{pmatrix}\Delta {\tilde{u}}(k |t) \end{aligned}$

化简得到新的状态空间表达式：

$\large \xi (k+1|t)=A\xi (k|t)+B\Delta {\tilde{u}}(k |t)$

$\large \eta(k|t)=C\xi (k|t)=\begin{pmatrix} I_{x}& 0 \end{pmatrix}\xi (k|t)$

其中 $\large I_{u}$ 为 $\large N_{u}\times N_{u}$ 的单位阵， $\large I_{x}$ 为 $\large N_{x}\times N_{x}$ 单位阵，A为 $\large (N_{x}+N_{u},N_{x}+N_{u})$ 的矩阵,B为 $\large (N_{x}+N_u,N_{u})$ 的矩阵， $\large N_{x}$ 为状态向量矩阵的维数， $\large N_{u}$ 为输出向量的维数。

MPC滚动预测计算结果和流程如下,其中 $\large N_{p}$ 预测时域， $\large N_{c}$ 为控制时域，在我们的计算过程中 $\large N_{p}$ 取60， $\large N_{c}$ 取30，状态量计算过程：

$\large \begin{cases}\xi (k+1|t)=A\xi (k|t)+B\Delta {\tilde{u}}(k |t) \\ \xi (k+2|t)= A\xi (k+1|t)+B\Delta {\tilde{u}}(k+1 |t) =A^{2}\xi(k|t)+AB\Delta {\tilde{u}}(k |t) +A^{0}B\Delta {\tilde{u}}(k+1 |t) \\ \xi (k+3|t)=A\xi (k+2|t)+B\Delta {\tilde{u}}(k +2|t) =A^{3}\xi(k|t)+A^{2}B\Delta {\tilde{u}}(k |t) +A^{1}B\Delta {\tilde{u}}(k+1 |t)+ A^{0}B\Delta {\tilde{u}}(k+2 |t) \\ \dots.\dots.\\ \xi (k+N_{c}|t)=A\xi (k+N_{c}-1|t)+B\Delta {\tilde{u}}(k+N_{c}-1|t) =A^{N_{c}}\xi(k)+A^{N_{c}-1}\Delta {\tilde{u}}(k |t)+...+A^{0}\Delta {\tilde{u}}(k+N_{c}-1 |t)\\ \dots.\dots.\\ \xi (k+N_{p}|t)=A\xi (k+N_{p}-1|t)+B\Delta {\tilde{u}}(k+N_{p}-1|t) =A^{N_{p}}\xi(k)+A^{N_{p}-1}\Delta {\tilde{u}}(k |t)+...+A^{0}\Delta {\tilde{u}}(k+N_{p}-1 |t)\\ \end{cases}$

输出的计算过程：

$\large \left\{\begin{matrix} \begin{aligned}&\eta(k+1|t)=CA\xi(k|t)+CA^0B_\Delta\tilde{u}(k|t)\\ &\eta(k+2|t)=CA^2\xi(k|t)+CAB_\Delta\tilde{u}(k|t)+CA^0B_\Delta\tilde{u}(k+1|t)\\ &\eta(k+3|t)=CA^3\xi(k|t)+CA^2B_\Delta\tilde{u}(k|t)+CAB_\Delta\tilde{u}(k+1|t)+CA^0B_\Delta\tilde{u}(k+2|t)\\ &\dots.\dots.\\ &\eta(k+N_c|t)=CA^{N_c}\xi(k|t)+CA^{N_c-1}B_\Delta\tilde{u}(k|t)+CA^{N_c-2}B_\Delta\tilde{u}(k+1|t)+\cdots+CA^0B_\Delta\tilde{u}(k+N_c-1|t)\\ &\dots.\dots.\\ &\eta(k+N_p|t)=CA^{N_p}\xi(k|t)+CA^{N_p-1}B_\Delta\tilde{u}(k|t)+CA^{N_p-2}B_\Delta\tilde{u}(k+1|t)+\cdots+CA^0B_\Delta\tilde{u}(k+N_p-1|t) \end{aligned}\end{matrix}\right.$

由上述方程可得到：

$\large \left.Y=\Psi\xi(k|t)+\Theta_\Delta U\\\\=\left[\begin{array}{c}\eta(k+1|t)\\\eta(k+2|t)\\\eta(k+3|t)\\\vdots\\\eta(k+N_c|t)\\\vdots\\\eta(k+N_p|t)\end{array}\right.\right] \\\hspace*{\fill}\\ \\\hspace*{\fill}\\ \\\hspace*{\fill}\\ \\\quad\Psi=\left[\begin{array}{c}CA\\CA^2\\CA^3\\\vdots\\CA^{N_c}\\\vdots\\CA^{N_c}\\\vdots\\GA^{N_p}\end{array}\right]\hspace*{\fill}\quad\xi(k|t)=\begin{bmatrix}\tilde{\chi}(k|t)\\\tilde{u}(k-1|t)\end{bmatrix}$

$\large \Theta=\begin{bmatrix}CB&0&0&\cdots&0\\CAB&CB&0&\cdots&0\\CA^2B&CAB&CB&\cdots&0\\\vdots&\vdots&\vdots&\ddots&\vdots\\CA^{N_c-1}B&CA^{N_c-2}B&CA^{N_c-3}B&\cdots&CA^0B\\\vdots&\vdots&\vdots&&\vdots\\CA^{N_p-1}B&CA^{N_p-2}B&CA^{N_p-3}B&\cdots&CA^{N_p-N_c}B\end{bmatrix}\\ \\ \hspace*{\fill} \\ \\ \hspace*{\fill} \\ \Delta U=\begin{bmatrix}\Delta\tilde{u}(k|t)\\\Delta\tilde{u}(k+1|t)\\\Delta\tilde{u}(k+2|t)\\\vdots\\\Delta\tilde{u}(k+N_c-1|t)\end{bmatrix}$

因为目标函数为 $\large J(k)=\sum_{j=1}^{N}||\eta (k+j|t)-\eta_{ref}(k+j|t)||^2+||\Delta U(k+j|k)||^2+\rho \varepsilon ^{2}$

其中 $\large ||\eta (k+j|t)-\eta_{ref}(k+j|t)||$ 这一项是输出的增量，也就是 $\large ||Y-Y_{ref}||$ ,令 $\large \Psi\xi(k|t)$ 为矩阵 $\large E$ ，得到：

$\large \begin{aligned} \text{J}& =[Y-Y_{ref}]^{\mathrm{T}}Q[Y-Y_{ref}]+\Delta U^{\mathrm{T}}R\Delta U+\rho\varepsilon^{2} \\ &=[E+\Theta_{\Delta}U-Y_{ref}]^{\mathrm{T}}\mathrm{Q}[E+\Theta_{\Delta}U-Y_{ref}]+\Delta\mathrm{U}^{\mathrm{T}}R\Delta\mathrm{U}+\rho\varepsilon^{2} \\ &=[E^TQ+\Delta U^T\Theta^TU-Y_{ref}^TQ][E+\Theta_\Delta U-Y_{ref}]+\Delta U^\text{T}R\Delta U+\rho\varepsilon^2 \\ \end{aligned}$

忽略不影响最优结果的常数项 $\large {E^TQE-2Y_{ref}^TQE+Y_{ref}^TQY_{ref}}$ ，化简得到：

$\large \text{J}=\Delta U^T(\Theta^TQE+R)\Delta U+2(E^TQ\Theta-Y_{ref}^TQ\Theta)\Delta U+\rho\varepsilon^2$

将优化函数配凑成标准二次规划函数得到：

$\large \text{J}=\begin{bmatrix}\Delta U^T&\varepsilon\end{bmatrix}\begin{bmatrix}\Theta^TQ\Theta&0\\0&\rho\end{bmatrix}\begin{bmatrix}\Delta U\\\varepsilon\end{bmatrix}+\begin{bmatrix}2(E^TQ\Theta-Y_{ref}^TQ\Theta)&0\end{bmatrix}\begin{bmatrix}\Delta U\\\varepsilon\end{bmatrix}$

将代价函数表示成标准的二次规划函数，在优化求解时的代价函数为：

$\large \text{J}=2\left[\left(\frac{1}{2}\Delta U^{T}HU\right)+g^{T}X\right]$

$\large H=\begin{bmatrix}\Theta^TQ\Theta&0\\0&\rho\end{bmatrix}$

$\large g^T=\begin{bmatrix}(E^TQ\Theta-Y_{ref}^TQ\Theta)&0\end{bmatrix}$

在有了代价函数之后，我们还要知道约束条件，MPC的约束条件有两个，分别为上下限约束和增量约束，上下限约束如下：

$\large \tilde{u}_{Min}\leqslant\ {\tilde{u}( k+1|t)}\leqslant \tilde{u}_{Max}\\ \tilde{u}_{Min}\leqslant\ {\tilde{u}( k+2|t)}\leqslant \tilde{u}_{Max}\\ \tilde{u}_{Min}\leqslant\ {\tilde{u}( k+3|t)}\leqslant \tilde{u}_{Max}\\ \tilde{u}_{Min}\leqslant\ {\tilde{u}( k+4|t)}\leqslant \tilde{u}_{Max}\\ \vdots\\ \tilde{u}_{Min}\leqslant\ {\tilde{u}( k+N_c-1|t)}\leqslant \tilde{u}_{Max}\\$

逐项计算他们的增量关系得到：

$\large \tilde{u}(k|t)=\tilde{u}(k-1|t)+\Delta\tilde{u}(k|t)\\ \tilde{u}(k+1|t)=\tilde{u}(k)+\Delta\tilde{u}(k+1|t)=\tilde{u}(k-1|t)+\Delta\tilde{u}(k|t)+\Delta\tilde{u}(k+1|t)\\ \vdots \\u(k+N_c-1|t)=\tilde{u}(k+N_c-2|t)+\Delta \tilde{u}(k+N_c-1|t)=\tilde{u}(k-1|t)+\Delta\tilde{u}(k|t)+\Delta\tilde{u}(k+1|t)+\cdots+\tilde{u}(k+N_c-1|t)$

令：

$\large U=\begin{bmatrix}\tilde{u}(k|t)\\\tilde{u}(k+1|t)\\\vdots\\\tilde{u}(k+N_c-1|t)\end{bmatrix},U_t=\begin{bmatrix}\tilde{u}(k-1|t)\\\tilde{u}(k-1|t)\\\vdots\\\tilde{u}(k-1|t)\end{bmatrix}\\\\\hspace*{\fill}\\\Delta U=\begin{bmatrix}\Delta\tilde{u}(k|t)\\\Delta\tilde{u}(k+1|t)\\\vdots\\\Delta\tilde{u}(k+N_c-1|t)\end{bmatrix}$

$\large \begin{bmatrix}\tilde{u}_{\min}\\\tilde{u}_{\min}\\\vdots\\\tilde{u}_{\min}\end{bmatrix}\leq U=\begin{bmatrix}\tilde{u}(k|t)\\\tilde{u}(k+1|t)\\\vdots\\\tilde{u}(k+N_{c}-1|t)\end{bmatrix}\leq\begin{bmatrix}\tilde{u}_{\max}\\\tilde{u}_{\max}\\\vdots\\\tilde{u}_{\max}\end{bmatrix}$

$\large A_t=\begin{bmatrix}1&0&0&\cdots&0\\1&1&0&\cdots&0\\\vdots&\vdots&\vdots&\ddots&\vdots\\1&1&1&\cdots&1\end{bmatrix},U_\mathrm{min}=\begin{bmatrix}\tilde{u}_\mathrm{min}\\\tilde{u}_\mathrm{min}\\\vdots\\\tilde{u}_\mathrm{min}\end{bmatrix},=U_{\max}=\begin{bmatrix}\tilde{u}_{\max}\\\tilde{u}_{\max}\\\vdots\\\tilde{u}_{\max}\end{bmatrix}$

则最终得到的约束条件为：

$\large U_{\min}\leq U_t+A_I\Delta U\leq U_{\max}$

增量约束的思想为，我们取每一次的增量都要小于我们人为设定的值：

$\large \begin{aligned} &\Delta \tilde{u}( k|t)_{Min}\leqslant\ \Delta {\tilde{u}( k|t)}\leqslant\Delta \tilde{u}( k|t)_{Max}\\ &\Delta \tilde{u}( k+1|t)_{Min}\leqslant\ \Delta {\tilde{u}( k+1|t)}\leqslant\Delta \tilde{u}( k+2|t)_{Max}\\ &\Delta \tilde{u}( k+2|t)_{Min}\leqslant\ \Delta {\tilde{u}( k+2|t)}\leqslant\Delta \tilde{u}( k+2|t)_{Max}\\ &\Delta \tilde{u}( k+3|t)_{Min}\leqslant\ \Delta {\tilde{u}( k+3|t)}\leqslant\Delta \tilde{u}( k+3|t)_{Max}\\ \vdots\\ &\Delta \tilde{u}( k+N_c-1|t)_{Min}\leqslant\ \Delta {\tilde{u}( k+N_c-1|t)}\leqslant\Delta \tilde{u}( k+N_c-1|t)_{Max}\\ \end{aligned}$

即：

$\large \Delta U_{Min}\leq \Delta U\leq \Delta U_{Max}$

二.MPC代码实战（carsim+simulink）

第一步，选择车型，这一步无所谓，喜欢什么车选什么车，但是要注意轴距要和写的s-function一一对应：

车辆基础参数如下，除了轴距之外，也没什么要注意的：

下面是导入到simulink的接口配置：

输入的接口配置如下：

①IMP_SPEED：车辆行驶速度

②IMP_STEER_L1：左前轮转角

③IMP_STEER_R1：右前轮转角

④IMP_STEER_L2：左后轮转角，我们用不到，simulink中置0

⑤IMP_STEER_R2:右后轮转角，用不到，simulink模型中取0

输出的接口配置如下：

①X0:carsim输出的X轴坐标

②Y0：carsim输出的Y轴坐标

③Yaw：carsim输出的航偏角

④Vx：carsim输出的速度

⑤Steer_SW:carsim输出的车辆前轮轮转角

仿真频率的设置如下：

配置好工程之后，点击send to simulink将其发送到simulink，搭建具体的模型如下，xy graph为simulink的画图模块，我们为了方便对比，将车辆的实际行驶轨迹x0，y0和我们预设的理想轨迹x,y用to workspace模块导入到matlab的工作区间中然后画图：

在导入到simulink之后，我们可以看到to workspace前面有out.的前缀，看着让人十分的不爽，下面是清除方法：

点击设置打开：

按步骤操作，将此处的对勾去掉即可。

单位换算模块：

这两个地方为：

下面是完整的MPC控制器脚本代码，可以自行复制，也可以对着教材一行行敲：

%主函数部分
function[sys,x0,str,ts]=MpcControll(t,x,u,flag)
switch flag
    case 0
    [sys,x0,str,ts]=mdlInitializeSizes;
    %初始化函数，对被控方程S-Function进行参数设置，类比构造类。
    case 2
    %更新离散状态量
    sys=mdlUpdates(t,x,u);
    %仅在离散系统中被调用，产生下一个状态
    case 3
    sys = mdlOutputs(t,x,u); 
    %计算输出量
    case {1,4,9}
    %无关的标志位
    sys = [];
 otherwise
  error(['unhandled flag = ',num2str(flag)]); % Error handling
end
%初始化主函数
    function [sys,x0,str,ts] = mdlInitializeSizes
    sizes = simsizes;
    sizes.NumContStates  = 0;
    sizes.NumDiscStates  = 6;%离散状态变量的个数
    sizes.NumOutputs     = 5;%输出量的个数[速度(km/h)，前轮转角(deg)]
    sizes.NumInputs      = 3;%输入量的个数[X0,Y0,Yaw]
    sizes.DirFeedthrough = 1;
    sizes.NumSampleTimes = 1;
    sys = simsizes(sizes); 
    x0 =[0;0;0;0;0;0];             %状态变量初始化   
    global U;
    U=[0;0];
    %初始化时间
    str = [];             % 置空即可
    ts  = [0.05 0];       % 仿真时间格式:[period, offset]
 %更新离散状态函数
    function sys = mdlUpdates(t,x,u)%算完了X(k)然后算X(k+1)
    sys = x;
 %计算输出子函数
    function sys = mdlOutputs(t,x,u)
    global a b u_piao;
    global U; %store chi_tilde=[vel-vel_ref; delta - delta_ref]
    global kesi;
    tic
    Nx=3;%状态量的个数
    Nu =2;%控制量的个数
    Np =60;%预测步长
    Nc=30;%控制步长
    Row=10;%松弛因子
    fprintf('Update start, t=%6.3f\n',t)
    yaw_angle =u(3)*3.1415926/180;%CarSim输出的Yaw angle为角度，角度转换为弧度
%    %直线路径
%    r(1)=5*t; %ref_x-axis
%    r(2)=5;%ref_y-axis
%    r(3)=0;%ref_heading_angle
%    vd1=5;% ref_velocity
%    vd2=0;% ref_steering
%    %半径为25m的圆形轨迹, 圆心为(0, 35), 速度为5m/s
    X_ba=25*sin(0.2*t);     %X坐标
    Y_ba=35-25*cos(0.2*t);  %Y坐标
    fai_ba=0.2*t;           %航向角
    vd1=5;                  %速度
    vd2=0.104;              %转角
%     %半径为35m的圆形轨迹, 圆心为(0, 35), 速度为3m/s
%     r(1)=25*sin(0.12*t);
%     r(2)=25+10-25*cos(0.12*t);
%     r(3)=0.12*t;
%     vd1=3;
%     vd2=0.104;
	%半径为25m的圆形轨迹, 圆心为(0, 35), 速度为10m/s
%      r(1)=25*sin(0.4*t);
%      r(2)=25+10-25*cos(0.4*t);
%      r(3)=0.4*t;
%      vd1=10;
%      vd2=0.104;
%     %半径为25m的圆形轨迹, 圆心为(0, 35), 速度为4m/s
%      r(1)=25*sin(0.16*t);
%      r(2)=25+10-25*cos(0.16*t);
%      r(3)=0.16*t;
%      vd1=4;
%      vd2=0.104;
%参数矩阵
    kesi=zeros(Nx+Nu,1);%构造一个新矩阵
    kesi(1)=u(1)-X_ba;
    kesi(2)=u(2)-Y_ba;
    kesi(3)=yaw_angle-fai_ba;
    kesi(4)=U(1);
    kesi(5)=U(2);
    T=0.1;%采样时间为0.1s
    T_all=40;%总的仿真时间。设置总的仿真时间防止仿真溢出。
    L = 2.6; %轴距为2.6m
    t_d1=fai_ba;
%对矩阵参数进行初始化
    u_piao=zeros(Nu,1);%用来存放二次优化函数解出来的最优输出增量
    Q=100 * eye(Nx*Np,Nx*Np);%状态变量权重矩阵，行列都为状态变量维数*预测步长=3*60=180
    R=5*eye(Nu*Nc);%输出增量权重矩阵R，控制量最优权重矩阵
    
    a=[1    0   -vd1*sin(t_d1)*T;
       0    1   vd1*cos(t_d1)*T;
       0    0   1;]; %a为我们线性离散化后的第一个系数矩阵
    b=[cos(t_d1)*T        0;
       sin(t_d1)*T        0;
       tan(vd2)*T/L      vd1*T/(cos(vd2)^2);];%b为我们线性离散化后的第二个系数矩阵
%初始化状态矩阵X(k|t)
    A_cell=cell(2,2);
    B_cell=cell(2,1);
    A_cell{1,1}=a; %将a矩阵放到A_cell的第一行第一个位置
    A_cell{1,2}=b; %将b矩阵放到A_cell的第一行第二个位置
    A_cell{2,1}=zeros(Nu,Nx); %将2*3的零矩阵放到A_cell第二行的第一个位置
    A_cell{2,2}=eye(Nu); %将2*2的单位阵放到A_cell第二行的第二个位置
    B_cell{1,1}=b; %将b矩阵放到B_cell的第一行
    B_cell{2,1}=eye(Nu); %将2*2的单位阵放到B_cell第二行
    A=cell2mat(A_cell); %这里的A就是我们在推导下一时刻的状态空间时候的A
    B=cell2mat(B_cell); %这里的B就是我们在推导下一时刻的状态空间时候的B
    C=[ 1 0 0 0 0;
        0 1 0 0 0;
        0 0 1 0 0];%C矩阵是我们输出方程的系数矩阵
    PHI_cell=cell(Np,1);%这个PHI是我们通过总结规律得到的等式右边的第一个系数矩阵，60*1维度
    THETA_cell=cell(Np,Nc);%这里的THETA为我们通过总结规律的到的等式右边的第二个系数矩阵，60*30维度
    for j=1:1:Np
        PHI_cell{j,1}=C*A^j;
        for k=1:1:Nc
            if k<=j
            THETA_cell{j,k}=C*A^(j-k)*B;
            else 
            THETA_cell{j,k}=zeros(Nx,Nu);
            end
        end
    end
    PHI=cell2mat(PHI_cell);%将元胞数组转换成矩阵
    THETA=cell2mat(THETA_cell);  
%下面的函数参考教材即可
    H_cell=cell(2,2); %这里的H为我们二次规划中的H矩阵，以下来构造二次规划中的H矩阵
    H_cell{1,1}=THETA'*Q*THETA+R;
    H_cell{1,2}=zeros(Nu*Nc,1);
    H_cell{2,1}=zeros(1,Nu*Nc);
    H_cell{2,2}=Row; %松弛因子
    H=cell2mat(H_cell);
    error=PHI*kesi; 
    f_cell=cell(1,2);%f为二次规划的第二个向量
    f_cell{1,1} = 2*(error'*Q*THETA);
    f_cell{1,2} = 0;
    f=cell2mat(f_cell);%将元胞数组转化为矩阵
%约束条件
    A_t=zeros(Nc,Nc);%构造At矩阵，见教材第四章
    for p=1:1:Nc
        for q=1:1:Nc
            if q<=p 
                A_t(p,q)=1;
            else 
                A_t(p,q)=0;
            end
        end 
    end 
    A_I=kron(A_t,eye(Nu));%求直积
    Ut=kron(ones(Nc,1), U);%此处的U表示上一时刻的输出矩阵。
    umin=[-0.2;  -0.332];%见教材第四章的约束
    umax=[0.2;   0.332];
    delta_umin = [-0.05;  -0.0082];%最小值约束合集
    delta_umax = [0.05;  0.0082];%最大值约束合集
    Umin=kron(ones(Nc,1),umin);
    Umax=kron(ones(Nc,1),umax);
    %二次规划不等式约束 Ax <= b
    A_cons_cell={A_I zeros(Nu*Nc, 1); -A_I zeros(Nu*Nc, 1)};
    b_cons_cell={Umax-Ut;-Umin+Ut};
    A_cons=cell2mat(A_cons_cell);%（求解方程）状态量不等式约束增益矩阵，转换为绝对值的取值范围
    b_cons=cell2mat(b_cons_cell);%（求解方程）状态量不等式约束的取值
    %状态量约束
    M = 10;%松弛因子
    delta_Umin = kron(ones(Nc,1),delta_umin);
    delta_Umax = kron(ones(Nc,1),delta_umax);
    lb = [delta_Umin; 0];%求解二次规划时变量取值的上限
    ub = [delta_Umax; M];%求解二次规划时变量取值的下限
    %% 开始求解过程
    options = optimset('Algorithm','interior-point-convex');    
    [X, fval,exitflag]=quadprog(H, f, A_cons, b_cons,[], [],lb,ub,[],options);
    fprintf('quadprog EXITFLAG = %d\n',exitflag);
    %% 计算输出   
    u_piao(1)=X(1);%X1为二次规划控制时域内最优的速度控制增量
    u_piao(2)=X(2);%X2为二次规划最优前轮转角控制增量
    U(1)=kesi(4)+u_piao(1);%速度误差+速度误差增量
    U(2)=kesi(5)+u_piao(2);%转角误差+转角误差增量
    u_real(1) = U(1) + vd1;%再与下面的参考速度相加才是真正的速度
    u_real(2) = U(2) + vd2;%道理同上
    sys(1)=u_real(1); 
    sys(2)=u_real(2);
    sys(3)=X_ba;
    sys(4)=Y_ba;
    sys(5)=fai_ba;
    toc

这里使用to worspace模块把理想轨迹数据和预测轨迹数据都发到工作区然后用plot函数画图：

plot(X,Y,'y'); % 使用plot函数将X和Y数据可视化成线图
hold on;
plot(X0,Y0,'r');
hold off;

运行结果如下：

参考：

无人驾驶车辆模型预测控制第四章_falcone论文_瑞瑞瑞12123的博客-CSDN博客

《无人驾驶车辆模型预测控制》（第二版）第四章详细学习——算法部分_无人驾驶车辆模型预测控制 pdf_总系学不废的博客-CSDN博客

《无人驾驶车辆模型预测控制》（第二版）

Auerbach°

关注

29
点赞
踩
73

收藏

觉得还不错? 一键收藏
2
评论
算法详解+代码（无人驾驶车辆模型预测控制第四章）

首先对汽车运动学模型进行建模：设后轴速度为,前轴行速度为考虑后轴轴心的速度约束，得到和后轴速度的关系如下：考虑到前后轴速度约束的运动学方程为：设后轴行驶中心为，前轴行驶中心为，根据前后轴之间的几何约束关系可得到: 将上述三个公式联立可得到横摆角速度的表达式为：
复制链接

扫一扫