%第一部分
%?參考軌跡生成
N=100;???%參考軌跡點(diǎn)數(shù)量
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

%?第二部分
%仿真系統(tǒng)基本情況介紹
Nx=3;????%狀態(tài)量個(gè)數(shù)
Nu=2;????%控制量個(gè)數(shù)
[NrNc]=size（Xout）;??%返回Xout的行數(shù)和列數(shù)
Tsim=20;???%仿真時(shí)間
X0=[0?0?pi/3];??%車輛初始狀態(tài)
L=1;???????????%車輛軸距
vd1=1;?????????%參考系統(tǒng)的縱向速度
vd2=0;?????????%參考系統(tǒng)的前輪偏角

%第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）;???%用于保存每個(gè)時(shí)刻預(yù)測的所有狀態(tài)值
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）;

%第四部分
%模型預(yù)測控制
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）;
?

?屬性????????????大小?????日期????時(shí)間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2017-10-02?10:28??模型預(yù)測控制\
?????文件????????4261??2017-08-23?17:32??模型預(yù)測控制\MPC.m
?????文件??????603247??2017-10-02?10:27??模型預(yù)測控制\基于模型預(yù)測控制的無人駕駛車輛軌跡跟蹤問題研究.pdf