update：MPC的QQ群
第一个群已经满500人(贫穷使我充不起鹅厂会员)，这是第二个群。
群都满了。
/tingfenghanlei/article/details/85046120在这篇文章里主要讲了下MPC的原理和C++实现的一个简单例子。
这篇文章里主要写MPC的MATLAB实现。许多做控制的同学还是很喜欢用MATLAB的，可以先用MATLAB跑跑看自己的代码效果怎么样。
我看MPC的MATLAB代码实现，主要看的是《无人驾驶车辆模型预测控制》这本书，书里的代码也比较完备。这里实现的代码基本上都是这本书中的，CSDN也有下载链接，大家可以去下载观看。
在实现MPC的代码之前，书中讲了LQR的代码实现。
LQR和MPC的区别：
LQR solves an optimization,
MPC solves aconstrainedoptimization
In practice, optimization could lead to over-voltage, ovre-current, excessive force etc. You want a motor starts very quickly? The optimizer tells you give it an infinite electric current. So you use a saturation which destroys the optimality. MPC solves an optimization without excessing the limits.
In addition, LQR can be solved offline for an LTI system. However, MPC is not a linear controller.Typically, it must be solved online at each sample time. It requires higher computational load. MPC has toolbox in MATLAB. You can use it before you learn its theory in deep.
参考链接/Whats-the-difference-between-constrained-LQR-and-MPC
function LQR_1()%这里先从简单开始，给定一个直线车道和车辆位置偏差。%参考轨迹的生成方法有两种：%1.车辆在Path上投影，然后在PATH上选取一系列的点作参考点%*现在遇到的问题是Q R的参数怎么设置。而且通用性怎么办？*%clear all;close all;clc;%% 给定参数：vel = 6; % 纵向车速,单位：m/sL=2.85;%轴距T=0.05;% sample time， control period% 给定圆形参考轨迹CEN=[0,0]; % 圆心Radius=20; % 半径%% 设置参数Hp =10;%predictive horizion, control horizon N_l=200;% 设置迭代次数Nx=3;%状态变量参数的个数Nu=1;%控制变量参数的个数FWA=zeros(N_l,1);%前轮偏角FWA(1,1)= 0; %初始状态的前轮偏角x_real=zeros(Nx,N_l);%实际状态x_real(:,1)= [22 0 pi/2]; %x0=车辆初始状态X_init初始状态% x_piao=zeros(N_l,Nx);%实际状态与参考轨迹的误差% % u_real=zeros(N_l,Nu);%实际的控制量% u_piao=zeros(N_l,Nu);%实际控制量与参考控制量的误差% X_PIAO=zeros(N_l,3*Hp);%通过DR估计的状态% % XXX=zeros(N_l,3*Hp);%用于保持每个时刻预测的所有状态值RefTraj=zeros(3,1);Delta_x = zeros(3,1);Q=[10 0 0; 0 10 0; 0 0 100];R=[10];%r是对控制量误差的weighting matricePk=[1 0 0; 0 1 0; 0 0 1]; %人为给定,相当于QNVk=[0 0 0]'; %人为给定,相当于QN%% 算法实现u_feedBackward=0;u_feedForward=0;%*首先生成参考轨迹，画出图来作参考*%[RefTraj_x,RefTraj_y,RefTraj_theta,RefTraj_delta]=Func_CircularReferenceTrajGenerate(x_real(1,1),x_real(1,2),CEN(1),CEN(2),Radius,250,vel,T,L);figure(1) %绘制参考路径plot(RefTraj_x,RefTraj_y,'k')xlabel('x','fontsize',14)ylabel('y','fontsize',14)title('Plot of x vs y - Ref. Trajectory');legend('reference traj');axis equal grid onhold onfor i=1:1:N_lG_Test = 3;%先确定参考点和确定矩阵A,B.这里姑且认为A和B是不变的[RefTraj_x,RefTraj_y,RefTraj_theta,RefTraj_delta]=Func_CircularReferenceTrajGenerate(x_real(1,i),x_real(2,i),CEN(1),CEN(2),Radius,Hp,vel,T,L);u_feedForward = RefTraj_delta(G_Test);%前馈控制量%u_feedForward=0;RefTraj_x(G_Test)RefTraj_y(G_Test)RefTraj_theta(G_Test)Delta_x(1,1) = x_real(1,i) - RefTraj_x(G_Test);Delta_x(2,1) = x_real(2,i) - RefTraj_y(G_Test);Delta_x(3,1) = x_real(3,i) - RefTraj_theta(G_Test);if Delta_x(3,1) > piDelta_x(3,1) = Delta_x(3,1)-2*pi;else if Delta_x(3,1) < -1*piDelta_x(3,1) = Delta_x(3,1) +2*pi;elseDelta_x(3,1) = Delta_x(3,1);end end% 通过Backward recursion 求K for j=Hp:-1:2 Pk_1 = Pk;Vk_1 = Vk;A=[1 0 -vel*sin(RefTraj_theta(j-1))*T; 0 1 vel*cos(RefTraj_theta(j-1))*T; 0 0 1;];% B=[cos(RefTraj_theta(j-1))*T 0; sin(RefTraj_theta(j-1))*T 0; 0 vel*T/L;]; COS2 = cos(RefTraj_delta(j-1))^2;B=[ 0 0 vel*T/(L*COS2)]'; K = (B'*Pk_1*A)/(B'*Pk_1*B+R);Ku = R/(B'*Pk_1*B+R);Kv = B'/(B'*Pk_1*B+R);Pk=A'*Pk_1*(A-B*K)+Q; Vk=(A-B*K)'*Vk_1 - K'*R*RefTraj_delta(j-1); endu_feedBackward = -K*(Delta_x)-Ku*u_feedForward-Kv*Vk_1; FWA(i+1,1)=u_feedForward+u_feedBackward;[x_real(1,i+1),x_real(2,i+1),x_real(3,i+1)]=Func_VehicleKineticModule_Euler(x_real(1,i),x_real(2,i),x_real(3,i),vel,FWA(i,1),FWA(i+1,1),T,L); end%% 绘图% figure(1);%plot(RefTraj_x,RefTraj_y,'b')%hold on;plot(x_real(1,:),x_real(2,:),'r*');title('跟踪结果对比');xlabel('横向位置X');% axis([-1 5 -1 3]);ylabel('纵向位置Y'); end
还有4个子函数
function K=Func_Alpha_Pos(Xb,Yb,Xn,Yn)AngleY=Yn-Yb;AngleX=Xn-Xb;%***求Angle*******%if Xb==Xnif Yn>YbK=pi/2;elseK=3*pi/2;endelseif Yb==Ynif Xn>XbK=0;elseK=pi;endelseK=atan(AngleY/AngleX);end end%****修正K,使之在0~360°之间*****%if (AngleY>0&&AngleX>0)%第一象限K=K;elseif (AngleY>0&&AngleX<0)||(AngleY<0&&AngleX<0)%第二、三象限K=K+pi;else if (AngleY<0&&AngleX>0)%第四象限K=K+2*pi; elseK=K;endendend
function Theta=Func_Theta_Pos(Alpha)if Alpha >= 3*pi/2Theta = Alpha-3*pi/2;elseTheta = Alpha+pi/2;endend
function [RefTraj_x,RefTraj_y,RefTraj_theta,RefTraj_delta]=Func_CircularReferenceTrajGenerate(Pos_x,Pos_y,CEN_x,CEN_y,Radius,N,Velo,Ts,L)%RefTraj为要生成的参考路径%Pos_x,Pos_y为车辆坐标%CEN_x,CEN_y,Radius圆心与半径%N要生成几个参考点，即预测空间。%Velo,Ts车速与采样时间%L汽车的轴距RefTraj=zeros(N,4);%生成的参考路径Alpha_init=Func_Alpha_Pos(CEN_x,CEN_y,Pos_x,Pos_y);%首先根据车辆位置和圆心确定alphaOmega=Velo/Radius%已知车速和半径，可以求得角速度。DFWA=atan(L/Radius);for k=1:1:NAlpha(k)=Alpha_init+Omega*Ts*(k-1);RefTraj(k,1)=Radius*cos(Alpha(k))+CEN_x;%xRefTraj(k,2)=Radius*sin(Alpha(k))+CEN_y;%yRefTraj(k,3)=Func_Theta_Pos(Alpha(k));%theta RefTraj(k,4)=DFWA;%前轮偏角，可以当做前馈量endRefTraj_x= RefTraj(:,1);RefTraj_y= RefTraj(:,2);RefTraj_theta= RefTraj(:,3);RefTraj_delta= RefTraj(:,4);end
function [X,Y,H]=Func_VehicleKineticModule_Euler(x,y,heading,vel,FWA,DFWA,T,L)%车辆运动学模型，状态量，x,y,heading;控制量：vel=constant，FWA%固定的步数，来求得数值解%%%initial the status of the vehiclenum=100;Xmc=zeros(1,num);Ymc=zeros(1,num);Headingmc=zeros(1,num);Xmc(1)=x;Ymc(1)=y;%x,y初始坐标Headingmc(1)=heading;%航向,Headingrate=zeros(1,num);FrontWheelAngle=zeros(1,num);t=T/num;%%FrontWheelAngle=linspace(FWA,DFWA,num);%前轮偏角Headingrate=vel*tan(FrontWheelAngle)/L;for i=2:numHeadingmc(i)=Headingmc(i-1)+Headingrate(i)*t;Xmc(i)=Xmc(i-1)+vel*t*cos(Headingmc(i-1));Ymc(i)=Ymc(i-1)+vel*t*sin(Headingmc(i-1));end%%X=Xmc(num);Y=Ymc(num);H=Headingmc(num);end%% test% [X,Y,H]=VehicleKineticModule_Euler(0,0,0,10,0,3,0.1,2.85)%plot(X,Y,'b');
现在再看看MPC的代码实现
clc;close all;clear all;%% 参考轨迹生成N=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:NXout(k,1)=k*T;Xout(k,2)=2;Xout(k,3)=0;Tout(k,1)=(k-1)*T;end%% Tracking a constant reference trajectoryNx=3;%状态量个数Nu =2;%控制量个数Tsim =20;%仿真时间X0 = [0 0 pi/3];%初始状态[Nr,Nc] = size(Xout); % Nr is the number of rows of Xout，100*3% Mobile Robot Parametersc = [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 Modelvd1 = Rr*w; % For circular trajectory，参考系统的纵向速度vd2 = 0;%参考系统的前轮偏角%根据控制系统的维度信息，提前定义好相关矩阵并赋值x_real=zeros(Nr,Nc);%X的真实状态x_piao=zeros(Nr,Nc);%X的误差状态u_real=zeros(Nr,2);%真实控制量u_piao=zeros(Nr,2);%误差控制量x_real(1,:)=X0;%初始状态x_piao(1,:)=x_real(1,:)-Xout(1,:);%与预期的误差值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:Tsimfor j=1:1:Tsimif i==jQ_cell{i,j}=q;else Q_cell{i,j}=zeros(Nx,Nx);end endendQ=cell2mat(Q_cell);%权重矩阵R=0.1*eye(Nu*Tsim,Nu*Tsim);%权重矩阵%模型预测控制主体for i=1:1:Nrt_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:TsimA_cell{j,1}=a^j;for k=1:1:Tsimif k<=jB_cell{j,k}=(a^(j-k))*b;elseB_cell{j,k}=zeros(Nx,Nu);endendendA=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=[-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);%二次规划求解tocX_PIAO(i,:)=(A*x_piao(i,:)'+B*X)';if i+j<Nrfor j=1:1:TsimXXX(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);endelsefor j=1:1:TsimXXX(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);endendu_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);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);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,:);endu_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:TsimX(i,k+1)=XXX(i,1+3*(k-1));Y(i,k+1)=XXX(i,2+3*(k-1));endX(i,1)=x_real(i,1);Y(i,1)=x_real(i,2);plot(X(i,:),Y(i,:),'y.')hold on;end% figure(5)% plot(X(2,:),Y(2,:),'b');%% 以下为绘图部分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)');
添加了一些注释，但是感觉这个代码写的不是很好。
下次看到好的MPC代码我会放上来。
