手把手教用matlab做无人驾驶（二十三）--LMPC

最新推荐文章于 2024-05-20 17:19:45 发布
最新推荐文章于 2024-05-20 17:19:45 发布
阅读量1.4k 收藏 15
点赞数 1
分类专栏： matlab Planning 文章标签： 自动驾驶 算法
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/caokaifa/article/details/123760395
版权
matlab 同时被 2 个专栏收录
25 篇文章 131 订阅
订阅专栏
Planning
10 篇文章 27 订阅
订阅专栏

 

 

 

 

 

 

代码段

% MATLAB program for Linear MPC: Single-input system
clear all;
close all
% System parameters and simulation parameters
A=[0.9 0.2;-0.4 0.8];
B=[0.1;0.01];
NT=50;N=5;n=2;m=1; 
Q=eye(n); QN=Q; R=eye(m);
Fx=[1 0;0 1;-1 0;0 -1];gx=[1;1;1;1];
Fu=[1;-1];gu=[1;1];


x0=[10;5]; 
x=zeros(n,NT+1); x(:,1)=x0;
Xk=zeros(n*(N+1),1); Xk(1:n,1)=x0;
u=zeros(m,NT);
Uk=zeros(m*N,1);
zk=[Xk;Uk];

% constructing AX,BU,QX,RU,FX,gX,FU,gU,H
for i=1:N+1
    AX((i-1)*n+1:i*n,:)=A^(i-1);
end
for i=1:N+1
  for j=1:N
      if i>j
          BU((i-1)*n+1:i*n,(j-1)*m+1:j*m)=A^(i-j-1)*B;
      else
          BU((i-1)*n+1:i*n,(j-1)*m+1:j*m)=zeros(n,m);
      end    
  end
end
QX=Q;RU=R;
FX=Fx;gX=gx;FU=Fu;gU=gu;
for i=1:N-1
  QX=blkdiag(QX,Q); RU=blkdiag(RU,R);
  FX=blkdiag(FX,Fx);gX=[gX;gx];
  FU=blkdiag(FU,Fu);gU=[gU;gu];
end
QX=blkdiag(QX,QN);
FX=blkdiag(FX,Fx);
gX=[gX;gx];
H=blkdiag(QX,RU);

% simulating system with MPC
for k=1:NT
   xk=x(:,k);  
   fun = @(z)z'*H*z;
   F=blkdiag(FX,FU);g=[gX;gU];Feq=[eye((N+1)*n) -BU];geq=AX*xk;
   lb=[];ub=[];
   z=fmincon(fun,zk,F,g,Feq,geq,lb,ub)
   u(:,k)=z((N+1)*n+1:(N+1)*n+m,1);
   x(:,k+1)=A*x(:,k)+B*u(:,k);
   zk=z;
end    

% plotting response
figure(1)
time = (0:NT);
subplot(2,1,1)
plot(time,x(1,:),'r.-','LineWidth',.7) 
hold on
plot(time,x(2,:),'k.-','LineWidth',.7) 
legend('$x_1$','$x_2$','Interpreter','latex');
%axis ([0 50-10 10])
xlabel('$k$','Interpreter','latex');ylabel('$\textbf{x}_{k}$','Interpreter','latex');
subplot(2,1,2)
stairs(time(1:end-1),u,'r.-','LineWidth',.7)
%axis([0 50 -10 0])
xlabel('$k$','Interpreter','latex');ylabel('${u}_{k}$','Interpreter','latex');

仿真：

注意：上面也可以把状态的不等式约束放入变量最大最小值中：

 MATLAB program for Linear MPC: Single-input system (alternate code for LMPC_1.m with constraints defined using lb and ub)
clear all;
close all
% System parameters and simulation parameters
A=[0.9 0.2;-0.4 0.8];
B=[0.1;0.01];
NT=50;N=5;n=2;m=1; 
Q=eye(n); QN=Q; R=eye(m);
xmin=-10;xmax=10;umin=-1;umax=1;

x0=[10;5]; 
x=zeros(n,NT+1); x(:,1)=x0;
Xk=zeros(n*(N+1),1); Xk(1:n,1)=x0;
u=zeros(m,NT);
Uk=zeros(m*N,1);
zk=[Xk;Uk];

% constructing AX,BU,QX,RU,H
for i=1:N+1
    AX((i-1)*n+1:i*n,:)=A^(i-1);
