【无标题】LQR倒立摆——matlab仿真笔记

最新推荐文章于 2023-04-24 23:45:04 发布

骑个大毛驴

最新推荐文章于 2023-04-24 23:45:04 发布

阅读量395

点赞数

分类专栏：现代控制理论文章标签： matlab 开发语言

本文链接：https://blog.csdn.net/qq_45801336/article/details/125473174

版权

现代控制理论专栏收录该内容

2 篇文章 0 订阅

订阅专栏

// A code block
var foo = 'bar';

clear all
syms g M m l s(t) J theta(t) f(t)
T1 = 1/2*M*diff(s,t)^2;
T2 = 1/2*J*diff(theta,t)^2 + 1/2*m*(diff(s+1/2*l*sin(theta),t)^2 + diff(1/2*l*cos(theta),t)^2);
%原dxp和dyp，这里直接带入了
V = m*g*1/2*l*cos(theta);
L = T1 + T2 - V;
eqn = functionalDerivative(L,[s;theta]) == [-f;0]
eqn = simplify(eqn)

syms M m l theta J dtheta F g
M=1;
m= 1;
l= 1;
%把上方方程写为矩阵形式 线性化后
Q = [M+m 0.5*m*l;0.5*m*l J+0.25*m*l^2];
J = 1/3*m*l^2;
%Q = [M+m 0.5*m*l;0.5*m*l J];
inv(Q)
%[s..theta..  ]
inv(Q)*[F;0.5*m*l*g*theta];
%化简
A1= [1 0;0 2*1/(m*l*g)];
inv(A1)*inv(Q)


A=[0 1 0 0;
   0 0 -5.88 0;
   0 0 0 1;
   0 0 23.52 0]
B=[0;0.8;0;-1.2]
C=[1 0 0 0;0 0 1 0]
D=[0;0]
%转化为状态空间模型模式
sys = ss(A,B,C,D)
%求解A的特征值
eig(A)
%判断是否可控，满秩则可控
Qc=ctrb(A,B)
rank(Qc)
%obsv是计算可观察性矩阵的函数
Qb=obsv(A,C)
rank(Qb)
Q = [1 0 0 0
     0 1 0 0
     0 0 10 0
     0 0 0 10];
%求控制器增益
%R权重 
R = 0.1;
K = lqr(A,B,Q,R)

Ac=A-B*K;
x0 = [0.1;0;0.1;0]; %初始状态
t = 0:0.05:20;
u = zeros(size(t));
[y,x]=lsim(Ac,B,C,D,u,t,x0); 
plot(t,y);