基于模型预测控制的车辆轨迹跟踪问题的MATLAB仿真

基于模型预测控制设计的无人驾驶车辆轨迹跟踪问题，内附有MATLAB程序与详细的建模过程，研究车辆转向的同学可以作为参考

%第一部分
% 参考轨迹生成
N=100;   %参考轨迹点数量
T=0.05;  %采样周期
Xout=zeros（N3）;    %N行3列矩阵
Tout=zeros（N1）;    %N行1列矩阵
for k=1:1:N
    Xout（k1）=k*T;
    Xout（k2）=2;
    Xout（k3）=0;
    Tout（k1）=（k-1）*T;
end

% 第二部分
%仿真系统基本情况介绍
Nx=3;    %状态量个数
Nu=2;    %控制量个数
[NrNc]=size（Xout）;  %返回Xout的行数和列数
Tsim=20;   %仿真时间
X0=[0 0 pi/3];  %车辆初始状态
L=1;           %车辆轴距
vd1=1;         %参考系统的纵向速度
vd2=0;         %参考系统的前轮偏角

%第3部分
%矩阵定义
x_real=zeros（NrNc）;
x_piao=zeros（NrNc）;
u_real=zeros（NrNu）;
u_piao=zeros（NrNu）;
x_real（1:）=X0;
x_piao（1:）=x_real（1:）-Xout（1:）;
X_PIAO=zeros（NrNx*Tsim）;
XXX=zeros（NrNx*Tsim）;   %用于保存每个时刻预测的所有状态值
q=[1 0 0;0 1 0;0 0 0.5];
Q_cell=cell（TsimTsim）;
for i=1:1:Tsim
    for j=1:1:Tsim
        if i==j
            Q_cell{ij}=q;
        else
            Q_cell{ij}=zeros（NxNx）;
        end
    end
end
Q=cell2mat（Q_cell）;
R=0.1*eye（Nu*TsimNu*Tsim）;

%第四部分
%模型预测控制
for i=1:1:Nr
    t_d=Xout（i3）;
    a=[1 0 -vd1*sin（t_d）*T;
       0 1 vd1*cos（t_d）*T;
       0 0 1];
    b=[cos（t_d）*T 0;
       sin（t_d）*T 0;
       vd2*T/L vd1*T/（cos（vd2）^2）];
    A_cell=cell（Tsim1）;
    B_cell=cell（TsimTsim）;
    for j=1:1:Tsim
        A_cell{j1}=a^j;
        for k=1:1:Tsim
            if k<=j
                B_cell{jk}=（a^（j-k））*b;
            else
                B_cell{jk}=zeros（NxNu）;
            end
        end
    end
    A=cell2mat（A_cell）;
    B=cell2mat（B_cell）;
    
    H=2*（B‘*Q*B+R）;
    f=2*B‘*Q*A*x_piao（i:）‘;
    A_cons=[];
    b_cons=[];
    lb=[-0.2;-0.64];
    ub=[0.2;0.64];
    [Xfval（i1）exitflag（i1）output（i1）]=quadprog（HfA_consb_cons[][]lbub）;
    X_PIAO（i:）=（A*x_piao（i:）‘+B*X）‘;
    if i+j        for j=1:1:Tsim
            XXX（i1+3*（j-1））=X_PIAO（i1+3*（j-1））+Xout（i+j1）;
            XXX（i2+3*（j-1））=X_PIAO（i2+3*（j-1））+Xout（i+j2）;
            XXX（i3+3*（j-1））=X_PIAO（i3+3*（j-1））+Xout（i+j3）;
        end
    else
        for j=1:1:Tsim
            XXX（i1+3*（j-1））=X_PIAO（i1+3*（j-1））+Xout（Nr1）;
            XXX（i2+3*（j-1））=X_PIAO（i2+3*（j-1））+Xout（Nr2）;
            XXX（i3+3*（j-1））=X_PIAO（i3+3*（j-1））+Xout（Nr3）;
        end
    end
    u_piao（i1）=X（11）;
    u_piao（i2）=X（21）;
    Tvec=[0:0.05:4];
    X00=x_real（i:）;
    vd11=vd1+u_piao（i1）;
    vd22=vd2+u_piao（i2）;
    XOUT=dsolve（‘Dx-vd11*cos（z）=0‘‘Dy-vd11*sin（z）=0‘‘Dz-vd22=0‘‘x（0）=X00（1）‘‘y（0）=X00（2）‘‘z（0）=X00（3）‘）;
    t=T;
    x_real（i+11）=eval（XOUT.x）;
    x_real（i+12）=eval（XOUT.y）;
    x_real（i+13）=eval（XOUT.z）;
    if i        x_piao（i+1:）=x_real（i+1:）-Xout（i+1:）;
    end
    u_real（i1）=vd1+u_piao（i1）;
    u_real（i2）=vd2+u_piao（i2）;
    
    figure（1）;
    plot（Xout（1:Nr1）Xout（1:Nr2））;
    hold on;
    plot（x_real（i1）x_real（i2）‘r*‘）;
    xlabel（‘X[m]‘）;
    ylabel（‘Y[m]‘）;
    hold on;
    for k=1:1:Tsim
        X（ik+1）=XXX（i1+3*（k-1））;
        Y（ik+1）=XXX（i2+3*（k-1））;
    end
    X（i1）=x_real（i1）;
 

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2017-10-02 10:28  模型预测控制\
     文件        4261  2017-08-23 17:32  模型预测控制\MPC.m
     文件      603247  2017-10-02 10:27  模型预测控制\基于模型预测控制的无人驾驶车辆轨迹跟踪问题研究.pdf