end
for i=1:N+1
  for j=1:N
      if i>j
          BU((i-1)*n+1:i*n,(j-1)*m+1:j*m)=A^(i-j-1)*B;
      else
          BU((i-1)*n+1:i*n,(j-1)*m+1:j*m)=zeros(n,m);
      end    
  end
end
QX=Q;RU=R;
for i=1:N-1
  QX=blkdiag(QX,Q); RU=blkdiag(RU,R);
end
QX=blkdiag(QX,QN);
H=blkdiag(QX,RU);

% simulating system with MPC
for k=1:NT
   xk=x(:,k);  
   fun = @(z)z'*H*z;
   F=[];g=[];Feq=[eye((N+1)*n) -BU];geq=AX*xk;
   lb=[xmin*ones(1,(N+1)*n),umin*ones(1,N*m)];
   ub=[xmax*ones(1,(N+1)*n),umax*ones(1,N*m)];
   z=fmincon(fun,zk,F,g,Feq,geq,lb,ub);
   u(:,k)=z((N+1)*n+1:(N+1)*n+m,1);
   x(:,k+1)=A*x(:,k)+B*u(:,k);
   zk=z;
end    

% plotting response
figure(1)
time = (0:NT);
subplot(2,1,1)
plot(time,x(1,:),'r.-','LineWidth',.7) 
hold on
plot(time,x(2,:),'k.-','LineWidth',.7) 
legend('$x_1$','$x_2$','Interpreter','latex');
%axis ([0 50-10 10])
xlabel('$k$','Interpreter','latex');ylabel('$\textbf{x}_{k}$','Interpreter','latex');
grid on
ax = gca;
set(gca,'xtick',[0:5:50])
set(gca,'ytick',[-10:5:10])
ax.GridAlpha = 1
ax.GridLineStyle = ':'
subplot(2,1,2)
stairs(time(1:end-1),u,'r.-','LineWidth',.7)
%axis([0 50 -10 0])
xlabel('$k$','Interpreter','latex');ylabel('${u}_{k}$','Interpreter','latex');
grid on
ax = gca;
set(gca,'xtick',[0:5:50])
set(gca,'ytick',[-1:.5:1])
ax.GridAlpha = 1
ax.GridLineStyle = ':'
print -dsvg lmpc1

 注意：上面代码发现计算中变量比较多，会损失计算效率，下面给出改进：

 

 

 

% MATLAB program for Linear MPC: Reducing online computation (method 1)
clear all;
close all
% System parameters and simulation parameters
A=[0.9 0.2;-0.4 0.8];
B=[0.1;0.01];
NT=50;N=5;n=2;m=1; 
Q=eye(n); QN=Q; R=eye(m);
Fx=[1 0;0 1;-1 0;0 -1];gx=[10;10;10;10];
Fu=[1;-1];gu=[1;1];


x0=[10;5];
x=zeros(n,NT+1); x(:,1)=x0;
Xk=zeros(n*(N+1),1); Xk(1:n,1)=x0;
u=zeros(m,NT);
Uk=zeros(m*N,1);
zk=Uk;

% constructing AX,BU,QX,RU
for i=1:N+1
      AX((i-1)*n+1:i*n,:)=A^(i-1);
  for j=1:N
      if i>j
          BU((i-1)*n+1:i*n,(j-1)*m+1:j*m)=A^(i-j-1)*B;
      else
          BU((i-1)*n+1:i*n,(j-1)*m+1:j*m)=zeros(n,m);
      end    
  end
end
QX=Q;RU=R;
FX=Fx;gX=gx;FU=Fu;gU=gu;
for i=1:N-1
  QX=blkdiag(QX,Q); RU=blkdiag(RU,R);
  FX=blkdiag(FX,Fx);gX=[gX;gx];
  FU=blkdiag(FU,Fu);gU=[gU;gu];
end
QX=blkdiag(QX,QN);
FX=blkdiag(FX,Fx);
gX=[gX;gx];
H=BU'*QX*BU+RU;


% simulating system with MPC
for k=1:NT
   xk=x(:,k);
   qk=2*xk'*AX'*QX*BU;rk=xk'*AX'*QX*AX*xk;
   fun = @(z)z'*H*z+qk*z+rk;
   F=[FX*BU;FU];g=[gX-FX*AX*xk;gU];Feq=[];geq=[];
   lb=[];ub=[];
   z=fmincon(fun,zk,F,g,Feq,geq,lb,ub);
   u(:,k)=z(1:m,1);
   x(:,k+1)=A*x(:,k)+B*u(:,k);
   zk=z;
