如果对Carsim的基础使用还不了解，可以参考：【Carsim Simulink自动驾驶仿真】基于MPC的速度控制
如果对MPC算法原理不清楚，可以参考：如何理解MPC模型预测控制理论
项目介绍：
教程为北理工的无人驾驶车辆模型预测控制第2版。所用的仿真软件为Carsim2020.0和MatlabR2021a。前几章处理的场景都是低速的情况，所以运动学的精度就可以保证很好的控制，但是当处于高速的时候，由于种种原因，比如轮胎不能看成刚体，单纯使用运动学不能很好的进行控制，就需要使用动力学模型，第五章使用考虑轮胎滑移的动力学模型对汽车进行一个转弯控制。

第五章的代码没有多少问题，但是对代码中的一些数学原理问题有需要讨论的地方。

效果图

控制量是前轮转角，目标是让汽车跟踪纵向函数和横摆角函数：


这里画了4个图，分别是Y-X，横摆角-X，前轮转角-t,前轮胎侧倾-t。

MATLAB框架搭建

MATLAB框架沿袭第四章的思路就可以：

轮胎滑移的动力学

这里我们可以不用关注动力学模型是如何推导的，主要看的是怎么用，首先看一下动力学${\dot{x}}_{dyn}=f_{dyn}({\dot{x}}_{dyn},u_{dyn})$的具体形式：
$$\begin{gathered} m{\dot{v}}_y=-m{v}_x\dot{\phi }+2[C_{\alpha f}({\delta }_f-\frac{v_y+l_f\dot{\phi }}{v_x})+C_{\alpha r}\frac{l_r\dot{\phi }-v_y}{v_x}] \\ m{\dot{v}}_x=m{v}_y\dot{\phi }+2[C_{lf}{s}_f+C_{\alpha f}({\delta }_f-\frac{v_y+l_f\dot{\phi }}{v_x}){\delta }_f+C_{lr}{s}_r] \\ {I}_z\ddot{\phi }=2[l_f{C}_{\alpha f}({\delta }_f-\frac{v_y+l_f\dot{\phi }}{v_x})-l_f{C}_{\alpha r}\frac{l_r\dot{\phi }-v_y}{v_x}] \\ \dot{y}=v_{x}sin\phi +v_{y}cos\phi \\ \dot{x}=v_{x}cos\phi -v_{y}sin\phi \end{gathered}$$
这里其实只需要知道，状态变量为：$\dot{y},\dot{x},\phi ,\dot{\phi },y,x$，控制变量为：${\delta }_f$就足够了，至于其他的问题不是我们关注的问题。我们这里处理的是高速问题，所以对MPC的实时性有很高的要求。因为动力学方程是非线性的，非线性模型预测控制难以满足，所以这里继续采用线性时变模型预测控制，因此需要对非线性动力学模型进行线性化。即将${\dot{x}}_{dyn}=f_{dyn}({\dot{x}}_{dyn},u_{dyn})$变成${\dot{x}}_{dyn}=A_{dyn}(t)x_{dyn}(t)+B_{dyn}(t)u_{dyn}(t)$。之后将其离散化处理，得到离散化的表达式：
$$x_{dyn}(k+1)=A_{dyn}(k)x_{dyn}(k)+B_{dyn}(k)u_{dyn}(k)$$
这里，$A_{dyn}(k)=I+T{A}_{dyn}(t)，B_{dyn}(k)=T{B}_{dyn}(t)$。

在MPC中对动力学处理的讨论

按照前几章的思路，有了运动学方程或者动力学方程之后带进去算就好，但是这里的代码我发现不仅使用了线性的动力学模型，也是用了非线性模型。在代码体现在(220-221行)：

上面是原来的代码，下面是按照原来前几章的思路的代码，两个误差效果如下：

重点看一下曲线，红色的是原代码跑出的结果，对应第一行代码中的误差，蓝色的是我认为按照前几章的思路做出的结果，对应第二行，也就是没备注的代码：

没有使用非线性动力学模型轨迹跟踪会有一个提前量，跟踪的没有那么精确，但是可以从动画效果来看误差其实不是很大。而轮胎侧偏比非线性小很多。这里还进行了步长的调整，会发现如果单纯看曲线轨迹的话，单纯的线性动力学参数可调整的范围比较大，但是当控制步长调整太大的时候，会出现在跟踪直线的情况下出现速度，前轮转角小幅震荡的情况，但是当使用这个参数对添加非线性的进行跟踪的时候，连轨迹都是震荡的，甚至会有灵车漂移的现象：

