本文是《无人驾驶车辆模型预测控制》的一些笔记，方便大家查阅，加深对原理和程序的理解。(其实这个工程实例我也不是很懂，总觉得有些公式和程序不符合。) 　　

　　MPC 示意图 　　

　　的必要文字说明 　　

　　一些公式推导出一些变量定义的含义 　　

　　型号的建立 　　

　　项目实例题目：车辆从原点(0，0)出发，追踪一条直线轨道，期望纵向速度v=1 m/s y=2，采样时间50ms，总模拟设定时间20 s建立预测模型 　　

　　由于上面没有对控制变量的描述，所以看第二章。控制变量应该是车速V和前轮偏转角。但是我不太明白这个A和B是怎么推导出来的。 　　

　　代码记录参考轨迹生成：%%参考轨迹生成N=1000分T=0.05%采样时间% Xout=零(2*N，3)；% Tout=zeros(2*N，1)；Xout=zeros(N，3)；Tout=zeros(N，1)；对于k=1:1:N Xout(k，1)=k * T；%x坐标，每采样一次x前进一个点Xout(k，2)=2；%y坐标，保持2 Xout(k，3)=0；%航向角保持在0 Tout(k，1)=(k-1)* T；末端控制系统参数初始化部分%%跟踪一个恒定参考轨迹NX=3；%状态量数量Nu=2；%控制变量Tsim=20%模拟时间题目说20s，我觉得这里可以理解为控制时域和预测时域，都是20x 0=0 0 pi/3；%初始状态量，原点0，0，航向角60度=size(Xout)；% Nr是Xout% Mobile Robot参数sc=1 0 0 0的行数；0 1 0 0;0 0 1 0;0 0 0 1;%好像没用，不管L=1；RR=1；w=1；%移动机器人变量modelvd 1=Rr * w；圆形小车参考系的纵向速度% vd2=0；%前轮偏转角x _ real=参考系的零点(Nr，Nc)；%Nr行号，参考点号100，Nc列号，3，所以是100 * 3x _ piano=zeros (NR，Nc)；%定义偏差矩阵x_piao，100*3u_real=zeros(Nr，2)；%控制量的实矩阵u _ piao=零(Nr，2)；%控制量的偏差矩阵x_real(1，)=X0；%初始试用状态，起点为0，0，前轮偏转角为60度x _ piano(13360)=x _ real(13360)-xout(13360)；%初始化状态偏差X _ PIAO=零(Nr，Nx * Tsim)；%X_PIAO矩阵，包括每个时间步的预测增量。因为Tsim=20，所以它的大小是100，* 20XXX=零(Nr，Nx * Tsim)；%XXX矩阵，用来保存每一时刻的所有预测状态值。每个州都有Tsim预测值，所以每个州有20个预测值q=1 0 0。0 1 0;0 0 0.5;%权重矩阵Q_cell=cell(Tsim，Tsim)；*20个单元阵列初始化大q和大r，对应于下式的Qe和re 　　

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);%初始化Q_cell矩阵 end endendQ=cell2mat(Q_cell);R=0.1*eye(Nu*Tsim,Nu*Tsim);%大小为2*20,2*20关键部分的迭代计算B对应下面这个矩阵，C应该就是1

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=; A_cell=cell(Tsim,1);%20*1，20行1列个元胞矩阵 B_cell=cell(Tsim,Tsim);%20*20，20行20列个元胞矩阵 for j=1:1:Tsim A_cell{j,1}=a^j;%先给个A^j，每个cell是3*3，一共20个，A是60*3 for k=1:1:Tsim if k<=j B_cell{j,k}=(a^(j-k))*b; else B_cell{j,k}=zeros(Nx,Nu);%如果行号小于列号，就是0 end end end A=cell2mat(A_cell);%60*3 B=cell2mat(B_cell);%B是大θt H=2*(B'*Q*B+R);%这里乘2不知何意，应该没有加松弛因子，跟书本有出入 f=2*B'*Q*A*x_piao(i,:)';%这里着实不太理解为何多了一个A A_cons=<>; b_cons=<>; lb=<-1;-1>; ub=<1;1>; tic =quadprog(H,f,A_cons,b_cons,<>,<>,lb,ub);%X是一个100*21的预测矩阵，每一个行对应每一个状态点-在每一个时刻的控制序列 toc X_PIAO(i,:)=(A*x_piao(i,:)'+B*X)';%A*偏差+B*控制序列，更新预测增量大矩阵 if i+j<Nr 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* u_piao(i,2)=X(2,1); Tvec=<0:0.05:4>; X00=x_real(i,:); vd11=vd1+u_piao(i,1);%速度控制量更新 vd22=vd2+u_piao(i,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);%eval函数是将字符串转为表达式的函数 x_real(i+1,2)=eval(XOUT.y); x_real(i+1,3)=eval(XOUT.z); if(i<Nr) x_piao(i+1,:)=x_real(i+1,:)-Xout(i+1,:);%偏差量更新 end u_real(i,1)=vd1+u_piao(i,1);%跟vd11是一样的 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)');总结一下这段代码的核心部分实在是太迷了，还是我自己太菜了，唉，我再多看看资料，加油~~~