end     

% plotting response
figure(1)
time = (0:NT);
subplot(2,1,1)
plot(time,x(1,:),'r.-','LineWidth',.7) 
hold on
plot(time,x(2,:),'k.-','LineWidth',.7) 
legend('$x_1$','$x_2$','Interpreter','latex');
axis([0 50 -10 10])
xlabel('$k$','Interpreter','latex');ylabel('$\textbf{x}_{k}$','Interpreter','latex');
grid on
ax = gca;
set(gca,'xtick',[0:5:50])
set(gca,'ytick',[-10:5:10])
ax.GridAlpha = 1
ax.GridLineStyle = ':'
subplot(2,1,2)
stairs(time(1:end-1),u,'r.-','LineWidth',.7)
%axis([0 50 -10 0])
xlabel('$k$','Interpreter','latex');ylabel('${u}_{k}$','Interpreter','latex');
grid on
ax = gca;
set(gca,'xtick',[0:5:50])
set(gca,'ytick',[-1:.5:1])
ax.GridAlpha = 1
ax.GridLineStyle = ':'
print -dsvg lmpc3

 

对于给定轨迹跟踪： 

 

 

% MATLAB program for Linear MPC: Set point tracking
clear all;
close all
% System parameters and simulation parameters
A=[0.9 0.2;-0.4 0.8];
B=[0.1;0.01];
NT=50;N=5;n=2;m=1; 
Q=eye(n); QN=Q; R=eye(m);
xr=[3;2];ur=pinv(B)*(eye(n)-A)*xr;
Fx=[1 0;0 1;-1 0;0 -1];gx=[10;10;10;10]-Fx*xr;
Fu=[1;-1];gu=[1;1]-Fu*ur;


x0=[10;5]; 
x=zeros(n,NT+1); x(:,1)=x0-xr;
Xk=zeros(n*(N+1),1); Xk(1:n,1)=x0;
u=zeros(m,NT);
Uk=zeros(m*N,1);
zk=[Xk;Uk];

% constructing AX,BU,QX,RU,FX,gX,FU,gU,H
for i=1:N+1
    AX((i-1)*n+1:i*n,:)=A^(i-1);
end
for i=1:N+1
  for j=1:N
      if i>j
          BU((i-1)*n+1:i*n,(j-1)*m+1:j*m)=A^(i-j-1)*B;
      else
          BU((i-1)*n+1:i*n,(j-1)*m+1:j*m)=zeros(n,m);
      end    
  end
end
QX=Q;RU=R;
FX=Fx;gX=gx;FU=Fu;gU=gu;
for i=1:N-1
  QX=blkdiag(QX,Q); RU=blkdiag(RU,R);
  FX=blkdiag(FX,Fx);gX=[gX;gx];
  FU=blkdiag(FU,Fu);gU=[gU;gu];
end
QX=blkdiag(QX,QN);
FX=blkdiag(FX,Fx);
gX=[gX;gx];
H=blkdiag(QX,RU);

% simulating system with MPC
for k=1:NT
   xk=x(:,k);  
   fun = @(z)z'*H*z;
   F=blkdiag(FX,FU);g=[gX;gU];Feq=[eye((N+1)*n) -BU];geq=AX*xk;
   lb=[];ub=[];
   z=fmincon(fun,zk,F,g,Feq,geq,lb,ub);
   u(:,k)=z((N+1)*n+1:(N+1)*n+m,1);
   x(:,k+1)=A*x(:,k)+B*u(:,k);
   zk=z;
end    

% plotting response
figure(1)
time = (0:NT);
subplot(2,1,1)
plot(time,x(1,:)+xr(1),'r.-','LineWidth',0.7) 
hold on
plot(time,x(2,:)+xr(2),'k.-','LineWidth',0.7) 
legend('$x_1$','$x_2$','Interpreter','latex');
axis ([0 50 -10 10])
xlabel('$k$','Interpreter','latex');ylabel('$\textbf{x}_{k}$','Interpreter','latex');
grid on
ax = gca;
set(gca,'xtick',[0:5:50])
set(gca,'ytick',[-10:5:10])
ax.GridAlpha = 1
ax.GridLineStyle = ':'
subplot(2,1,2)
stairs(time(1:end-1),u-pinv(B)*(A-eye(n))*xr,'r.-','LineWidth',0.7)
axis ([0 50 -1 1])
xlabel('$k$','Interpreter','latex');ylabel('${u}_{k}$','Interpreter','latex');
grid on
ax = gca;
set(gca,'xtick',[0:5:50])
set(gca,'ytick',[-1:.5:1])
ax.GridAlpha = 1
ax.GridLineStyle = ':'
print -dsvg lmpc5

 

 

