LQR、LQR-MPC、GP-MPC控制倒立摆
LQR控制倒立摆:
倒立摆状态方程:
目标任务:
模型参数:
%% LQR for the cart-pole systemload('cp_params.mat');
syms phi phid x xd I u
fr(1) = 0.2; % friction magnitude
Ts = 0.01;
Duration = 2.5;
Q = [100 0 1 0]; % LQ weights
R = 0.0001;q1 = [phi; phid; x; xd];
q1 = cp_dynmodel(q1,u,par,fr);Asym = jacobian(q1,[phi;phid;x;xd]);
A = double(subs(Asym,[phi;phid;x;xd;u],[0;0;0;0;0]));
Bsym = jacobian(q1,u);
B = double(subs(Bsym,[phi;phid;x;xd;u],[0;0;0;0;0]));
A = eye(4)+Ts*A;
B = Ts*B;
clear phi phid x xd I u k1 k2 k3 k4 q1%% LQR
C = diag([1,1,1,1]);
D = zeros(4,1);
K = dlqr(A,B,diag(Q),R);%% Simulation
qLQR = zeros(5,Duration/Ts+1);
q = [0.2;0;0;0];
qLQR(:,1) = [q; 0];
for i = 1 : Duration/Tsu = -K*q;[~,q] = ode45(@(t,q) cp_dynmodel(q,u,par,fr),[0 Ts/2 Ts],q);q = q(3,:)';qLQR(:,i+1) = [q; u];
end
%% Plots
plotTraj(qLQR,Ts)
首先非线性模型通过雅可比矩阵线性化:
Asym = jacobian(q1,[phi;phid;x;xd]);
A = double(subs(Asym,[phi;phid;x;xd;u],[0;0;0;0;0]));
Bsym = jacobian(q1,u);
B = double(subs(Bsym,[phi;phid;x;xd;u],[0;0;0;0;0]));
离散化模型:
A = eye(4)+Ts*A;
B = Ts*B;
通过LQR计算出反馈矩阵K值:
K = dlqr(A,B,diag(Q),R);
仿真,通过
%% Simulation
qLQR = zeros(5,Duration/Ts+1);
q = [0.2;0;0;0];
qLQR(:,1) = [q; 0];
for i = 1 : Duration/Tsu = -K*q;[~,q] = ode45(@(t,q) cp_dynmodel(q,u,par,fr),[0 Ts/2 Ts],q);q = q(3,:)';qLQR(:,i+1) = [q; u];
end
%% Plots
plotTraj(qLQR,Ts)
结果:
LQR-MPC控制倒立摆:
这里就是使用mpc产生输入补偿LQR输入:
input = fmincon(@(u) cost_func_lin(u,horizon,q,Q,R,A2,B),input,[],...[],[],[],(K*q-40)*ones(horizon,1),(K*q+40)*ones(horizon,1),[],options);u = -K*q + input(1);
%% MPC for the cart-pole systemload('cp_params.mat');
syms phi phid x xd I u
fr(1) = 0.2; % friction magnitude
Ts = 0.01;
horizon = 25;
Duration = 10;
Q = [100 0 1 0]; % MPC weights
R = 0.0001;q1 = [phi; phid; x; xd];
q1 = cp_dynmodel(q1,u,par,fr);Asym = jacobian(q1,[phi;phid;x;xd]);
A = double(subs(Asym,[phi;phid;x;xd;u],[0;0;0;0;0]));
Bsym = jacobian(q1,u);
B = double(subs(Bsym,[phi;phid;x;xd;u],[0;0;0;0;0]));
A = eye(4)+Ts*A;
B = Ts*B; %离散化
clear phi phid x xd I u k1 k2 k3 k4 q1%% LQ feedback gain
C = diag([1,1,1,1]);
D = zeros(4,1);
K = dlqr(A,B,diag(Q),R);%% Simulation
A2 = A-B*K;
u = 0;
qlinMPC = zeros(5,Duration/Ts+1);
q = [0.2; 0; 0; 0]; % initial condition
qlinMPC(:,1) = [q; u];
input = zeros(horizon,1);
options = optimoptions('fmincon','Algorithm','sqp');for i = 1: Duration / Tsinput = fmincon(@(u) cost_func_lin(u,horizon,q,Q,R,A2,B),input,[],...[],[],[],(K*q-40)*ones(horizon,1),(K*q+40)*ones(horizon,1),[],options);u = -K*q + input(1);[~,q] = ode45(@(t,q) cp_dynmodel(q,u,par,fr),[0 Ts/2 Ts],q);q = q(3,:)';qlinMPC(:,i+1) = [q; u];
end%% Plots
plotTraj(qlinMPC,Ts)
关于GPR参考资料:
Gaussian process 的重要组成部分——关于那个被广泛应用的Kernel的林林总总 - 知乎
Documentation for GPML Matlab Code
%% GPMPC for cart-pole dynamic system
addpath '..\TDK_2020\gp' % write path of the containing folder
addpath '..\TDK_2020\util'
load('cp_params.mat');
load('cp_trainingpoints.mat');
fr(1) = 0.2;
Ts = 0.01;
horizon = 25;
Duration = 2.5;
Q = [100 0 1 0];
R = 0.0001;
%% GP training
gpmodel.inputs = xtrain';
gpmodel.targets = ytrain';
[gpmodel2, nlml] = train(gpmodel, 1, 100);
%% Precomputation of GP matrices and vectors
N = length(xtrain(1,:));
L = zeros(4,4,4);
Kxx = zeros(N,N,4);
Kinv = Kxx;
alpha = zeros(N,1,4);
sf = 0.1;
sn = 0.01;
for k = 1:4
L(:,:,k) = diag(1./exp(2*gpmodel2.hyp(1:4,k)));
for i=1:N
for j=1:N
Kxx(i,j,k)=sf^2*kernel(xtrain(:,i),xtrain(:,j),L(:,:,k));
end
end
alpha(:,:,k) = (Kxx(:,:,k)+sn^2*eye(N))\ytrain(k,:)';
Kinv(:,:,k) = inv(Kxx(:,:,k)+sn^2*eye(N));
end
%% Discretisation, linearisation
syms phi phid x xd I u
q1 = [phi; phid; x; xd];
q1 = cp_dynmodel(q1,u,par,fr);
Asym = jacobian(q1,[phi;phid;x;xd]);
A = double(subs(Asym,[phi;phid;x;xd;u],[0;0;0;0;0]));
Bsym = jacobian(q1,u);
B = double(subs(Bsym,[phi;phid;x;xd;u],[0;0;0;0;0]));
A = eye(4)+Ts*A;
B = Ts*B;
clear phi phid x xd I u k1 k2 k3 k4 q1
%% LQR
C = diag([1,1,1,1]);
D = zeros(4,1);
K = dlqr(A,B,diag(Q),R);
%% GPMPC Simulation
A2 = A-B*K;
q = [0.2; 0; 0; 0]; % initial condition
qGPMPC = zeros(5,Duration/Ts+1);
qGPMPC(1:4,1) = q;
input = zeros(horizon,1);
options = optimoptions('fmincon','Algorithm','sqp');
for i = 1: Duration / Ts
input = fmincon(@(u) cost_func_GP(u,horizon,q,Q,R,A2,B,xtrain,L,alpha,Kinv),...
input,[],[],[],[],(K*q-40)*ones(horizon,1),(K*q+40)*ones(horizon,1),[],options);
u = -K*q + input(1);
[~,q] = ode45(@(t,q) cp_dynmodel(q,u,par,fr),[0 Ts/2 Ts],q);
q = q(3,:)';
qGPMPC(:,i+1) = [q; u];
end
%% Plots
plotTraj(qGPMPC,Ts)
%% Plot the prediction at a certain time instant and the simulation
% results at that time interval
niter = horizon;
start = 12; % t = 0.12 s
mean = zeros(4,niter+1);
mean(:,1) = qGPMPC(1:4,start);
u = qGPMPC(5,start);
Ktest = zeros(1,length(xtrain(1,:)));
M = zeros(4,niter);
S = M;
N = length(xtrain(1,:));
for iter = 1:niter
for k = 1:4
for j=1:N
Ktest(1,j,k)=sf^2*kernel(mean(:,iter),xtrain(:,j),L(:,:,k));
end
M(k,iter) = Ktest(:,:,k)*alpha(:,:,k);
S(k,iter) = sf^2 - Ktest(:,:,k)*Kinv(:,:,k)*Ktest(:,:,k)';
end
mean(:,iter+1) = A*mean(:,iter) + B*u + M(:,iter);
end
tsimu = start*Ts:Ts:(start+horizon-1)*Ts;
figure()
subplot(2,1,1)
hold on
plot(tsimu,qGPMPC(1,start:start+niter-1),'r','LineWidth',1)
errorbar(tsimu,mean(1,1:25),2*sqrt(S(1,:)),'k','LineWidth',1)
xlabel('$t$ (s)')
ylabel('$\varphi$ (rad)')
legend('Simulation result','Prediction at $t=0.12$ s')
hold off
subplot(2,1,2)
hold on
plot(tsimu,qGPMPC(3,start:start+niter-1),'r','LineWidth',1)
errorbar(tsimu,mean(3,1:25),2*sqrt(S(3,:)),'k','LineWidth',1)
xlabel('$t$ (s)')
ylabel('$x$ (m)')
hold off
具体代码:
MPC/LQR. LQR-MPC.GP-MPC.rar at main · caokaifa/MPC · GitHub
LQR、LQR-MPC、GP-MPC控制倒立摆相关推荐
- MATLAB强化学习实战(七) 在Simulink中训练DDPG控制倒立摆系统
在Simulink中训练DDPG控制倒立摆系统 倒立摆的Simscape模型 创建环境接口 创建DDPG智能体 训练智能体 DDPG智能体仿真 此示例显示了如何训练深度确定性策略梯度(DDPG)智能体 ...
- 【深度强化学习】神经网络、爬山法优化控制倒立摆问题实战(附源码)
需要源码请点赞关注收藏后评论区留言私信~~~ 直接优化策略 直接优化策略强化学习算法中,根据采用的是确定性策略还是随机性策略,又分为确定性策略搜索和随机性策略搜索两类.随机性策略搜索算法有策略梯度法和 ...
- 【Simscape】用Simscape实现三维物理仿真(四)——用PID控制倒立摆系统
仿真文件下载:https://download.csdn.net/download/ReadAir/12232591 1 建立一个倒立摆系统 我们使用[Simscape]用Simscape实现三维物理 ...
- matlab控制倒立摆小车并绘制二维动态效果图
clc;close all;clear A = [0 1 0 0;0 0 -1.176 0;0 0 0 1;0 0 18.293 0];%设置倒立摆小车控制系统参数 B = [0; 1 ;0;-1.6 ...
- 【控制理论】用ADRC控制倒立摆
- 倒立摆的实现 5.步进电机的控制
步进电机的控制 先是步进电机的头文件 /*------------------------------------------------------------------------| 文件名 : ...
- OpenAI Gym 经典控制环境介绍——CartPole(倒立摆)
摘要:OpenAI Gym是一款用于研发和比较强化学习算法的工具包,本文主要介绍Gym仿真环境的功能和工具包的使用方法,并详细介绍其中的经典控制问题中的倒立摆(CartPole-v0/1)问题.最后针 ...
- MFC的固高环形倒立摆GRIP2002实验平台
固高环形倒立摆GRIP2002是基于GT-400-SV-PCI运动控制卡的一个二级环形倒立摆(摆杆和连杆两根杆的摆),固高公司提供了一个DOS环境下的Demo和MATLAB 7.0的simulink的 ...
- SimMechanics/Second Generation倒立摆模型建立及初步仿真学习
笔者最近捣鼓Simulink,发现MATLAB的仿真模块真的十分强大,以前只是在命令窗口敲点代码,直到不小心敲入simulink,就一发不可收拾.话说simulink的模块化建模确实方便,只要拖拽框框 ...
- 【双足机器人(3)】3D线性倒立摆Python仿真(附代码)
往期 本文是双足机器人系列的第三篇,在前面的文章中我们介绍了2D线性倒立摆的基本理论,详见: [双足机器人(1)]线性倒立摆及其运动控制(附代码) 在这篇文章中我们要详细介绍3D线性倒立摆的基本内容, ...
最新文章
- Java 全能手册火了!Redis/Nginx/Dubbo/Spring 全家桶/啥都有
- 华为交换机配置命令 华为QuidWay交换机配置命令手册
- oracle 事务故障,处理Oracle的分布式事务故障
- php通过Mysqli和PDO连接mysql数据详解
- 用Python评测三种批量查询经纬度的方法,你pick哪一种?
- php安装编译时 configure: error: Cannot find OpenSSL's evp.h
- 经典面试题之 TCP三次握手 和 TCP四次挥手过程----详解
- PTA|团体程序设计天梯赛-练习集|JAVA版
- 数学建模论文注意事项
- 禁用win10触摸屏手势_怎么修改win10 触控板手势设置
- 网利友联CEO王卫平受邀坐客飞官七直播间
- 苹果手机为什么Apple ID会被停用
- 中国车牌归属地数据库
- 手把手教会你视频转文字怎么弄,这个方法建议收藏备用
- JimuReport 1.4.0-beta 里程碑版本发布,免费的低代码报表
- 用荧光素酶基因标记肿瘤细胞的实验步骤
- [工具]更新音乐下载网站,MP3音乐无损音乐下载器
- 研发质量管理---(1)质量管理总结
- innerHTML与innerText与outHTML与outText 的区别。
- 论文阅读笔记 Jointly Attentive Spatial-Temporal Pooling Networks for Video-based Person Re-Identification