这里为什么在求误差的时候要加上非线性动力学这一项，作者在代码中有一个备注是参考falcone(B,4c)，但是我并没有找到。

对误差中加入非线性动力学的讨论

先明确说一点，我其实没有想明白，按照我之后的推导和理解，认为这里面有一项不是很好理解。这里可能会说的不是很明白，如果有大佬知道或者想讨论，可以评论留言或者私信。
我们主要看一下PSI*kesi+GAMMA*PHI算出来的是什么，我们以$k=2$作为讨论，不针对本例问题，单纯当作一个数学推导，对于第一项PSI*kesi：
$$\begin{gathered} PSI=\left(\begin{array}{c}CA\\ C{A}^2\end{array}\right) \\ kesi=stat{e}_{linear}(k) \end{gathered}$$
这个结果也是前几章直接减去的东西，加的这一项在后面。对于第二项GAMMA*PHI：
$$\begin{gathered} GAMMA=\left(\begin{array}{cc}C& 0\\ CA& C\end{array}\right) \\ PHI=\left(\begin{array}{c}stat{e}_{nolinear}(k+1)-stat{e}_{linear}(k+1)\\ stat{e}_{nolinear}(k+2)-stat{e}_{linear}(k+2)\end{array}\right) \end{gathered}$$
这里代码中的PHI进行了简化，都是用了(k+1)的值。在这里我们假设$C$是一个单位阵，也就是都输出，看一下多减去的东西是什么：
$$\begin{array}{rl}ans& =\left(\begin{array}{c}stat{e}_{nolinear}(k+1)-stat{e}_{linear}(k+1)\\ Astat{e}_{nolinear}(k+1)-Astat{e}_{linear}(k+1)+stat{e}_{nolinear}(k+2)-stat{e}_{linear}(k+2)\end{array}\right)\end{array}$$
我们这里将第$k$时刻的非线性和线性的模型之间的误差记录为：$e_{model}(k)$。那么多家的这一项就变成了：
$$ans=\left(\begin{array}{c}{e}_{model}(k+1)\\ A{e}_{model}(k+1)+e_{model}(k+2)\end{array}\right)$$
所以个人认为，本来非线性模型和真车就有一个动力学误差，为了计算时效性，把非线性变成线性又产生了一个误差，这样会有两个误差，按照我们以前的思路，是直接使用线性的，但是这样误差比较大，所以他这里的代码可能是希望将非线性和线性的误差考虑进去，但是这样推到下来，$A{e}_{model}(k+1)$不是很清楚里面的意义。因为$A$本身就是线性模型弄出来的东西，现在还要再乘一个非线性的结果，这个地方很怪。

附录：全代码

function [sys,x0,str,ts] = chap5DMC(t,x,u,flag)
% 状态量=[y_dot,x_dot,phi,phi_dot,Y,X]，控制量为前轮偏角delta_f

switch flag
    case 0
        [sys,x0,str,ts] = mdlInitializeSizes; % Initialization
    case 2
        sys = mdlUpdates(t,x,u); % Update discrete states
    case 3
        sys = mdlOutputs(t,x,u); % Calculate outputs
    case {1,4,9} % Unused flags
        sys = [];
    otherwise
        error(['unhandled flag = ',num2str(flag)]); % Error handling
end
% End of dsfunc.

%==============================================================
% 初始化
%==============================================================
function [sys,x0,str,ts] = mdlInitializeSizes

sizes = simsizes;
sizes.NumContStates  = 0;
sizes.NumDiscStates  = 6;
sizes.NumOutputs     = 1;
sizes.NumInputs      = 8;
sizes.DirFeedthrough = 1; % Matrix D is non-empty.
sizes.NumSampleTimes = 1;
sys = simsizes(sizes); 
x0 =[0.001;0.0001;0.0001;0.00001;0.00001;0.00001];    
global U;
U=[0];%控制量初始化,这里面加了一个期望轨迹的输出，如果去掉，U为一维的
% global x;
% x = zeros(md.ne + md.pye + md.me + md.Hu*md.me,1);   
% Initialize the discrete states.
str = [];             % Set str to an empty matrix.
ts  = [0.02 0];       % sample time: [period, offset]
%End of mdlInitializeSizes

