基于动力学模型的无人驾驶车辆MPC轨迹跟踪算法及carsim+matlab联合仿真学习笔记_实现基于车辆运动学模型在frenet坐标系下通过mpc算法跟踪一个圆的matlab代码

1.1.2 线性时变预测模型推导

2 代码解析

2.1 模板框架

2.1.1 S-Function

function [sys,x0,str,ts] = chapter5_2_2(t,x,u,flag) % u表示来自carsim的输入
switch flag,
 case 0
  [sys,x0,str,ts] = mdlInitializeSizes; % 初始化系统参数的一些基本的维度
 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)]);
end

2.1.2 mdlInitializeSizes函数

function [sys,x0,str,ts] = mdlInitializeSizes
sizes = simsizes;
sizes.NumContStates  = 0; % 连续状态量个数为 0 
sizes.NumDiscStates  = 6; % 离散状态量个数为 6
sizes.NumOutputs     = 1; % 输出个数为 1，delta_f 作为输出
sizes.NumInputs      = 8; % 输入个数为 8，来自 carsim，还包括2个滑移率
sizes.DirFeedthrough = 1; % mdlOutputs函数模块中直接调用了u的数据，所以为1
sizes.NumSampleTimes = 1; 
sys = simsizes(sizes); 
x0 =[0.001;0.0001;0.0001;0.00001;0.00001;0.00001]; % 6个状态量的初始值
global U; % 1维控制量，前轮转角输出 delta_f
U=[0]; % delta_f(0)=0，起步时前轮转角为 0°
str = [];
ts  = [0.05 0]; % 仿真间隔是0.05秒，第二项为0表示从0秒开始立即仿真

2.1.3 mdlUpdates()函数

function sys = mdlUpdates(t,x,u) % 这部分没有用到
sys = x;

2.1.4 mdlOutputs()函数

u 表示来自carsim的 8 个状态输入，

function sys = mdlOutputs(t,x,u)
    tic % 开始计时
    fprintf('Update start, t=%6.3f\n',t) % 日志记录当前时刻控制动作的开始
    Nx=6;
    Nu=1;
    Ny=2;
    Np =20;
    Nc=5;
    T=0.05; % 采样时间

2.2 MPC 算法主体

tic 含义：

下面这部分是车体的参数，目的是为了求解雅可比矩阵 a 和 b

%% a b 常量参数：车体
    Sf=0.2; Sr=0.2;
    lf=1.232;lr=1.468;
    Ccf=66900;Ccr=62700;
    Clf=66900;Clr=62700;
    m=1723;g=9.8;I=4175;
%% a b 变量参数，carsim 实时采集
    y_dot=u(1)/3.6; % Km/h => m/s
    x_dot=u(2)/3.6+0.0001; % 防止分母为0
    phi=u(3)*3.141592654/180; % degree => rad
    phi_dot=u(4)*3.141592654/180;
    Y=u(5);
    X=u(6);
    Y_dot=u(7); % 前轮纵向滑移率
    X_dot=u(8); % 后轮纵向滑移率
    global U; % 控制量
    delta_f=U(1); 
    fprintf('Update start, u(1)=%4.2f\n',U(1)) % 日志记录每个控制周期的控制量 

雅可比矩阵 a  b 求解

接下来就是雅可比矩阵 a 和 b 的矩阵表达式，这里只写出了结果：

%% a b
    global a b;
    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];  

定义雅可比矩阵 a 和 b 的目的，是为了求解 A 和 B 矩阵：

代码实现：

    %%  A B C 
    A_cell=cell(2,2);
    A_cell{1,1}=a;
    A_cell{1,2}=b;
    A_cell{2,1}=zeros(Nu,Nx);
    A_cell{2,2}=eye(Nu);
    A=cell2mat(A_cell);
    
    B_cell=cell(2,1);
    B_cell{1,1}=b;
    B_cell{2,1}=eye(Nu);
    B=cell2mat(B_cell);
    
    C=[0 0 1 0 0 0 0; 
       0 0 0 0 1 0 0;];

