# 0.简介

Code: https://github.com/Cc19245/Inverted-pendulum.git

# 1.建立数学模型

## 1.1.牛顿运动定律分析

H = m x ¨ + m l ( θ ¨ cos ⁡ θ − θ ˙ 2 sin ⁡ θ ) \begin{aligned} H = m\ddot{x}+ml(\ddot{\theta}\cos{\theta}-\dot{\theta}^2\sin{\theta}) \end{aligned} H=mx¨+ml(θ¨cosθθ˙2sinθ)

V = m g − m l ( θ ¨ sin ⁡ θ + θ ˙ 2 cos ⁡ θ ) \begin{aligned} V = mg -ml(\ddot{\theta}\sin{\theta} + \dot{\theta}^2\cos\theta) \end{aligned} V=mgml(θ¨sinθ+θ˙2cosθ)

V l sin ⁡ θ − H l cos ⁡ θ = I θ ¨ \begin{aligned} Vl\sin\theta – Hl\cos\theta = I \ddot{\theta} \end{aligned} VlsinθHlcosθ=Iθ¨

## 1.2.欧拉-拉格朗日方程分析

T = 1 2 M x ˙ 2 + 1 2 I θ ˙ 2 + 1 2 m { [ d d t ( x + l sin ⁡ θ ) ] 2 + [ d d t ( l cos ⁡ θ ) ] } V = m g l cos ⁡ θ \begin{aligned} \begin{array}{l} T=\frac{1}{2} M \dot{x}^{2}+\frac{1}{2} I \dot{\theta}^{2}+\frac{1}{2} m\left\{\left[\frac{d}{d t}(x+l \sin \theta)\right]^{2}+\left[\frac{d}{d t}(l \cos \theta)\right]\right\}\\ V=mgl \cos \theta \end{array}\end{aligned} T=21Mx˙2+21Iθ˙2+21m{ [dtd(x+lsinθ)]2+[dtd(lcosθ)]}V=mglcosθ

[ M + m m l cos ⁡ θ m l cos ⁡ θ I + m l 2 ] [ x ¨ θ ¨ ] + [ 0 − m l sin ⁡ θ θ ˙ 0 0 ] [ x ˙ θ ˙ ] = [ u m g l sin ⁡ θ ] \begin{aligned} \left[\begin{array}{cc} M+m & m l \cos \theta \\ m l \cos \theta & I+m l^{2} \end{array}\right]\left[\begin{array}{c} \ddot{x} \\ \ddot{\theta} \end{array}\right]+\left[\begin{array}{cc} 0 & -m l \sin \theta \dot{\theta} \\ 0 & 0 \end{array}\right]\left[\begin{array}{c} \dot{x} \\ \dot{\theta} \end{array}\right]=\left[\begin{array}{c} u \\ m g l \sin \theta \end{array}\right] \end{aligned} [M+mmlcosθmlcosθI+ml2][x¨θ¨]+[00mlsinθθ˙0][x˙θ˙]=[umglsinθ]

# 4.在平衡点附近模型线性化

[ M + m m l m l I + m l 2 ] [ x ¨ θ ¨ ] = [ u m g l θ ] \begin{aligned} \left[\begin{array}{cc} M+m & m l \\ m l & I+m l^{2} \end{array}\right]\left[\begin{array}{c} \ddot{x} \\ \ddot{\theta} \end{array}\right]=\left[\begin{array}{c} u \\ m g l \theta \end{array}\right] \end{aligned} [M+mmlmlI+ml2][x¨θ¨]=[umglθ]

[ x ˙ x ¨ θ ˙ θ ¨ ] = [ 0 1 0 0 0 0 − m 2 g l 2 I ( M + m ) + M m l 2 0 0 0 0 1 0 0 m g l ( M + m ) I ( M + m ) + M m l 2 0 ] [ x x ˙ θ θ ˙ ] + [ 0 I + m l 2 I ( M + m ) + M m l 2 0 − m l I ( M + m ) + M m l 2 ] u y = x = [ 1 0 0 0 ] [ x x ˙ θ θ ˙ ] \begin{aligned} \begin{array}{c} {\left[\begin{array}{c} \dot{x} \\ \ddot{x} \\ \dot{\theta} \\ \ddot{\theta} \end{array}\right]=\left[\begin{array}{cccc} 0 & 1 & 0 & 0 \\ 0 & 0 & \dfrac{-m^{2} g l^{2}}{I(M+m)+M m l^{2}} & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & \dfrac{m g l(M+m)}{I(M+m)+M m l^{2}} & 0 \end{array}\right]\left[\begin{array}{c} x \\ \dot{x} \\ \theta \\ \dot{\theta} \end{array}\right]+\left[\begin{array}{c} 0 \\ \dfrac{I+m l^{2}}{I(M+m)+M m l^{2}} \\ 0 \\ \dfrac{-m l}{I(M+m)+M m l^{2}} \end{array}\right] u} \\ y=x=\left[\begin{array}{llll} 1 & 0 & 0 & 0 \end{array}\right]\left[\begin{array}{c} x \\ \dot{x} \\ \theta \\ \dot{\theta} \end{array}\right] \end{array}\end{aligned} x˙x¨θ˙θ¨=000010000I(M+m)+Mml2m2gl20I(M+m)+Mml2mgl(M+m)0010xx˙θθ˙+0I(M+m)+Mml2I+ml20I(M+m)+Mml2mluy=x=[1000]xx˙θθ˙

