[LQR简要快速入门]+[一级倒立摆的LQR控制]
1. 什么是LQR
LQR是一种最优控制算法,简要讲即为寻求一种算法,使得在满足系统稳定性能的同时,系统在达到稳定的过程中消耗的能量也最少(具有实际意义)。
利用最优控制理论的知识可以知道,既然要达到两个指标(1. 性能;2. 能量)的最优,可以很容易列出积分形式的最优指标:
J
=
∫
0
∞
(
x
T
Q
x
+
u
T
R
u
)
d
t
(1)
J=\int _0 ^ \infty (x^T Q x+u^T R u)dt \tag{1}
J=∫0∞(xTQx+uTRu)dt(1)(有关于最优控制理论的部分,以后慢慢补齐博客,这里不影响理解)
一般地,最优指标
J
J
J的选取有三种:拉格朗日型(a),迈耶尔型(b),波尔茨型(d),三者形式如下:
J
=
∫
t
0
t
k
F
(
t
,
x
⃗
,
x
⃗
˙
,
u
⃗
)
d
t
(a)
J = \int _{t_0} ^{t_k} F \left( t, \vec x, \dot{\vec x},\vec u \right) dt \tag{a}
J=∫t0tkF(t,x,x˙,u)dt(a)
J
=
Φ
(
x
⃗
(
t
0
)
,
x
⃗
(
t
k
)
)
(b)
J = \Phi \left( \vec x(t_0), \vec x(t_k) \right) \tag{b}
J=Φ(x(t0),x(tk))(b)
J
=
∫
t
0
t
k
F
(
t
,
x
⃗
,
x
⃗
˙
,
u
⃗
)
d
t
+
Φ
(
x
⃗
(
t
0
)
,
x
⃗
(
t
k
)
)
(d)
J = \int _{t_0} ^{t_k} F \left( t, \vec x, \dot{\vec x},\vec u \right) dt + \Phi \left( \vec x(t_0), \vec x(t_k) \right) \tag{d}
J=∫t0tkF(t,x,x˙,u)dt+Φ(x(t0),x(tk))(d)
这里选取拉格朗日型,原因很容易理解:在控制过程中,希望总的性能和能量最小,因此只有积分形式才能代表整个过程的总量。
在系统控制框图(上图)中,系统状态量
x
x
x经过增益矩阵
K
K
K返回到
u
u
u处,即
u
=
−
K
x
+
r
u=-Kx+r
u=−Kx+r
如果再令
r
=
0
r=0
r=0,那么
x
˙
=
A
x
+
B
u
=
A
x
−
B
K
x
=
(
A
−
B
K
)
u
y
=
C
x
+
D
u
C
x
−
D
K
u
=
(
C
−
D
K
)
u
\begin{aligned} \dot x&=Ax+Bu=Ax-BKx=\left( A-BK \right) u \\ y&=Cx+DuCx-DKu=\left( C-DK \right) u \end{aligned}
x˙y=Ax+Bu=Ax−BKx=(A−BK)u=Cx+DuCx−DKu=(C−DK)u通过改变
K
K
K的值,从而达到控制系统性能的目的。
2. 公式含义
x
x
x – 状态量;
u
u
u – 控制量;
Q
,
R
Q, R
Q,R – 权重矩阵(对角阵)。
公式
(
1
)
(1)
(1)中
x
T
Q
x
x^T Q x
xTQx可以粗略理解为
Q
x
2
Qx^2
Qx2,同理第二项粗略理解为
R
u
2
Ru^2
Ru2,这样一来
J
=
∫
0
∞
(
Q
x
2
+
R
u
2
)
d
t
(1-1)
J=\int _0 ^ \infty \left(Qx^2+Ru^2 \right)dt \tag{1-1}
J=∫0∞(Qx2+Ru2)dt(1-1)里的被积函数部分为非负值。在实际控制过程中,
x
,
u
x,u
x,u都有可能取负值或正值,而要知道系统消耗的能量,势必要用绝对值来进行计算。因此,
J
=
∫
0
∞
(
∣
Q
x
∣
+
∣
R
u
∣
)
d
t
J=\int _0 ^\infty \left( \left|Qx \right|+\left| Ru \right| \right)dt
J=∫0∞(∣Qx∣+∣Ru∣)dt显然,公式
(
1
−
1
)
(1-1)
(1−1)和这种方法是等效的,且更加计算简便。
另一方面, x x x为状态量, u u u为控制量,则 x T Q x x^T Q x xTQx和 R u 2 Ru^2 Ru2分别间接代表了系统性能和所需能量。 J J J为二者加和,那么 J J J实则是同时综合考虑了性能和能量两方面指标。
式中还用到了两个权重系数 Q Q Q和 R R R。上面说到, J J J实则是同时综合考虑了性能和能量两方面指标,那么 Q Q Q和 R R R这两个权重矩阵取值的不同直接决定了 J J J中性能和能量两部分各自所占“比例”(权重)的大小(例如, Q Q Q大些,表示考虑性能要更多些; R R R大些,表示考虑能量更多些),并进一步间接决定了系统控制过程的好坏。因此,LQR算法中最重要的一步也是不断调整 Q , R Q,R Q,R的取值,使得系统达到较满意的状态。
3. 倒立摆的建模
这里不加证明地给出倒立摆的数学模型:
(
M
+
m
)
x
¨
+
m
l
θ
¨
cos
θ
−
m
l
θ
˙
2
sin
θ
+
b
1
x
˙
=
F
(
I
+
m
l
2
)
θ
¨
+
m
x
¨
l
cos
θ
−
m
g
l
sin
θ
+
b
2
θ
˙
=
0
(2)
\begin{aligned} (M+m) \ddot x + ml \ddot \theta \cos{\theta}-ml \dot \theta ^2 \sin{\theta}+b_1 \dot x = F \\ (I+ml^2) \ddot \theta +m \ddot x l \cos{\theta} -mgl \sin{\theta} +b_2 \dot \theta = 0 \end{aligned} \tag{2}
(M+m)x¨+mlθ¨cosθ−mlθ˙2sinθ+b1x˙=F(I+ml2)θ¨+mx¨lcosθ−mglsinθ+b2θ˙=0(2)其中:
M
M
M – 小车质量;
m
m
m – 摆杆质量;
x
x
x – 小车位移坐标;
l
l
l – 杆长一半;
θ
\theta
θ – 摆杆与竖直向上夹角(顺时针为正);
b
1
b_1
b1 – 小车与地面摩擦系数,与速度成正比;
b
2
b_2
b2 – 摆杆与小车连接处摩擦系数,与角速度成正比;
F
F
F – 施加在小车上的外力;
g
g
g – 重力加速度;
I
I
I – 摆杆的转动惯量。
3.1 线性化
假设倒立摆初始状态为竖直向上(即稳定态),初始时刻有一个脉冲信号作为干扰。
假设角度变化
Δ
θ
\Delta \theta
Δθ极小,则
cos
θ
≈
1
,
sin
θ
≈
θ
,
θ
˙
2
≈
0
\cos{\theta} \approx 1,\sin{\theta} \approx \theta, \dot \theta ^2 \approx 0
cosθ≈1,sinθ≈θ,θ˙2≈0,公式
(
2
)
(2)
(2)可以线性化为:
(
M
+
m
)
x
¨
+
m
l
θ
¨
+
b
1
x
˙
=
F
(
I
+
m
l
2
)
θ
¨
+
m
x
¨
l
−
m
g
l
θ
+
b
2
θ
˙
=
0
(3)
\begin{aligned} (M+m) \ddot x + ml \ddot \theta+b_1 \dot x = F \\ (I+ml^2) \ddot \theta +m \ddot x l -mgl \theta +b_2 \dot \theta = 0 \end{aligned} \tag{3}
(M+m)x¨+mlθ¨+b1x˙=F(I+ml2)θ¨+mx¨l−mglθ+b2θ˙=0(3)
3.2 状态空间建立
设状态向量
x
=
[
x
1
x
2
x
3
x
4
]
T
=
[
x
x
˙
θ
θ
˙
]
T
x=[x_1 \quad x_2 \quad x_3 \quad x_4]^T=[x \quad \dot x \quad \theta \quad \dot \theta]^T
x=[x1x2x3x4]T=[xx˙θθ˙]T,那么显然需要利用
(
3
)
(3)
(3)式求出
x
¨
\ddot x
x¨和
θ
¨
\ddot \theta
θ¨。
联立
(
3
)
(3)
(3)的两个方程可以解出:
x
¨
=
1
(
M
+
m
)
(
I
+
m
l
2
)
−
m
2
l
2
[
−
b
1
(
I
+
m
l
2
)
x
2
−
m
2
g
l
2
x
3
+
b
2
m
l
x
4
+
F
(
I
+
m
l
2
)
]
θ
¨
=
1
m
2
l
2
−
(
M
+
m
)
(
I
+
m
l
2
)
[
−
b
1
m
l
x
2
−
(
M
+
m
)
m
g
l
x
3
+
b
2
(
M
+
m
)
x
4
+
m
l
F
]
\begin{aligned} \ddot x = \frac{1}{(M+m)(I+ml^2)-m^2l^2} \left[ -b_1(I+ml^2)x_2-m^2gl^2x_3+b_2mlx_4+F(I+ml^2) \right]\\ \ddot \theta = \frac{1}{m^2l^2-(M+m)(I+ml^2)} \left[ -b_1mlx_2-(M+m)mglx_3+b_2 (M+m)x_4+mlF \right] \end{aligned}
x¨=(M+m)(I+ml2)−m2l21[−b1(I+ml2)x2−m2gl2x3+b2mlx4+F(I+ml2)]θ¨=m2l2−(M+m)(I+ml2)1[−b1mlx2−(M+m)mglx3+b2(M+m)x4+mlF]记
N
=
(
M
+
m
)
(
I
+
m
l
2
)
−
m
2
l
2
N=(M+m)(I+ml^2)-m^2l^2
N=(M+m)(I+ml2)−m2l2那么可以建立状态空间表达式:
[
x
˙
1
x
˙
2
x
˙
3
x
˙
4
]
=
[
0
1
0
0
0
−
b
1
(
I
+
m
l
2
)
N
−
m
2
g
l
2
N
b
2
m
l
N
0
0
0
1
0
b
1
m
l
N
(
M
+
m
m
g
l
N
−
b
2
(
M
+
m
)
N
]
⋅
[
x
1
x
2
x
3
x
4
]
+
[
0
I
+
m
l
2
N
0
−
m
l
N
]
F
y
=
[
1
0
0
0
0
0
1
0
]
⋅
[
x
1
x
2
x
3
x
4
]
=
[
x
1
x
3
]
\begin{aligned} \left[ \begin{matrix} \dot x_1 \\ \dot x_2 \\ \dot x_3 \\ \dot x_4 \end{matrix} \right]&= \left[ \begin{matrix} 0 & 1 & 0 & 0 \\ 0 & -\frac{b_1(I+ml^2)}{N} & -\frac{m^2gl^2}{N} & \frac{b_2ml}{N} \\ 0 & 0 & 0 & 1 \\ 0 & \frac{b_1ml}{N} & \frac{(M+mmgl}{N} & -\frac{b_2(M+m)}{N} \end{matrix} \right] \cdot \left[ \begin{matrix} x_1 \\ x_2 \\ x_3 \\ x_4 \end{matrix} \right] + \left[ \begin{matrix} 0 \\ \frac{I+ml^2}{N} \\ 0 \\ -\frac{ml}{N} \end{matrix} \right] F \\ y&= \left[ \begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \end{matrix} \right] \cdot \left[ \begin{matrix} x_1 \\ x_2 \\ x_3 \\ x_4 \end{matrix} \right] = \left[ \begin{matrix} x_1 \\ x_3 \end{matrix} \right] \end{aligned}
⎣⎢⎢⎡x˙1x˙2x˙3x˙4⎦⎥⎥⎤y=⎣⎢⎢⎡00001−Nb1(I+ml2)0Nb1ml0−Nm2gl20N(M+mmgl0Nb2ml1−Nb2(M+m)⎦⎥⎥⎤⋅⎣⎢⎢⎡x1x2x3x4⎦⎥⎥⎤+⎣⎢⎢⎡0NI+ml20−Nml⎦⎥⎥⎤F=[10000100]⋅⎣⎢⎢⎡x1x2x3x4⎦⎥⎥⎤=[x1x3]其中
A
,
B
,
C
,
D
A,B,C,D
A,B,C,D分别为
A
=
[
0
1
0
0
0
−
b
1
(
I
+
m
l
2
)
N
−
m
2
g
l
2
N
b
2
m
l
N
0
0
0
1
0
b
1
m
l
N
(
M
+
m
m
g
l
N
−
b
2
(
M
+
m
)
N
]
B
=
[
0
I
+
m
l
2
N
0
−
m
l
N
]
C
=
[
1
0
0
0
0
0
1
0
]
D
=
[
0
0
]
\begin{aligned} A&=\left[ \begin{matrix} 0 & 1 & 0 & 0 \\ 0 & -\frac{b_1(I+ml^2)}{N} & -\frac{m^2gl^2}{N} & \frac{b_2ml}{N} \\ 0 & 0 & 0 & 1 \\ 0 & \frac{b_1ml}{N} & \frac{(M+mmgl}{N} & -\frac{b_2(M+m)}{N} \end{matrix} \right] \\ B&=\left[ \begin{matrix} 0 \\ \frac{I+ml^2}{N} \\ 0 \\ -\frac{ml}{N} \end{matrix} \right] \\ C&=\left[ \begin{matrix} 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 \end{matrix} \right] \\ D&=\left[ \begin{matrix} 0 \\ 0 \end{matrix} \right] \end{aligned}
ABCD=⎣⎢⎢⎡00001−Nb1(I+ml2)0Nb1ml0−Nm2gl20N(M+mmgl0Nb2ml1−Nb2(M+m)⎦⎥⎥⎤=⎣⎢⎢⎡0NI+ml20−Nml⎦⎥⎥⎤=[10000100]=[00]
4. LQR算法实现
选取权重矩阵
Q
=
d
i
a
g
(
1000
,
1
,
100
,
1
)
,
R
=
1
Q=diag(1000,1,100,1),R=1
Q=diag(1000,1,100,1),R=1。
在MATLAB中有很方便的函数
K = lqr(A, B, Q, R)
即可得到反馈矩阵 K K K。
5. MATLAB代码仿真
这里贴出代码,如果使用,请点个赞或收藏,谢谢!
clc;
clear variables;
% ---------倒立摆基本参数---------
M = 2;
m = 0.1;
l = 0.5;
b1 = 0.1;
b2 = 0.1;
g = 9.8;
L = 2*l;
J = 1/3*m*L^2;
JJ = J + m*l^2;
N = (M+m)*JJ-m^2*l^2;
% ---------状态空间建立----------
A = [0 1 0 0;
0 -b1*JJ/N -m^2*g*l^2/N b2*m*l/N;
0 0 0 1;
0 b1*m*l/N (M+m)*m*g*l/N -b2*(M+m)/N];
B = [0;
JJ/N;
0;
-m*l/N];
C = [1 0 0 0;
0 0 1 0];
D = [0;
0];
%% 设置Q R
q = [1000 1 100 1];
Q = diag(q);
R = 1;
%% 计算K
K = lqr(A, B, Q, R);
%% 进行LQR计算
Ac = A - B*K;
%% LQR仿真,脉冲信号激发
t = 0 : 0.01 : 15;
ssold = ss(A, B, C, D);
ssnew = ss(Ac, B, C, D);
imold = impulse(ssold, t);
imnew = step(ssnew, t);
xold = imold(:, 1);
theold = imold(:, 2);
xnew = imnew(:, 1);
thenew = imnew(:, 2);
%% 画图
figure(1);
clf;
plot(t, xnew, 'linewidth', 2);
grid on;
grid minor;
xlabel('Time, s');
ylabel('$x$/m', 'interpreter', 'latex');
title('Time – Position');
set(gca, 'fontname', 'times new roman', 'fontsize', 25);
figure(2);
clf;
plot(t, thenew / 3.14 * 180, 'linewidth', 2);
grid on;
grid minor;
xlabel('Time, s');
ylabel('$\theta$/m', 'interpreter', 'latex');
title('Time – Angle');
set(gca, 'fontname', 'times new roman', 'fontsize', 25);
仿真结果如下(角度单位为
°
\degree
°):
6. 优缺点
LQR的优点:
1.不需要大量计算,只需要进行线性化即可;
2.存在现成函数lqr
可以使用,计算方便快捷;
3.仿真速度快,鲁棒性强。
LQR缺点:
1.需要将系统线性化,但在有些位置处不可进行线性化;
2.状态空间
x
˙
=
A
x
+
B
u
\dot x=Ax+Bu
x˙=Ax+Bu默认为零初始状态,因此
x
0
=
0
x_0=0
x0=0,如果要研究非零初始状态的系统,需要进行变换。
博主刚接触LQR控制,很多地方理解还很浅,此篇博客权当记录学习笔记和一次小小仿真实践,如果有观点欢迎在评论区评论,也欢迎有所收获的小伙伴点赞收藏!