模型预测控制的缺点_代码详解——无人驾驶车辆模型预测控制3.3.3代码详解

%《无人驾驶车辆模型预测控制》3.3.3代码详解

%代码下载链接：http://www.bitpress.com.cn/book/book_detail.php?id=9055

%北京科技大学 白国星注释 david.gx.bai@gmail.com

clc; %清空命令空间，目的是好看

clear all; %清空工作空间，避免同名变量影响程序初始值

%% 参考轨迹生成

N=100; %生成100个点的参考路径

T=0.05; %时间间隔

% Xout=zeros(2*N,3);

% Tout=zeros(2*N,1);

Xout=zeros(N,3);  %预留数组空间

Tout=zeros(N,1);

for k=1:1:N

   Xout(k,1)=k*T;  %横坐标

   Xout(k,2)=2;  %纵坐标

   Xout(k,3)=0;  %航向角

   Tout(k,1)=(k-1)*T;  %参考轨迹时间

end

%% Tracking a constant reference trajectory

Nx=3; %状态变量

Nu =2; %输入变量

Tsim =20; %仿真时间

X0 = [0 0 pi/3];  %车辆初始位置姿态

[Nr,Nc] = size(Xout); % Nr is the number ofrows of Xout

% Mobile Robot Parameters

c = [1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1];

L = 1;

Rr = 1;

w = 1;

% Mobile Robot variable Model

vd1 = Rr*w; % For circular trajectory  %车速设置为1，参考路径中Xout(k,1)=k*T，其实省略了速度，应该是Xout(k,1)=k*T*v，vd1需要与v相等，如果不等，轨迹跟踪可能出现错误

vd2 = 0;

x_real=zeros(Nr,Nc);

x_piao=zeros(Nr,Nc);

u_real=zeros(Nr,2);

u_piao=zeros(Nr,2);

x_real(1,:)=X0;

x_piao(1,:)=x_real(1,:)-Xout(1,:);   %此处与书上公式存在一些差异，式3.14、3.15中，误差为一系列预测位姿与参考路径点之间的误差，但代码中实质上是当前位姿与当前跟踪目标点之间的误差，所以公式3.15应该修订为e=φ*(x(t)-xref(t))

X_PIAO=zeros(Nr,Nx*Tsim);

XXX=zeros(Nr,Nx*Tsim);%用于保持每个时刻预测的所有状态值  %用于绘图

q=[1 0 0;0 1 0;0 0 0.5];

Q_cell=cell(Tsim,Tsim);

for i=1:1:Tsim

   for j=1:1:Tsim

       if i==j

           Q_cell{i,j}=q;

       else

           Q_cell{i,j}=zeros(Nx,Nx);

       end

   end

end

Q=cell2mat(Q_cell);

R=0.1*eye(Nu*Tsim,Nu*Tsim);  %权重系数