# 5.系统能控性、能观性和稳定性分析

## 5.1.能控性分析

M = 2;
m = 0.1;
g = 9.8;
l =0.5;
I = 1/3*m*l^2;

a23 = -m*m*g*l*l/(I*(m+M)+M*m*l*l);
a43 = m*g*l*(M+m)/(I*(m+M)+M*m*l*l);
b2 = (I+m*l*l)/(I*(m+M)+M*m*l*l);
b4 = -m*l/(I*(m+M)+M*m*l*l);

A = [0 1 0 0;
0 0 a23 0;
0 0 0 1;
0 0 a43 0];
B = [0;
b2;
0;
b4];
C =[1 0 0 0];

S_c = [B A*B A^2*B A^3*B]
fprintf('rank(S_c) = %d\n', rank(S_c));


## 5.2.能观性分析

Q_o = [C;
C*A;
C*A^2;
C*A^3];
fprintf('rank(Q_o) = %d\n', rank(Q_o));


## 5.3.稳定性分析

### 5.3.1.Routh-Hurwitz判据

Φ = C ( s I − A ) − 1 B \begin{aligned} \varPhi = C(sI-A)^{-1}B\end{aligned} Φ=C(sIA)1B

disp('eig(A)=');
eig(A)


### 5.3.2.Lyapunov函数

Lyapunov定理给出线性时不变系统在平衡点 x e = 0 x_e = 0 xe=0处渐进稳定的充分必要条件是对任意给定的对称正定矩阵 Q Q Q，存在一个对称正定矩阵 P P P，满足李雅普诺夫方程 A ⊤ P + P A = − Q \begin{aligned} A^\top P + PA = -Q \end{aligned} AP+PA=Q

P = lyap(A',eye(4))


# 6.基于极点配置方法的控制器设计

% 调节器的极点配置问题
J = [-1 -2 -1+j*sqrt(3) -1-j*sqrt(3)];
K1 = acker(A,B,J);
K2 = place(A,B,J);
disp('K1 = ');
disp(K1);
disp('K2 = ');
disp(K2);


# 7.状态观测器设计

## 7.1.全阶观测器

x ~ ˙ = ( A − K e C ) x ~ + B u + K e y \begin{aligned} \dot{\widetilde{x}} = (A-K_eC)\widetilde{x}+Bu+K_ey \end{aligned} x ˙=(AKeC)x +Bu+Key

A_ = A';
B_ = C';
C_ = B';
J_ = [-2 -6 -2-j*2*sqrt(3) -2+j*2*sqrt(3)];
Ke = (acker(A_, B_, J_))';
disp('Ke = ');
disp(Ke);


## 7.2.最小阶观测器

η ~ ˙ = A ^ η ~ + B ^ y + F ^ u \begin{aligned} \dot{\tilde{\eta}}=\hat{\boldsymbol{A}} \tilde{\boldsymbol{\eta}}+\hat{\boldsymbol{B}} y+\hat{\boldsymbol{F}} \boldsymbol{u}\end{aligned} η~˙=A^η~+B^y+F^u

x = [ x a x b ] = [ x x ˙ θ θ ˙ ] , A = [ 0 1 0 0 0 0 − 0.3630 0 0 0 0 1 0 0 15.2444 0 ] , B = [ 0 0.4938 0 − 0.7407 ] \begin{aligned} x = \left[ \begin{array}{c} x_a \\ \hdashline \boldsymbol {x_b} \end{array}\right] = \left[ \begin{array}{c} x \\ \hdashline \dot{x} \\ \theta \\ \dot{\theta} \end{array} \right] , A = \left[\begin{array}{c:ccc} 0 & 1 & 0 & 0 \\ \hdashline 0 & 0 & -0.3630 &0\\ 0 & 0 & 0 & 1 \\ 0 & 0 & 15.2444 & 0 \end{array}\right] , B = \left[ \begin{array}{c} 0 \\ \hdashline 0.4938 \\ 0 \\ -0.7407 \end{array} \right]\end{aligned} x=[xaxb]=xx˙θθ˙,A=0000100000.3630015.24440010,B=00.493800.7407

Aaa = A(1, 1);
Aab = A(1, 2:end);
Aba = A(2:end, 1);
Abb = A(2:end, 2:end);
J_j = [-6 -2+j*2*sqrt(3) -2-j*2*sqrt(3)];
Ke_j = (acker(Abb',Aab',J_j))';
disp('Ke_j = ');
disp(Ke_j);


# 8.基于观测器的状态反馈控制

## 8.1.基于全阶观测器的状态反馈控制

[ x ˙ e ˙ ] = [ A − B K 0 A − K e C ] [ x e ] \begin{aligned} \begin{bmatrix} \dot{x} \\ \dot{e} \end{bmatrix} = \begin{bmatrix} A & -BK \\ 0 & A-K_eC \end{bmatrix} \begin{bmatrix} x \\ e \end{bmatrix} \end{aligned} [x˙e˙]=[A0BKAKeC][xe]

% 一阶倒立摆系统的全阶观测器s-function建模
function [sys,x0,str,ts,simStateCompliance]=
order1_all_observer_sfun(t,x,u,flag, x_0, th_0)
switch flag,
case 0,
[sys,x0,str,ts,simStateCompliance]=
mdlInitializeSizes(t,x,u, x_0, th_0);
case 1,
sys=mdlDerivatives(t,x,u);
case 2,
sys=mdlUpdate(t,x,u);
case 3,
sys=mdlOutputs(t,x,u);
case 4,
sys=mdlGetTimeOfNextVarHit(t,x,u);
case 9,
sys=mdlTerminate(t,x,u);
otherwise
num2str(flag));
end
% 主函数结束