Robert--cao
关注
专栏目录
MATLAB无人驾驶模型预测
05-03
无人驾驶MPC控制模型预测，包括书籍PDF讲解基本原理，以及MATLAB代码的具体实现。
离散控制Matlab代码-lmpc-matlab:lmpc-matlab
05-23
离散控制Matlab代码[removed] </ script> [removed] hljs.initHighlightingOnLoad（）; </ script> [removed] MathJax.Hub.Config（{tex2jax：{inlineMath：[['$'，'$']，['\\（'，'\\）']]}}））;; </ ...
手把手教用matlab做无人驾驶（三）-路径规划A*算法
木牛的博客
07-23 1971
这里，我们更新主程序如下： % editor: Robert.Cao % 2018.9.1 clc clear all close all disp('A Star Path Planing start!!') p.start=[1,1]; %起始点 p.goal=[10,3]; %目标点 p.XYMAX=11; obstacle=GetBoundary(p);%得到边界数据 nObstacle=20; obstacle=GetObstacle(nObstacle,obstacle,p)
手把手教用matlab做无人驾驶（十六）--matlab/simulink 高级应用
caokaifa的博客
08-21 7349
1.首先，介绍一下matlab/simulink中把模块封装为库函数 通过以下建立Library 如下是我的库内容，保存库名字为mathlib： 然后建立一个模型，打开mathlib.slx模块，把这个模块加入模型中，建立如下模型，仿真即可。 如果想改这个模型，打开拖动会出现：Attempt to modify link 'untitled1/Subsystem'. ...
手把手教用matlab做无人驾驶（七）-扩展Kalman滤波（extended_kalman_filter）
caokaifa的博客
09-27 5881
对于Kalman滤波在我看来就是五个公式，kalman的计算过程在我的前面博客里面也给出了，不了解kalman的可以看看我前面的博客，这里讲解扩展卡尔曼滤波（extended_kalman_filter）。 为什么要用EKF KF的假设之一就是高斯分布的xx预测后仍服从高斯分布，高斯分布的xx变换到测量空间后仍服从高斯分布。可是，假如F、H是非线性变换，那么上述条件则不成立。 将非线性系统线...
手把手教用matlab做无人驾驶（十五）--matlab/simulink stateflow学习教程
caokaifa的博客
07-27 1万+
1.打开stateflow的五种方式方式: (1)在maltab命令行里面输入：sf (2)命令行输入：sflib (3)命令行输入：sfnew -matlab (4)命令行输入：sfnew -c 这是打开stateflowc语言编写的 (5)直接在simulink library中直接打开stateflow 如上图，随便哪一种方式都可以。 2.在matlab/simul...
手把手教用matlab做无人驾驶（四）-pure pursuit
热门推荐
caokaifa的博客
09-03 1万+
如上图汽车的自行车模型，上面轻灰色显示的是车，这个车坐标系是红色，这个世界坐标系是蓝色，轮子转向角是,两个轴扩展虚线然后相交于ICR。ICR到后轮和到前轮的距离分别为R1，R2。 运动等式： 对于上面的公式车身角度增量与轮子转向的关系的推导： 轮子的约束条件： 纯追踪算法对于路径跟随而言是简单和有效的方法，这里给出跟踪圆的路径， 在matlab/si...
毕业设计&课设-双积分器系统的MATLAB LMPC实现.zip
01-13
matlab算法，工具源码，适合毕业设计、课程设计作业，所有源码均经过严格测试，可以直接运行，可以放心下载使用。有任何使用问题欢迎随时与博主沟通，第一时间进行解答！ matlab算法，工具源码，适合毕业设计、课程...
MATLAB LMPC implementation for a double integrator system.zip
07-23
标题 "MATLAB LMPC implementation for a double integrator system" 指的是使用 MATLAB 实现的线性模型预测控制（Linear Model Predictive Control, LMPC）针对双积分器系统的应用。LMPC 是一种先进的控制策略，它...
无人驾驶车辆模型预测控制Matlab代码
05-14
在原书代码之上，新增注释，修改部分错误，代码成功运行，无任何错误，只含该书第三章基于运动学仿真代码
matlab教程绝对手把手教你使用-matlab教程！绝对手把手教你使用.rar
08-13
matlab教程绝对手把手教你使用-matlab教程！绝对手把手教你使用.rar matlab教程！绝对手把手教你使用
无人驾驶车辆模型预测直线轨迹跟踪控制MATLAB实现
03-06
此代码是关于无人驾驶车辆的直线轨迹跟踪模型预测控制算法实现
使用 MATLAB 和 Simulink 开发自动驾驶
12-14
本文主要介绍了如何使用 MATLAB 和 Simulink 这两套工具开发自动驾驶。
无人驾驶车辆模型预测控制matlab代码.zip_AACC-191684-FEABE_MATLAB无人驾驶_matlab车辆_无
07-15
《无人驾驶车辆模型预测控制》对应代码，添加注释，成功运行！实现了车辆无人驾驶控制的matlab仿真
【无人车】无人驾驶车辆模型预测控制（Matlab代码实现）
weixin_46039719的博客
01-11 1321
其次，以准确的动力学模型作为预测模型，可以提高控制器对车辆未来行为的预测能力，进而在保证车辆稳定运行的同时，充分发挥车辆的机动潜能，比如高速运行状况下的主动危险规避运动规划与控制，充分体现车辆平台运动学与动力学约束条件的影响。本章首先介绍无人车辆通用概念、无人驾驶车辆与无人车辆的关系，说明无人车辆的主要组成部分,即任务决策、环境感知、路径规划、路径跟踪与车辆平台控制子系统，总结。%计算第一个状态量误差。
无人机自动驾驶仪的 MatLab 仿真
qq_59747472的博客
09-15 148
在过去的几十年里，无人机技术取得了长足的发展。从最初的军事用途到如今的商业和个人应用，无人机已经成为了现代社会中不可或缺的一部分。然而，无人机的发展并不仅仅停留在机身结构和飞行控制系统的改进上，无人机自动驾驶仪的出现和发展更是为无人机技术带来了巨大的突破。无人机自动驾驶仪是一种能够自主控制无人机飞行的设备。它通过搭载各种传感器和计算机系统，能够实时感知周围环境并做出相应的飞行决策。与传统的遥控飞行相比，无人机自动驾驶仪能够大幅提高飞行的安全性和效率，同时也减轻了操作员的负担。
无人驾驶车辆自动驾驶局部路径规划matlab仿真设计
2301_76765778的博客
03-01 662
注意哦，包含有1.5万字左右的说明书 不仅仅是一段代码 说明书有很大的参考作用。（2）基于matlab软件对Astar和RRT算法进行仿真。无人驾驶车辆自动驾驶局部路径规划matlab仿真设计。（4）对改进的Astar算法进行仿真，证明其可行性。（1）建立Astar和RRT算法的逻辑。（3）分析结果，改进Astar算法。
【无人机】无人机自动驾驶仪（Matlab实现）
最新发布
你的Matlab小助手
05-20 402
无人机自动驾驶仪是一种用于控制和导航无人机的关键设备，它通过一系列传感器、计算机视觉技术、机器学习算法和实时数据处理来实现飞行任务的自主执行。自动驾驶仪与地面控制站之间通过通信链路进行数据交换和指令传递，地面操作人员可以通过地面控制站实时监控无人机的状态、调整任务参数、下达飞行指令等。无人机自动驾驶仪作为无人机系统中的核心部件，扮演着实现无人机自主飞行任务的关键角色。通过感知环境、规划路径、控制飞行等功能，它使得无人机能够在各种复杂环境下安全、高效地执行各类飞行任务，广泛应用于军事、民用、科研等领域。
手把手教用matlab做无人驾驶
weixin_47251251的博客
04-23 438
这里写自定义目录标题欢迎使用Markdown编辑器新的改变功能快捷键合理的创建标题，有助于目录的生成如何改变文本的样式插入链接与图片如何插入一段漂亮的代码片生成一个适合你的列表创建一个表格设定内容居中、居左、居右SmartyPants创建一个自定义列表如何创建一个注脚注释也是必不可少的KaTeX数学公式新的甘特图功能，丰富你的文章UML 图表FLowchart流程图导出与导入导出导入 欢迎使用Ma...
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值