%==============================================================
% Update the discrete states
%==============================================================
function sys = mdlUpdates(t,x,u)
  
sys = x;
%End of mdlUpdate.

function sys = mdlOutputs(t,x,u)
    global a b;
    global U;
    tic
    Nx=6;%状态量的个数
    Nu=1;%控制量的个数
    Ny=2;%输出量的个数
    Np=35;%预测步长
    Nc=2;%控制步长

    %输入接口转换,x_dot后面加一个非常小的数，是防止出现分母为零的情况
    % y_dot=u(1)/3.6-0.000000001*0.4786;%CarSim输出的是km/h，转换为m/s
    y_dot=u(1)/3.6;
    x_dot=u(2)/3.6+0.0001;
    phi=u(3)*3.141592654/180; %CarSim输出的为角度，角度转换为弧度
    phi_dot=u(4)*3.141592654/180;
    Y=u(5);%单位为m
    X=u(6);%单位为米
    S_L=u(7);
    S_R=u(8);
    
    %% 车辆参数输入
%syms sf sr;%分别为前后车轮的滑移率,需要提供
    Sf=0.2; Sr=0.2;
%syms lf lr;%前后车轮距离车辆质心的距离，车辆固有参数
    lf=1.232;lr=1.468;
%syms C_cf C_cr C_lf C_lr;%分别为前后车轮的纵横向侧偏刚度，车辆固有参数
    Ccf=66900;Ccr=62700;Clf=66900;Clr=62700;