% ---------------------------------------------
function [sys,x0,str,ts,simStateCompliance]=
mdlInitializeSizes(t,x,u,x_0, th_0)
% 初始化
sizes = simsizes;% 生成sizes数据结构

sizes.NumContStates  = 4;% 连续状态数, 分别是x', theta1', theta2', x, theta1, theta2
sizes.NumDiscStates  = 0;% 离散状态数,缺省为 0
sizes.NumOutputs     = 4;% 输出量个数,缺省为 0,
sizes.NumInputs      = 1;% 输入量个数，缺省为 0, 这里的输入取为e
sizes.DirFeedthrough = 1;% 是否存在直接馈通。1：存在；0：不存在，缺省为 1
sizes.NumSampleTimes = 1;   % at least one sample time is needed

sys = simsizes(sizes);
x0  = [x_0; 0; th_0; 0];% 设置初始状态
str = [];% 保留变量置空
ts  = [0 0]; % 连续系统
simStateCompliance = 'UnknownSimState';
% end mdlInitializeSizes

% ---------------------------------------------
function sys=mdlDerivatives(t, x, u)
%  计算导数例程子函数
M = 2; m = 0.1; l =0.5; I = 1/3*m*l^2; g = 9.8;
a23 = -m*m*g*l*l/(I*(m+M)+M*m*l*l);
a43 = m*g*l*(M+m)/(I*(m+M)+M*m*l*l);
b2 = (I+m*l*l)/(I*(m+M)+M*m*l*l);
b4 = -m*l/(I*(m+M)+M*m*l*l);
A = [0 1 0 0;
0 0 a23 0;
0 0 0 1;
0 0 a43 0];
b = [0; b2; 0; b4];
K = [-1.1020, -2.2041, -37.5147, -8.2194];
Ke = [12; 75.2; -988.9; -3689.2];
sys = (A-b*K)*x + Ke*u ;  % 注意这里的u就是x(1)的观测误差e(1)

% ---------------------------------------------
function sys=mdlUpdate(t,x,u)
%3. 状态更新例程子函数
sys = [];

% ---------------------------------------------
function sys=mdlOutputs(t,x,u)
%4. 计算输出例程子函数
sys=x;

% ---------------------------------------------
function sys=mdlGetTimeOfNextVarHit(t,x,u)
% 5. 计算下一个采样时间，仅在系统是变采样时间系统时调用
sampleTime = 1;    %  Example, set the next hit to be one second later.
sys = t + sampleTime;

% ---------------------------------------------
function sys=mdlTerminate(t,x,u)
% 6. 仿真结束时要调用的例程函数
sys = [];


## 8.2.基于最小观测器的状态反馈控制

[ x ˙ e ˙ ] = [ A − B K B K b 0 A b b − K e A a b ] [ x e ] \begin{aligned} \begin{bmatrix} \dot x \\ \dot{e} \end{bmatrix} = \begin{bmatrix} A – BK & BK_b \\ 0 & A_{bb} – K_eA_{ab} \end{bmatrix} \begin{bmatrix} x \\ e \end{bmatrix}\end{aligned} [x˙e˙]=[ABK0BKbAbbKeAab][xe]

