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。

相关文章

Nginx实现动态封禁IP的步骤指南

Nginx实现动态封禁IP的步骤指南

《Nginx实现动态封禁IP的步骤指南》在日常的生产环境中,网站可能会遭遇恶意请求、DDoS攻击或其他有害的访问行为,为了应对这些情况,动态封禁IP是一项十分重要的安全策略,本篇博客将介绍如何通过NG... 目录1、简述2、实现方式3、使用 fail2ban 动态封禁3.1 安装 fail2ban3.2 配
阅读更多...
Java中实现订单超时自动取消功能(最新推荐)

Java中实现订单超时自动取消功能(最新推荐)

《Java中实现订单超时自动取消功能(最新推荐)》本文介绍了Java中实现订单超时自动取消功能的几种方法,包括定时任务、JDK延迟队列、Redis过期监听、Redisson分布式延迟队列、Rocket... 目录1、定时任务2、JDK延迟队列 DelayQueue（1）定义实现Delayed接口的实体类 （
阅读更多...
将java程序打包成可执行文件的实现方式

将java程序打包成可执行文件的实现方式

《将java程序打包成可执行文件的实现方式》本文介绍了将Java程序打包成可执行文件的三种方法：手动打包（将编译后的代码及JRE运行环境一起打包）,使用第三方打包工具（如Launch4j）和JDK自带... 目录1.问题提出2.如何将Java程序打包成可执行文件2.1将编译后的代码及jre运行环境一起打包2
阅读更多...
通俗易懂的Java常见限流算法具体实现

通俗易懂的Java常见限流算法具体实现

《通俗易懂的Java常见限流算法具体实现》：本文主要介绍Java常见限流算法具体实现的相关资料,包括漏桶算法、令牌桶算法、Nginx限流和Redis+Lua限流的实现原理和具体步骤,并比较了它们的... 目录一、漏桶算法1.漏桶算法的思想和原理2.具体实现二、令牌桶算法1.令牌桶算法流程:2.具体实现2.1
阅读更多...
MySQL8.0设置redo缓存大小的实现

MySQL8.0设置redo缓存大小的实现

《MySQL8.0设置redo缓存大小的实现》本文主要在MySQL8.0.30及之后版本中使用innodb_redo_log_capacity参数在线更改redo缓存文件大小,下面就来介绍一下,具有一... mysql 8.0.30及之后版本可以使用innodb_redo_log_capacity参数来更改
阅读更多...
C++使用栈实现括号匹配的代码详解

C++使用栈实现括号匹配的代码详解

《C++使用栈实现括号匹配的代码详解》在编程中,括号匹配是一个常见问题,尤其是在处理数学表达式、编译器解析等任务时,栈是一种非常适合处理此类问题的数据结构,能够精确地管理括号的匹配问题,本文将通过C+... 目录引言问题描述代码讲解代码解析栈的状态表示测试总结引言在编程中，括号匹配是一个常见问题，尤其是在
阅读更多...
Java实现检查多个时间段是否有重合

Java实现检查多个时间段是否有重合

《Java实现检查多个时间段是否有重合》这篇文章主要为大家详细介绍了如何使用Java实现检查多个时间段是否有重合,文中的示例代码讲解详细,感兴趣的小伙伴可以跟随小编一起学习一下... 目录流程概述步骤详解China编程步骤1：定义时间段类步骤2：添加时间段步骤3：检查时间段是否有重合步骤4：输出结果示例代码结语作
阅读更多...
使用C++实现链表元素的反转

使用C++实现链表元素的反转

《使用C++实现链表元素的反转》反转链表是链表操作中一个经典的问题,也是面试中常见的考题,本文将从思路到实现一步步地讲解如何实现链表的反转,帮助初学者理解这一操作,我们将使用C++代码演示具体实现,同... 目录问题定义思路分析代码实现带头节点的链表代码讲解其他实现方式时间和空间复杂度分析总结问题定义给定
阅读更多...
Java覆盖第三方jar包中的某一个类的实现方法

Java覆盖第三方jar包中的某一个类的实现方法

《Java覆盖第三方jar包中的某一个类的实现方法》在我们日常的开发中,经常需要使用第三方的jar包,有时候我们会发现第三方的jar包中的某一个类有问题,或者我们需要定制化修改其中的逻辑,那么应该如何... 目录一、需求描述二、示例描述三、操作步骤四、验证结果五、实现原理一、需求描述需求描述如下：需要在
阅读更多...
如何使用Java实现请求deepseek

如何使用Java实现请求deepseek

《如何使用Java实现请求deepseek》这篇文章主要为大家详细介绍了如何使用Java实现请求deepseek功能,文中的示例代码讲解详细,感兴趣的小伙伴可以跟随小编一起学习一下... 目录1.deepseek的api创建2.Java实现请求deepseek2.1 pom文件2.2 json转化文件2.2
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2