%syms m g I;%m为车辆质量，g为重力加速度，I为车辆绕Z轴的转动惯量，车辆固有参数
    m=1723;g=9.8;I=4175;
    
    %% 参考轨迹生成
    shape=2.4;%参数名称，用于参考轨迹生成
    dx1=25;dx2=21.95;%没有任何实际意义，只是参数名称
    dy1=4.05;dy2=5.7;%没有任何实际意义，只是参数名称
    Xs1=27.19;Xs2=56.46;%参数名称
    X_predict=zeros(Np,1);%用于保存预测时域内的纵向位置信息，这是计算期望轨迹的基础
    phi_ref=zeros(Np,1);%用于保存预测时域内的期望轨迹
    Y_ref=zeros(Np,1);%用于保存预测时域内的期望轨迹
    
    %%  以下计算kesi,即状态量与控制量合在一起
    kesi=zeros(Nx+Nu,1);
    kesi(1)=y_dot;%u(1)==X(1)
    kesi(2)=x_dot;%u(2)==X(2)
    kesi(3)=phi; %u(3)==X(3)
    kesi(4)=phi_dot;
    kesi(5)=Y;
    kesi(6)=X;
    kesi(7)=U(1);
    delta_f=U(1);

    T=0.02;%仿真步长
    T_all=20;%总的仿真时间，主要功能是防止计算期望轨迹越界

    %%  关键参数设置
    Row=1000;%松弛因子权重
    Q_cell=cell(Np,Np);
    for i=1:1:Np
        for j=1:1:Np
            if i==j
                Q_cell{i,j}=[200 0;0 100;];
                %Q_cell{i,j}=[2000 0;0 10000;];
            else 
                Q_cell{i,j}=zeros(Ny,Ny);               
            end
        end 
    end 
    R=5*10^4*eye(Nu*Nc);
    %R=5*10^5*eye(Nu*Nc);
    
    %%  动力学方程设置
    a=[                 1 - (259200*T)/(1723*x_dot),                                                         -T*(phi_dot + (2*((460218*phi_dot)/5 - 62700*y_dot))/(1723*x_dot^2) - (133800*((154*phi_dot)/125 + y_dot))/(1723*x_dot^2)),                                    0,                     -T*(x_dot - 96228/(8615*x_dot)), 0, 0
        T*(phi_dot - (133800*delta_f)/(1723*x_dot)),                                                                                                                  (133800*T*delta_f*((154*phi_dot)/125 + y_dot))/(1723*x_dot^2) + 1,                                    0,           T*(y_dot - (824208*delta_f)/(8615*x_dot)), 0, 0
                                                  0,                                                                                                                                                                                  0,                                    1,                                                   T, 0, 0
            (33063689036759*T)/(7172595384320*x_dot), T*(((2321344006605451863*phi_dot)/8589934592000 - (6325188028897689*y_dot)/34359738368)/(4175*x_dot^2) + (5663914248162509*((154*phi_dot)/125 + y_dot))/(143451907686400*x_dot^2)),                                   0, 1 - (813165919007900927*T)/(7172595384320000*x_dot), 0, 0
                                          T*cos(phi),                                                                                                                                                                         T*sin(phi),  T*(x_dot*cos(phi) - y_dot*sin(phi)),                                                   0, 1, 0
                                         -T*sin(phi),                                                                                                                                                                         T*cos(phi), -T*(y_dot*cos(phi) + x_dot*sin(phi)),                                                   0, 0, 1];
   
    b=[                                                               133800*T/1723
       T*((267600*delta_f)/1723 - (133800*((154*phi_dot)/125 + y_dot))/(1723*x_dot))
                                                                                 0
                                                5663914248162509*T/143451907686400
                                                                                 0
                                                                                 0]; 
                                    
    d_k=zeros(Nx,1);%计算偏差
    state_k1=zeros(Nx,1);%预测的下一时刻状态量，用于计算偏差
     %以下即为根据离散非线性模型预测下一时刻状态量
    state_k1(1,1)=y_dot+T*(-x_dot*phi_dot+2*(Ccf*(delta_f-(y_dot+lf*phi_dot)/x_dot)+Ccr*(lr*phi_dot-y_dot)/x_dot)/m);
    state_k1(2,1)=x_dot+T*(y_dot*phi_dot+2*(Clf*Sf+Clr*Sr+Ccf*delta_f*(delta_f-(y_dot+phi_dot*lf)/x_dot))/m);
    state_k1(3,1)=phi+T*phi_dot;
    state_k1(4,1)=phi_dot+T*((2*lf*Ccf*(delta_f-(y_dot+lf*phi_dot)/x_dot)-2*lr*Ccr*(lr*phi_dot-y_dot)/x_dot)/I);
    state_k1(5,1)=Y+T*(x_dot*sin(phi)+y_dot*cos(phi));
    state_k1(6,1)=X+T*(x_dot*cos(phi)-y_dot*sin(phi));
    d_k=state_k1-a*kesi(1:6,1)-b*kesi(7,1);%根据falcone公式（2.11b）求得d(k,t)
    d_piao_k=zeros(Nx+Nu,1);%d_k的增广形式，参考falcone(B,4c)
    d_piao_k(1:6,1)=d_k;
    d_piao_k(7,1)=0;
    
    A_cell=cell(2,2);
    B_cell=cell(2,1);
    A_cell{1,1}=a;
    A_cell{1,2}=b;
    A_cell{2,1}=zeros(Nu,Nx);
    A_cell{2,2}=eye(Nu);
    B_cell{1,1}=b;
    B_cell{2,1}=eye(Nu);
    %A=zeros(Nu+Nx,Nu+Nx);
    A=cell2mat(A_cell);
    B=cell2mat(B_cell);
   % C=[0 0 1 0 0 0 0;0 0 0 1 0 0 0;0 0 0 0 1 0 0;];%这是和输出量紧密关联的
    C=[0 0 1 0 0 0 0;0 0 0 0 1 0 0;];
    PSI_cell=cell(Np,1);
    THETA_cell=cell(Np,Nc);
    GAMMA_cell=cell(Np,Np);
    PHI_cell=cell(Np,1);
    for p=1:1:Np
        PHI_cell{p,1}=d_piao_k;%理论上来说，这个是要实时更新的，但是为了简便，这里又一次近似
        for q=1:1:Np
            if q<=p
                GAMMA_cell{p,q}=C*A^(p-q);
            else 
                GAMMA_cell{p,q}=zeros(Ny,Nx+Nu);
            end 
        end
    end
    for j=1:1:Np
     PSI_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(Ny,Nu);
            end
        end
    end
    PSI=cell2mat(PSI_cell);%size(PSI)=[Ny*Np Nx+Nu]
    THETA=cell2mat(THETA_cell);%size(THETA)=[Ny*Np Nu*Nc]
    GAMMA=cell2mat(GAMMA_cell);%大写的GAMMA
    PHI=cell2mat(PHI_cell);
    Q=cell2mat(Q_cell);
    H_cell=cell(2,2);
    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_1=zeros(Ny*Np,1);
    Yita_ref_cell=cell(Np,1);
    for p=1:1:Np
        if t+p*T>T_all
            X_DOT=x_dot*cos(phi)-y_dot*sin(phi);%惯性坐标系下纵向速度
            X_predict(Np,1)=X+X_DOT*Np*T;
            %X_predict(Np,1)=X+X_dot*Np*t;
            z1=shape/dx1*(X_predict(Np,1)-Xs1)-shape/2;
            z2=shape/dx2*(X_predict(Np,1)-Xs2)-shape/2;
            Y_ref(p,1)=dy1/2*(1+tanh(z1))-dy2/2*(1+tanh(z2));
            phi_ref(p,1)=atan(dy1*(1/cosh(z1))^2*(1.2/dx1)-dy2*(1/cosh(z2))^2*(1.2/dx2));
            Yita_ref_cell{p,1}=[phi_ref(p,1);Y_ref(p,1)];
            
        else
            X_DOT=x_dot*cos(phi)-y_dot*sin(phi);%惯性坐标系下纵向速度
            X_predict(p,1)=X+X_DOT*p*T;%首先计算出未来X的位置，X(t)=X+X_dot*t
            z1=shape/dx1*(X_predict(p,1)-Xs1)-shape/2;
            z2=shape/dx2*(X_predict(p,1)-Xs2)-shape/2;
            Y_ref(p,1)=dy1/2*(1+tanh(z1))-dy2/2*(1+tanh(z2));
            phi_ref(p,1)=atan(dy1*(1/cosh(z1))^2*(1.2/dx1)-dy2*(1/cosh(z2))^2*(1.2/dx2));
            Yita_ref_cell{p,1}=[phi_ref(p,1);Y_ref(p,1)];

        end
    end
    Yita_ref=cell2mat(Yita_ref_cell);
    error_1=Yita_ref-PSI*kesi-GAMMA*PHI; %求偏差
    %error_1 = Yita_ref-PSI*kesi;
    f_cell=cell(1,2);
    f_cell{1,1}=2*error_1'*Q*THETA;
    f_cell{1,2}=0;