% 一阶倒立摆系统的最小阶观测器s-function建模
function [sys,x0,str,ts,simStateCompliance] =
order1_min_observer_sfun(t,x,u,flag, x_0, th_0)
switch flag,
case 0,
[sys,x0,str,ts,simStateCompliance]=
mdlInitializeSizes(t,x,u, x_0, th_0);
case 1,
sys=mdlDerivatives(t,x,u);
case 2,
sys=mdlUpdate(t,x,u);
case 3,
sys=mdlOutputs(t,x,u);
case 4,
sys=mdlGetTimeOfNextVarHit(t,x,u);
case 9,
sys=mdlTerminate(t,x,u);
otherwise
num2str(flag));
end
% 主函数结束

% ---------------------------------------------
function [sys,x0,str,ts,simStateCompliance]=
mdlInitializeSizes(t,x,u,x_0, th_0)
% 初始化
sizes = simsizes;% 生成sizes数据结构

sizes.NumContStates  = 3;% 连续状态数
sizes.NumDiscStates  = 0;% 离散状态数,缺省为 0
sizes.NumOutputs     = 4;% 输出量个数,缺省为 0,
sizes.NumInputs      = 2;% 输入量个数，缺省为 0, 这里的输入取为输入u和y
sizes.DirFeedthrough = 1;% 是否存在直接馈通。1：存在；0：不存在，缺省为 1
sizes.NumSampleTimes = 1;   % at least one sample time is needed
sys = simsizes(sizes);
x0  = [0; th_0; 0] - [10; -152.2041; -684.4898]*x_0;  % 设置初始状态
str = [];% 保留变量置空
ts  = [0 0]; % 连续系统
simStateCompliance = 'UnknownSimState';
% end mdlInitializeSizes

% ---------------------------------------------
function sys=mdlDerivatives(t, x, u)
%  计算导数例程子函数
M = 2; m = 0.1; l =0.5; I = 1/3*m*l^2; g = 9.8;
a23 = -m*m*g*l*l/(I*(m+M)+M*m*l*l);
a43 = m*g*l*(M+m)/(I*(m+M)+M*m*l*l);
b2 = (I+m*l*l)/(I*(m+M)+M*m*l*l);
b4 = -m*l/(I*(m+M)+M*m*l*l);
A = [0 1 0   0;
0 0 a23 0;
0 0 0   1;
0 0 a43 0];
B = [0; b2; 0; b4];
Ke_j = [10;  -152.2041; -684.4898];
A_hat = A(2:end, 2:end) - Ke_j*A(1, 2:end);
B_hat = A_hat*Ke_j + A(2:end, 1) - Ke_j*A(1,1);
F_hat = B(2:end) - Ke_j*B(1);
sys = A_hat*x + B_hat*u(2) + F_hat*u(1) ;  % u1就是输入，u2是原系统输出y

% ---------------------------------------------
function sys=mdlUpdate(t,x,u)
%3. 状态更新例程子函数
sys = [];

% ---------------------------------------------
function sys=mdlOutputs(t,x,u)
%4. 计算输出例程子函数
C_hat = [0 0 0; 1 0 0; 0 1 0; 0 0 1];
D_hat = [1; 10; -152.2041; -684.4898];
sys = C_hat*x + D_hat*u(2);

% ---------------------------------------------
function sys=mdlGetTimeOfNextVarHit(t,x,u)
% 5. 计算下一个采样时间，仅在系统是变采样时间系统时调用
sampleTime = 1;    %  Example, set the next hit to be one second later.
sys = t + sampleTime;

% ---------------------------------------------
function sys=mdlTerminate(t,x,u)
% 6. 仿真结束时要调用的例程函数
sys = [];


# 9.LQR控制

LQR问题就是确定下列最佳控制向量的矩阵 K K K u ( t ) = − K x ( t ) \begin{aligned} u(t)=-Kx(t)\end{aligned} u(t)=Kx(t) 使得下列性能指标达到最小值：
J = ∫ 0 ∞ [ x ⊤ Q x + u ⊤ R u ] d t \begin{aligned} J = \int_0^\infty [\boldsymbol x^\top\boldsymbol{Qx} + \boldsymbol u^\top \boldsymbol{Ru}]dt\end{aligned} J=0[xQx+uRu]dt

Q = eye(4);
R = eye(1);
K_lqr = lqr(A,B,Q,R);
disp('K_lqr = ');
disp(K_lqr);


原文作者：Cc1924
原文地址: https://blog.csdn.net/qq_42731705/article/details/122464642
本文转自网络文章，转载此文章仅为分享知识，如有侵权，请联系博主进行删除。