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

代码段

% 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+1AX((i-1)*n+1:i*n,:)=A^(i-1);
end
for i=1:N+1for j=1:Nif i>jBU((i-1)*n+1:i*n,(j-1)*m+1:j*m)=A^(i-j-1)*B;elseBU((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-1QX=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:NTxk=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+1AX((i-1)*n+1:i*n,:)=A^(i-1);
end
for i=1:N+1for j=1:Nif i>jBU((i-1)*n+1:i*n,(j-1)*m+1:j*m)=A^(i-j-1)*B;elseBU((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-1QX=blkdiag(QX,Q); RU=blkdiag(RU,R);
end
QX=blkdiag(QX,QN);
H=blkdiag(QX,RU);% simulating system with MPC
for k=1:NTxk=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+1AX((i-1)*n+1:i*n,:)=A^(i-1);for j=1:Nif i>jBU((i-1)*n+1:i*n,(j-1)*m+1:j*m)=A^(i-j-1)*B;elseBU((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-1QX=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:NTxk=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+1AX((i-1)*n+1:i*n,:)=A^(i-1);
end
for i=1:N+1for j=1:Nif i>jBU((i-1)*n+1:i*n,(j-1)*m+1:j*m)=A^(i-j-1)*B;elseBU((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-1QX=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:NTxk=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

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

手把手教用matlab做无人驾驶（二十五）--基于控制障碍函数的MPC（MPC-CBF）
这边博客主要完成论文的内容:Safety-Critical Model Predictive Control with Discrete-Time Control Barrier Function,论 ...
手把手教用matlab做无人驾驶（八）-无迹Kalman滤波算法
1.KF.EKF.UKF都是高斯滤波,下面的介绍一下他们的优缺点: KF优点:计算简单 KF缺点:高斯线性模型约束 EKF优点:可以近似非线性问题 EKF缺点:高斯噪声约束,线性化引入了误差会可 ...
足疗师手把手教你家中做足疗
足疗师手把手教你家中做足疗一.含苞未放:把脚擦干,之后涂抹润肤油; 二.金鱼摆尾:双手横向拍打双脚外侧,起到放松小腿肌肉的作用; 三.隔墙有耳:双手握住一只脚,向内稍用力挤压; 四.仙鹤展翅:双手在 ...
手把手教你给 SSH 启用二次身份验证
目前来说,二次验证是比较常用的安全手段,通过设置二次验证,就可以有效的避免账户密码的泄露导致的安全问题.因为,每次登陆前都需要获取一次性验证码,如果没有验证码的话就无法成功登陆. 1.安装 PAM 模 ...
手把手教你如何做自媒体赚钱，揭开自媒体赚钱真相！记得收藏
这真是一个好时代,一个人人都是自媒体的时代,一个信息爆炸的时代. 自媒体变现,几乎人人都可以实现,离我们并不远,就在你我身边,普通人都可以通过自媒体赚钱. 手把手教你如何做自媒体赚钱,揭开自媒体赚钱真 ...
2019-1-30手把手教你怎么用AbaqusGUI二次开发攻略
2019年1月30日13:05:51 手把手教你怎么用Abaqus进行GUI二次开发攻略 1.前言: 最近研究了下GUI二次开发,做了如下几个插件,学到了一些东西特地就跟大家做个分享,其中插件注册到了 ...
手把手教你如何做建模竞赛（baseline代码讲解）
点击上方"视学算法",选择加"星标"或"置顶" 重磅干货,第一时间送达 1.大赛背景随着科技发展,银行陆续打造了线上线下.丰富多样的客户触 ...
echarts formatter_手把手教你玩转echarts(二)折线图
茫茫人海中与你相遇相信未来的你不会很差作者:婷酱Yaaa 来源:https://juejin.im/post/5f0292d35188252e5a5dbed0 前言哈喽,everybody,我又 ...
手把手教你如何做网线接头
教你如何做网线接头:站长不会做网线就太丢脸了. 教你如何做网线接头:作为站长,不会做网线就太丢脸了.目前局域网构建已经十分普遍,计算机上集成以太网卡已经是标准配置,局域网的踪影在我们周围无处不在,简单 ...
手把手教你从零开始做一个好看的 APP
前言从零开始,手把手带你实现一个「专注睡前的 APP」.睡觉之前如果能有一个 APP,能让我们写一写这一天的见闻或者心得,同时又能看一会段子.瞄一会好看的妹子,放松一下疲惫的身心那该多好,这也是我完 ...

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

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

最新文章

热门文章