无人驾驶车辆模型预测直线轨迹跟踪控制MATLAB实现

N=100;
T=0.05;
Xout=zeros（N3）;
Tout=zeros（N1）;
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）;
Tsim=30;
X0=[0 0 pi/3];
L=1;
vd1=1;
vd2=0;


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=[-2.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