这里 得到的A 和 B 矩阵的目的是为了得到 ψ θ Γ Φ 矩阵，参考公式（13-a）：

 由于 θ 同时被矩阵E H f 调用，所以这里可以首先实现 θ：

    %% THETA (被 E H f 调用)
    THETA_cell=cell(Np,Nc);
    for j=1:1:Np
        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
    THETA=cell2mat(THETA_cell);

我们的最终目标是定义 E H f 矩阵，以便构造二次规划标准型矩阵，下面我们按顺序依次实现 E H 和 f 矩阵，结合公式按顺序定义矩阵可以使代码段更加直观。

E 矩阵

第一项便是参考轨迹，这里可以用外部文件生成，也可以直接定义。

参考轨迹 Y ref

   %% Y_ref 
    % phi_ref Y-ref 
    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);%用于保存预测时域内的参考横向位置信息
    % Yita_ref
    Yita_ref_cell=cell(Np,1); % ===========这里命名应该是Y_out_ref_cell，因为η的维度是2，不是 Np，但因为输出量Y和横向坐标命名冲突================
    T_all=20; % 总的仿真时间，防止计算期望轨迹越界
    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;
            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的斜率，由非线性动力学公式（2.29给出）
            X_predict(p,1)=X+X_DOT*p*T; % X是由carsim输出的车辆当前纵向坐标，X_predict(p,1)表示下一时刻的纵坐标
            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)]; % [phi_rel; Y_ref]
        end
    end
    Yita_ref=cell2mat(Yita_ref_cell); % Y_ref 矩阵规范化

接下来实现 ψ

    %% ψ  PSI 
    PSI_cell=cell(Np,1);
    for j=1:1:Np
        PSI_cell{j,1}=C*A^j; 
    end
    PSI=cell2mat(PSI_cell);

然后是 ξ，这里 ξ 是经过重构的增广矩阵：

    %% ξ  kesi 
    kesi=zeros(Nx+Nu,1); % ξnew(k)=[ξ(k); u(k-1)]
    kesi(1)=y_dot;
    kesi(2)=x_dot;
    kesi(3)=phi; 
    kesi(4)=phi_dot;
    kesi(5)=Y;
    kesi(6)=X; 
    kesi(7)=U(1); % u(k-1)

GAMMA

    %% Γ  GAMMA
    GAMMA_cell=cell(Np,Np); % 维度 20*20
    for p=1:1:Np;
        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 
    GAMMA=cell2mat(GAMMA_cell);

最后是 Φ 的实现比较复杂，首先来看表达式，

dk,t 表达式：

 ξt_hat(k+1)：这里需要理解：

    %% φ   PHI
    % ξt_hat_(k+1) 为求解 PHI 做准备
    state_k1=zeros(Nx,1);% 下面开始公式（10-a1）即k+1时刻的值 = k时刻的初始值 + T * k时刻的斜率（公式2.38）
    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));

 因为前面已经定义了雅可比矩阵 a 和 b，现在可以直接写出 dk,t 矩阵形式：

% dk 为求解 PHI 做准备
    d_k=zeros(Nx,1); % (6,1)
    d_k=state_k1-a*kesi(1:6,1)-b*kesi(7,1); % 公式（11-b）d_k=ξt_hat_(k+1)-Ak,t * ξt_hat_(k) -Bk,t * u(k-1)

这里还需要重构 d_piao_k：

% d_piao_k 为求解 PHI 做准备
    d_piao_k=zeros(Nx+Nu,1) % (7,1)
    d_piao_k(1:6,1)=d_k; % d_piao_k=[dk; 0]，见公式（12-a）
    d_piao_k(7,1)=0;

最终可以求 Φ 矩阵：

    % PHI 最终求解
    PHI_cell=cell(Np,1); % φ(20*7,1)
    for p=1:1:Np;
        PHI_cell{p,1}=d_piao_k; 
    end
    PHI=cell2mat(PHI_cell);

接下来可以实现 E 矩阵

%% E 最终求解
    E=zeros(Ny*Np,1);% 源代码名为 error_1
    E=Yita_ref-PSI*kesi-GAMMA*PHI; % E=Yref-phi*ξ-Γφ,这里Yita_ref就是Yref

