MPC模型预测控制（二）-MATLAB代码实现

update：MPC的QQ群

第一个群已经满500人(贫穷使我充不起鹅厂会员)，这是第二个群。

群都满了。

https://blog.csdn.net/tingfenghanlei/article/details/85046120在这篇文章里主要讲了下MPC的原理和C++实现的一个简单例子。

这篇文章里主要写MPC的MATLAB实现。许多做控制的同学还是很喜欢用MATLAB的，可以先用MATLAB跑跑看自己的代码效果怎么样。

我看MPC的MATLAB代码实现，主要看的是《无人驾驶车辆模型预测控制》这本书，书里的代码也比较完备。这里实现的代码基本上都是这本书中的，CSDN也有下载链接，大家可以去下载观看。

在实现MPC的代码之前，书中讲了LQR的代码实现。

LQR和MPC的区别：

LQR solves an optimization,

MPC solves a constrained optimization

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.

参考链接https://www.quora.com/Whats-the-difference-between-constrained-LQR-and-MPC

function LQR_1()
%这里先从简单开始，给定一个直线车道和车辆位置偏差。
%参考轨迹的生成方法有两种：
%1.车辆在Path上投影，然后在PATH上选取一系列的点作参考点
%*现在遇到的问题是Q R的参数怎么设置。而且通用性怎么办？*%clear all;
close all;
clc;
%% 给定参数：vel = 6; % 纵向车速,单位：m/s
L=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]; %人为给定,相当于QN
Vk=[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 on
hold 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;end
elseif 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;endend
end

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;%前轮偏角，可以当做前馈量end
RefTraj_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 vehicle
num=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 trajectory
Nx=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 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，参考系统的纵向速度
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 end
end
Q=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代码我会放上来。

MPC模型预测控制（二）-MATLAB代码实现相关推荐

MPC模型预测控制原理和Matlab以及Python代码实现
MPC模型预测控制原理和代码一. 介绍模型预测控制(MPC)原理简要解释一下最优控制最优控制的目标是在一定的约束条件下达到最优的系统表现,那么要让系统达到最优表现,一般是通过定义损失函数J,通过最 ...
MPC模型预测控制+matlab代码实现+simulink仿真实现
MPC模型预测控制+matlab代码实现 MPC模型预测控制原理步骤 matlab代码实现程序主程序函数参数调试 MPC模型预测控制原理步骤 matlab代码实现程序主程序 cle ...
基于MPC 模型预测控制的轨迹跟随，横向控制模型，车道保持
基于MPC 模型预测控制的轨迹跟随,横向控制模型,车道保持,simulink模型采用二自由度车辆动力学模型,可以自定义车辆参数,自定义目标轨迹,图中为单移线目标轨迹与实际轨迹偏差 ID:485067 ...
MPC模型预测控制学习笔记-2021.10.27
MPC模型预测控制学习笔记-点击目录就可以跳转 1. 笔者介绍 2. 参考资料 3. MPC分类 4. 数据的标准化与归一化 5. MATLAB-MPC学习笔记 5.1 获取测试信号:gensig( ...
基于扰动模型预测控制的 matlab仿真程序，程序封装完善
基于扰动模型预测控制的 matlab仿真程序,程序封装完善,只需要输入被控系统(适用于一般时不变离散线性系统,扰动序列已知)参数(状态转移矩阵,控制增益矩阵,预测步长等)即可计算出最优控制律,也可根据 ...
MPC模型预测控制及在Matlab中实现函数定义
基于b站DR_CAN老师的MPC控制视频[MPC模型预测控制器]4_数学建模推导--Matlab代码详解_哔哩哔哩_bilibili的学习分享如下: 一.研究目的在约束条件(物理限制)下达到最优的系 ...
MPC(模型预测控制)_附matlab例程
写在前面: 本文为科研理论笔记的第二篇,其余笔记目录传送门: 理论笔记专栏目录介绍结束下面开始进入正题: 1 基本概念 1.1 最优控制最优控制(optimal control): ...
【DR_CAN-MPC学习笔记】34.详细的MPC建模例子和matlab代码
上一篇博客:[DR_CAN-MPC学习笔记]2.最优化数学建模推导参照二次规划一般形式,详细推导了MPC的数学模型,即最小化代价函数的表达式,最终推导结果为: DR_CAN的视频: [MPC模型预测 ...
python预测控制_无人驾驶——4.控制之MPC模型预测控制
源自:<无人驾驶无人驾驶车辆模型预测控制>--龚建伟参考:https://wenku.baidu.com/view/8e4633d519e8b8f67c1cb9fa.html 0.车辆模 ...

MPC模型预测控制（二）-MATLAB代码实现

MPC模型预测控制（二）-MATLAB代码实现相关推荐

最新文章

热门文章