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

2024-04-10 14:58
文章标签 代码 matlab 实现 模型 控制 预测 mpc

本文主要是介绍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代码实现的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/891382。

相关文章

python生成随机唯一id的几种实现方法

python生成随机唯一id的几种实现方法

《python生成随机唯一id的几种实现方法》在Python中生成随机唯一ID有多种方法,根据不同的需求场景可以选择最适合的方案,文中通过示例代码介绍的非常详细,需要的朋友们下面随着小编来一起学习学习... 目录方法 1：使用 UUID 模块（推荐）方法 2：使用 Secrets 模块（安全敏感场景）方法
阅读更多...
Spring StateMachine实现状态机使用示例详解

Spring StateMachine实现状态机使用示例详解

《SpringStateMachine实现状态机使用示例详解》本文介绍SpringStateMachine实现状态机的步骤,包括依赖导入、枚举定义、状态转移规则配置、上下文管理及服务调用示例,重点解... 目录什么是状态机使用示例什么是状态机状态机是计算机科学中的​​核心建模工具​​，用于描述对象在其生命
阅读更多...
Spring Boot 结合 WxJava 实现文章上传微信公众号草稿箱与群发

Spring Boot 结合 WxJava 实现文章上传微信公众号草稿箱与群发

《SpringBoot结合WxJava实现文章上传微信公众号草稿箱与群发》本文将详细介绍如何使用SpringBoot框架结合WxJava开发工具包,实现文章上传到微信公众号草稿箱以及群发功能,... 目录一、项目环境准备1.1 开发环境1.2 微信公众号准备二、Spring Boot 项目搭建2.1 创建
阅读更多...
IntelliJ IDEA2025创建SpringBoot项目的实现步骤

IntelliJ IDEA2025创建SpringBoot项目的实现步骤

《IntelliJIDEA2025创建SpringBoot项目的实现步骤》本文主要介绍了IntelliJIDEA2025创建SpringBoot项目的实现步骤,文中通过示例代码介绍的非常详细,对大家... 目录一、创建 Spring Boot 项目1. 新建项目2. 基础配置3. 选择依赖4. 生成项目5.
阅读更多...
Linux下删除乱码文件和目录的实现方式

Linux下删除乱码文件和目录的实现方式

《Linux下删除乱码文件和目录的实现方式》：本文主要介绍Linux下删除乱码文件和目录的实现方式,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录linux下删除乱码文件和目录方法1方法2总结Linux下删除乱码文件和目录方法1使用ls -i命令找到文件或目录
阅读更多...
SpringBoot+EasyExcel实现自定义复杂样式导入导出

SpringBoot+EasyExcel实现自定义复杂样式导入导出

《SpringBoot+EasyExcel实现自定义复杂样式导入导出》这篇文章主要为大家详细介绍了SpringBoot如何结果EasyExcel实现自定义复杂样式导入导出功能,文中的示例代码讲解详细,... 目录安装处理自定义导出复杂场景1、列不固定，动态列2、动态下拉3、自定义锁定行/列，添加密码4、合并
阅读更多...
mybatis执行insert返回id实现详解

mybatis执行insert返回id实现详解

《mybatis执行insert返回id实现详解》MyBatis插入操作默认返回受影响行数,需通过useGeneratedKeys+keyProperty或selectKey获取主键ID,确保主键为自... 目录 两种方式获取自增 ID：1. ​​useGeneratedKeys+keyProperty（推
阅读更多...
Spring Boot集成Druid实现数据源管理与监控的详细步骤

Spring Boot集成Druid实现数据源管理与监控的详细步骤

《SpringBoot集成Druid实现数据源管理与监控的详细步骤》本文介绍如何在SpringBoot项目中集成Druid数据库连接池,包括环境搭建、Maven依赖配置、SpringBoot配置文件... 目录1. 引言1.1 环境准备1.2 Druid介绍2. 配置Druid连接池3. 查看Druid监控
阅读更多...
Linux在线解压jar包的实现方式

Linux在线解压jar包的实现方式

《Linux在线解压jar包的实现方式》：本文主要介绍Linux在线解压jar包的实现方式,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录linux在线解压jar包解压 jar包的步骤总结Linux在线解压jar包在 Centos 中解压 jar 包可以使用 u
阅读更多...
浅析Spring如何控制Bean的加载顺序

浅析Spring如何控制Bean的加载顺序

《浅析Spring如何控制Bean的加载顺序》在大多数情况下,我们不需要手动控制Bean的加载顺序,因为Spring的IoC容器足够智能,但在某些特殊场景下,这种隐式的依赖关系可能不存在,下面我们就来... 目录核心原则：依赖驱动加载手动控制 Bean 加载顺序的方法方法 1：使用@DependsOn(最直
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2