1 问题建模
首先对待研究的问题建立数学模型
在倒立摆模型分析这篇文章里,我们已经做了完整的受力分析。最终得到了关于系统变量的微分方程。
(
M
+
m
)
x
¨
+
b
x
˙
−
m
l
ψ
¨
=
u
(M+m)\ddot{x}+b\dot{x}-ml\ddot{\psi}= u
(M+m)x¨+bx˙−mlψ¨=u
(
I
+
m
l
2
)
ψ
¨
−
m
g
l
ψ
=
m
l
x
¨
(I+ml^2)\ddot{\psi}-mgl\psi = ml\ddot{x}
(I+ml2)ψ¨−mglψ=mlx¨
2 状态空间
可以将状态空间理解为一个包含系统输入、系统输出和状态变量的集合,它们之间的关系可以用一个一阶微分方程表达出来。
状
态
空
间
(
集
合
)
=
{
系
统
输
入
系
统
输
出
状
态
变
量
}
=
一
阶
微
分
方
程
状态空间(集合)=\left\{ \begin{aligned} 系统输入\\ 系统输出\\ 状态变量 \end{aligned} \right\}=一阶微分方程
状态空间(集合)=⎩⎪⎨⎪⎧系统输入系统输出状态变量⎭⎪⎬⎪⎫=一阶微分方程
通过观察我们知道,目前系统模型是以二阶微分方程的形式描述的。为了消除方程中的高阶项,可以整理得到下面的式子。
{
x
˙
=
x
˙
x
¨
=
m
2
l
2
g
I
(
m
+
M
)
+
m
M
l
2
ψ
−
b
(
I
+
m
l
2
)
I
(
m
+
M
)
+
m
M
l
2
x
˙
+
(
I
+
m
l
2
)
I
(
m
+
M
)
+
m
M
l
2
u
ψ
˙
=
ψ
˙
ψ
¨
=
m
l
g
(
m
+
M
)
I
(
m
+
M
)
+
m
M
l
2
ψ
−
m
l
b
I
(
m
+
M
)
+
m
M
l
2
x
˙
+
m
l
I
(
m
+
M
)
+
m
M
l
2
u
\left\{\begin{array}{l} \dot{x}=\dot{x}\\ \ddot{x}=\frac{m^2l^2g}{I(m+M)+mMl^2} \psi-\frac{b(I+ml^2)}{I(m+M)+mMl^2} \dot{x}+\frac{(I+ml^2)}{I(m+M)+mMl^2}u\\ \dot{\psi}=\dot{\psi}\\ \ddot{\psi}=\frac{mlg(m+M)}{I(m+M)+mMl^2}\psi-\frac{mlb}{I(m+M)+mMl^2}\dot{x}+\frac{ml}{I(m+M)+mMl^2}u \end{array}\right.
⎩⎪⎪⎪⎨⎪⎪⎪⎧x˙=x˙x¨=I(m+M)+mMl2m2l2gψ−I(m+M)+mMl2b(I+ml2)x˙+I(m+M)+mMl2(I+ml2)uψ˙=ψ˙ψ¨=I(m+M)+mMl2mlg(m+M)ψ−I(m+M)+mMl2mlbx˙+I(m+M)+mMl2mlu
已知系统状态空间方程的标准形式为:
{
x
˙
=
A
x
+
B
u
y
=
C
x
+
D
u
\left\{\begin{array}{l} \dot{x}=Ax+Bu\\ y=Cx+Du \end{array}\right.
{x˙=Ax+Buy=Cx+Du
式中
x
˙
\dot{x}
x˙表示系统中的一阶微分项,
y
y
y表示系统状态。以矩阵运算的形式表示系统的状态空间方程:
[
x
˙
x
¨
ψ
˙
ψ
¨
]
=
[
0
1
0
0
0
−
b
(
I
+
m
l
2
)
I
(
m
+
M
)
+
m
M
l
2
m
2
l
2
g
I
(
m
+
M
)
+
m
M
l
2
0
0
0
0
1
0
−
m
l
b
I
(
m
+
M
)
+
m
M
l
2
m
l
g
(
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
\begin{bmatrix} \dot{x}\\ \ddot{x}\\ \dot{\psi}\\ \ddot{\psi} \end{bmatrix}= \begin{bmatrix} 0 & 1 & 0 & 0\\ 0 & -\frac{b(I+ml^2)}{I(m+M)+mMl^2} & \frac{m^2l^2g}{I(m+M)+mMl^2} & 0\\ 0 & 0 & 0 & 1 \\ 0 & -\frac{mlb}{I(m+M)+mMl^2} & \frac{mlg(m+M)}{I(m+M)+mMl^2} & 0 \end{bmatrix} \times \begin{bmatrix} x\\ \dot{x}\\ \psi\\ \dot{\psi} \end{bmatrix} + \begin{bmatrix} 0\\ \frac{(I+ml^2)}{I(m+M)+mMl^2}\\ 0\\ \frac{ml}{I(m+M)+mMl^2} \end{bmatrix} u
⎣⎢⎢⎡x˙x¨ψ˙ψ¨⎦⎥⎥⎤=⎣⎢⎢⎢⎡00001−I(m+M)+mMl2b(I+ml2)0−I(m+M)+mMl2mlb0I(m+M)+mMl2m2l2g0I(m+M)+mMl2mlg(m+M)0010⎦⎥⎥⎥⎤×⎣⎢⎢⎡xx˙ψψ˙⎦⎥⎥⎤+⎣⎢⎢⎢⎡0I(m+M)+mMl2(I+ml2)0I(m+M)+mMl2ml⎦⎥⎥⎥⎤u
y
=
[
1
0
0
0
0
0
1
0
]
×
[
x
x
˙
ψ
ψ
˙
]
+
[
0
]
×
u
y= \begin{bmatrix} 1 & 0 & 0 & 0\\ 0 & 0 & 1 & 0 \end{bmatrix} \times \begin{bmatrix} x\\ \dot{x}\\ \psi\\ \dot{\psi} \end{bmatrix} + \begin{bmatrix} 0 \\ \end{bmatrix} \times u
y=[10000100]×⎣⎢⎢⎡xx˙ψψ˙⎦⎥⎥⎤+[0]×u
3 LQR控制器设计
L(Linear)Q(Quadratic)R(Regulator),直译为线性二次型控制器。
可以通过加入反馈,使系统最终能达到稳定状态,但如何选取最好的系统特征值(极点)在系统收敛的前提下实现最优的收敛过程,这是我们目前要考虑的问题。
这里我们引入目标函数(价值函数),使系统在收敛的同时,满足
J
J
J最小:
J
=
∫
t
0
t
f
[
X
T
Q
X
+
U
T
R
U
]
d
t
m
i
n
(
J
)
J=\int ^{t_f}_{t_0}[X^TQX+U^TRU]dt\\ min(J)
J=∫t0tf[XTQX+UTRU]dtmin(J)
-
Q
Q
Q是一个对角矩阵,
X
T
Q
X
=
a
x
1
2
+
b
x
2
2
+
c
x
3
2
+
.
.
.
.
.
.
X^TQX=ax_1^2+bx_2^2+cx_3^2+......
XTQX=ax12+bx22+cx32+......,当系统中的状态变量
x
≠
0
x≠0
x=0时,我们可以通过调节
Q
Q
Q中元素的值来改变该变量的对
J
J
J的影响。
Q
Q
Q中较大的一项对应在收敛过程中优先考虑的系统变量。
- 同理,
U
T
R
U
U^TRU
UTRU则代表了系统输入
U
U
U对价值函数
J
J
J的影响。因为
J
J
J是积分的形式,当矩阵
R
R
R中某一项较大,则意味着我们希望该项对应的系统输入能够快速收敛到0。这么做的现实意义往往是以最小的代价(例如能耗)实现系统的稳态。
式子中代表系统输入的
U
U
U在一个能够实现自稳定的系统中(例如我们这里设计的倒单摆系统)代表控制器反馈回路的输出。
们目前涉及的倒立摆系统只有一个输入,即倒立摆所受到的外部牵引力
U
U
U,所以矩阵
R
R
R仅有一个元素。另外有四个系统状态变量,分别是
x
,
x
˙
,
ψ
,
ψ
˙
x,\dot{x},\psi,\dot{\psi}
x,x˙,ψ,ψ˙,因此对角矩阵
Q
Q
Q的规模为
4
×
4
4\times4
4×4。
在MATLAB中,我们可以调用
l
q
r
(
)
lqr()
lqr()函数生成满足
J
J
J最小的反馈矩阵
K
K
K。
K
K
K对应的就是各个系统变量反馈路径中的增益。即:
U
=
[
K
1
,
K
2
,
K
3
,
K
4
]
×
[
x
x
˙
ψ
ψ
˙
]
U=[K_1, K_2, K_3, K_4]\times\begin{bmatrix} x\\ \dot{x}\\ \psi\\ \dot{\psi} \end{bmatrix}
U=[K1,K2,K3,K4]×⎣⎢⎢⎡xx˙ψψ˙⎦⎥⎥⎤
下图是倒立摆开环系统的阶跃响应。显然系统是不收敛的。
引入LQR反馈后系统的阶跃响应
4 MATLAB 代码
clc;
clear;
close all;
% Parameters:
% m: mass of pendulum (kg)
% M: mass of cart (kg)
% b: dampling coefficient
% I: rotional inertia
% g: acceleration of gravity
% L: the distance from mass center to the hinge
m = 3.375;
M = 5.40;
b = 0.01;
I = 0.0703125;
g = 9.80665;
L = 0.25;
% create transfer function model
s = tf('s');
q = (m + M) * (I + m * L^2) - (m * L)^2;
P_cart = (((I + m * L^2) / q) * s^2 - (m * g * L / q)) / ...
(s^4 + (b * (I + m * L^2)) * s^3 / q - ((M + m) * m * g * L) * s^2 / q - b * m * g * L * s / q);
P_pend = (m * L * s / q) / ...
(s^3 + (b * (I + m * L^2)) * s^2 / q - ((M + m) * m * g * L) * s / q - b * m * g * L / q);
sys_tf = [P_cart; P_pend];
inputs = {'u'};
outputs = {'x'; 'phi'};
set(sys_tf,'InputName',inputs);
set(sys_tf,'OutputName',outputs);
sys_tf
% create state-space model
p = I * (m + M) + m * M * L^2;
A = [0, 1, 0, 0;
0, -b * (I + m * L^2) / p, (m^2 * L^2 * g) / p, 0;
0, 0, 0, 1;
0, -(m * L * b) / p, m * L * g * (M + m) / p, 0];
B = [0;
(I + m * L^2) / p;
0;
m * L / p];
C = [1, 0, 0, 0;
0, 0, 1, 0];
D = [0;
0];
% definitions of system variables
states = {'x' 'x_dot' 'phi' 'phi_dot'};
inputs = {'u'}; outputs = {'x'; 'phi'};
sys_ss = ss(A, B, C, D, 'statename', states, 'inputname', inputs, 'outputname', outputs);
% poles of open loop system
poles = pole(sys_tf);
poles
% impulse response of system
t = 0: 0.01: 1;
impulse(sys_ss, t);
% step response of system
t = 0: 0.01: 1;
step(sys_ss, t);
% LQR simulation
% Q matrix of LQR controller
Q = [1000, 0, 0, 0;
0, 0, 0, 0;
0, 0, 500, 0;
0, 0, 0, 0];
R = 0.1;
% optimal gain matrix K
K = lqr(A, B, Q, R);
Ac = A - B * K;
sys_lqr = ss(Ac, B, C, D, 'statename', states, 'inputname', inputs, 'outputname', outputs);
% impulse response of system
t = 0: 0.01: 2;
impulse(sys_lqr, t);100
% step response of system
t = 0: 0.01: 3;
step(sys_lqr, t);
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)