for i=1:1:Nr

   t_d =Xout(i,3);

   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;

       0            T;];

   A_cell=cell(Tsim,1);

   B_cell=cell(Tsim,Tsim);

   for j=1:1:Tsim

       A_cell{j,1}=a^j;

       for k=1:1:Tsim

           if k<=j

                B_cell{j,k}=(a^(j-k))*b;

           else

                B_cell{j,k}=zeros(Nx,Nu);

           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,:)';  %式3.16中的H和G

   A_cons=[];

   b_cons=[];

   lb=[-1;-1];

   ub=[1;1];  %约束条件

   tic

   [X,fval(i,1),exitflag(i,1),output(i,1)]=quadprog(H,f,A_cons,b_cons,[],[],lb,ub);  %求解

   toc

   X_PIAO(i,:)=(A*x_piao(i,:)'+B*X)'; %绘图用变量

   if i+j

       for j=1:1:Tsim

           XXX(i,1+3*(j-1))=X_PIAO(i,1+3*(j-1))+Xout(i+j,1);

           XXX(i,2+3*(j-1))=X_PIAO(i,2+3*(j-1))+Xout(i+j,2);

           XXX(i,3+3*(j-1))=X_PIAO(i,3+3*(j-1))+Xout(i+j,3);

       end

   else

       for j=1:1:Tsim

           XXX(i,1+3*(j-1))=X_PIAO(i,1+3*(j-1))+Xout(Nr,1);

           XXX(i,2+3*(j-1))=X_PIAO(i,2+3*(j-1))+Xout(Nr,2);

           XXX(i,3+3*(j-1))=X_PIAO(i,3+3*(j-1))+Xout(Nr,3);

       end

   end  %绘图用变量结束

   u_piao(i,1)=X(1,1);  %求解结果

   u_piao(i,2)=X(2,1);  %求解结果

   Tvec=[0:0.05:4];

   X00=x_real(i,:);

   vd11=vd1+u_piao(i,1);  %产生的控制输入1

   vd22=vd2+u_piao(i,2);  %产生的控制输入2，控制器运行结束

   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+1,1)=eval(XOUT.x);

   x_real(i+1,2)=eval(XOUT.y);

   x_real(i+1,3)=eval(XOUT.z);

   if(i

       x_piao(i+1,:)=x_real(i+1,:)-Xout(i+1,:);

   end

   u_real(i,1)=vd1+u_piao(i,1);

   u_real(i,2)=vd2+u_piao(i,2);  %迭代车辆下一时刻的位置，被控车辆模型结束

   figure(1);  %绘轨迹图

   plot(Xout(1:Nr,1),Xout(1:Nr,2));

   hold on;

   plot(x_real(i,1),x_real(i,2),'r*');

   title('跟踪结果对比');

   xlabel('横向位置X');

   axis([-1 5 -1 3]);

   ylabel('纵向位置Y');

   hold on;

   for k=1:1:Tsim

       X(i,k+1)=XXX(i,1+3*(k-1));

       Y(i,k+1)=XXX(i,2+3*(k-1));

   end

   X(i,1)=x_real(i,1);

   Y(i,1)=x_real(i,2);

   plot(X(i,:),Y(i,:),'y')

   hold on;  %绘轨迹图结束

end  %仿真循环结束

%% 以下为绘图部分

figure(2)

subplot(3,1,1);

plot(Tout(1:Nr),Xout(1:Nr,1),'k--');

hold on;

plot(Tout(1:Nr),x_real(1:Nr,1),'k');

%grid on;

%title('状态量-横向坐标X对比');

xlabel('采样时间T');

ylabel('横向位置X')

subplot(3,1,2);

plot(Tout(1:Nr),Xout(1:Nr,2),'k--');

hold on;

plot(Tout(1:Nr),x_real(1:Nr,2),'k');

%grid on;

%title('状态量-横向坐标Y对比');

xlabel('采样时间T');

ylabel('纵向位置Y')

subplot(3,1,3);

plot(Tout(1:Nr),Xout(1:Nr,3),'k--');

hold on;

plot(Tout(1:Nr),x_real(1:Nr,3),'k');

%grid on;

hold on;

%title('状态量-\theta对比');

xlabel('采样时间T');

ylabel('\theta')

figure(3)

subplot(2,1,1);

plot(Tout(1:Nr),u_real(1:Nr,1),'k');

%grid on;

%title('控制量-纵向速度v对比');

xlabel('采样时间T');

ylabel('纵向速度')

subplot(2,1,2)

plot(Tout(1:Nr),u_real(1:Nr,2),'k');

%grid on;

%title('控制量-角加速度对比');

xlabel('采样时间T');

ylabel('角加速度')

figure(4)

subplot(3,1,1);

plot(Tout(1:Nr),x_piao(1:Nr,1),'k');

%grid on;

xlabel('采样时间T');

ylabel('e(x)');

subplot(3,1,2);

plot(Tout(1:Nr),x_piao(1:Nr,2),'k');

%grid on;

xlabel('采样时间T');

ylabel('e(y)');

subplot(3,1,3);

plot(Tout(1:Nr),x_piao(1:Nr,3),'k');

%grid on;

xlabel('采样时间T');

ylabel('e(\theta)');