%     f=(cell2mat(f_cell))';
    f=-cell2mat(f_cell);
    %% 以下为约束生成区域
 %控制量约束
    A_t=zeros(Nc,Nc);%见falcone论文 P181
    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(1));
    umin=-0.1744;%维数与控制变量的个数相同
    umax=0.1744;
    delta_umin=-0.0148*0.4;
    delta_umax=0.0148*0.4;
    Umin=kron(ones(Nc,1),umin);
    Umax=kron(ones(Nc,1),umax);
    
    %输出量约束
    ycmax=[0.21;5];
    ycmin=[-0.3;-3];
    Ycmax=kron(ones(Np,1),ycmax);
    Ycmin=kron(ones(Np,1),ycmin);
    
    %结合控制量约束和输出量约束
    A_cons_cell={A_I zeros(Nu*Nc,1);-A_I zeros(Nu*Nc,1);THETA zeros(Ny*Np,1);-THETA zeros(Ny*Np,1)};
    b_cons_cell={Umax-Ut;-Umin+Ut;Ycmax-PSI*kesi-GAMMA*PHI;-Ycmin+PSI*kesi+GAMMA*PHI};
    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','active-set');
       x_start=zeros(Nc+1,1);%加入一个起始点
      [X,fval,exitflag]=quadprog(H,f,A_cons,b_cons,[],[],lb,ub,x_start,options);
      
%% 计算输出
    u_piao=X(1);%得到控制增量
    U(1)=kesi(7,1)+u_piao;%当前时刻的控制量为上一刻时刻控制+控制增量
   %U(2)=Yita_ref(2);%输出dphi_ref
    sys= U;
    toc
% End of mdlOutputs.

最后

以上就是优秀萝莉为你收集整理的【Carsim Simulink自动驾驶仿真】基于MPC的动力学控制效果图MATLAB框架搭建轮胎滑移的动力学在MPC中对动力学处理的讨论附录：全代码的全部内容，希望文章能够帮你解决【Carsim Simulink自动驾驶仿真】基于MPC的动力学控制效果图MATLAB框架搭建轮胎滑移的动力学在MPC中对动力学处理的讨论附录：全代码所遇到的程序开发问题。

如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。