H 矩阵

% Q 
    Q_cell=cell(Np,Np);
    for i=1:1:Np; 
        for j=1:1:Np;
            if i==j
                Q_cell{i,j}=[2000 0;0 10000;]; % 分别为横摆角权重、横向位置权重
            else 
                Q_cell{i,j}=zeros(Ny,Ny); % Ny = 2
            end
        end 
    end 
    Q=cell2mat(Q_cell);
% R  
    R=5*10^5*eye(Nu*Nc);
% 松弛因子ρ
    Row=1000;
 % H 最终求解
    H_cell=cell(2,2);
    H_cell{1,1}=THETA'*Q*THETA+R; % H = θ'* Q * θ + 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); 
    H=(H+H')/2; % 保证 H 矩阵是严格的对称阵

f 矩阵

    f_cell=cell(1,2);
    f_cell{1,1}=2 * E' * Q * THETA;
    f_cell{1,2}=0;
    f=-cell2mat(f_cell);

到目前为止已经实现了全部要素矩阵 H f，接下俩可以开始定义约束矩阵。

约束矩阵

%% 控制量u(k)的约束，
    A_t=zeros(Nc,Nc);
    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)); % 这里A_I就是A_t，因为 Nu=1
    Ut=kron(ones(Nc,1),U(1)); Ut = u(k -1)
    umin=-0.1744; % 前轮偏角的上约束
    umax=0.1744; % 前轮偏角的下约束
    Umin=kron(ones(Nc,1),umin);
    Umax=kron(ones(Nc,1),umax);

输出量 Y(k) 的约束形式：

%% 输出量约束
    ycmax=[0.21; 5]; % 横摆角和纵向位置的约束
    ycmin=[-0.3;-3];
    Ycmax=kron(ones(Np,1),ycmax);
    Ycmin=kron(ones(Np,1),ycmin);

u(k) 和 Y(k) 约束的增广混合形式：

% 结合控制量约束和输出量约束
    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);%（求解方程）状态量不等式约束的取值

Δu(k) 的约束，也就是QP求解方程的标准状态量X的约束：

%% 控制增量约束
    delta_umin=-0.0148*0.4; % 前轮偏角变化量的下约束
    delta_umax= 0.0148*0.4; % 前轮偏角变化量的上约束
    delta_Umin=kron(ones(Nc,1),delta_umin);
    delta_Umax=kron(ones(Nc,1),delta_umax); 
    M=10; % ε的上界 
    lb=[delta_Umin;0]; % [Δu(k);ε]的增广下界约束 lb
    ub=[delta_Umax;M]; % [Δu(k);ε]的增广上界约束 ub

quadprog 求解器

调用 matlab 自带的二次规划求解器 quadprog 进行求解，这部分是求解器的核心

%% quadprog求解器
options = optimset('Algorithm','interior-point-convex');% 选用内点法求解
x_start=zeros(Nc+1,1); % [Δu(k);ε]的初始值为 0
[X,fval,exitflag]=quadprog(H,f,A_cons,b_cons,[],[],lb,ub,x_start,options);
% [Δu(k);ε]的解存放在变量 X 中，包含未来Nc个时间点的Δu(k+i)

结果处理，将计算结果 u(k) 输出到carsim中：

%% 计算输出
u_piao=X(1); % 从Nc个未来时刻的Δu(k+i)中提取第一个时刻的Δu(k)作为当前时刻的最优解
U(1)=kesi(7,1)+u_piao; % 这里kesi(7,1)是u(k-1)，加上Δu(k),则为当前时刻实际输出u(k)
sys= U; % 这里的 U 作为simulink的输出量，直接复制给carsim，进行前轮转角控制
toc % 一个完整的控制周期计时结束

现在已经实现了matlab全部的代码，下一节将进行联合仿真。

3. carsim、simulink联合仿真

本节主要以实例讲解通过搭建 simulink/carsim联合仿真平台，对设计的MPC控制器进行验证。下图是基于动力学模型的MPC轨迹跟踪控制器进行carsim与simulink联合仿真的实例。carsim版本是8.02，matlab版本是2018a。