文章目录
LQR
1.概念
LQR是线性二次型调节器 Linear Quadratic Regulator。
公式是连续的，实际使用时是离散的。
2.公式推倒
系统方程
x
˙
=
A
x
+
B
u
\ {\dot x} = Ax + Bu\
x˙=Ax+Bu
离散方法：
x
(
t
+
1
)
=
x
(
t
)
+
A
ζ
x(t+1) = x(t) + A\zeta
x(t+1)=x(t)+Aζ
向前欧拉法
x
(
t
+
1
)
=
(
I
+
A
)
x
(
t
)
x(t+1) = (I + A)x(t)
x(t+1)=(I+A)x(t)
向后欧拉法
x
(
t
+
1
)
=
(
I
−
A
)
−
1
x
(
t
)
x(t+1) = (I-A)^{-1}x(t)
x(t+1)=(I−A)−1x(t)
中间欧拉法
x
(
t
+
1
)
=
(
I
−
A
/
2
)
−
1
(
I
+
A
/
2
)
x
(
t
)
x(t+1) = (I-A/2)^{-1}(I+A/2)x(t)
x(t+1)=(I−A/2)−1(I+A/2)x(t)
设计一个线性反馈控制器
u
=
−
K
x
\ u = -Kx \
u=−Kx
代价函数
J
=
x
T
Q
x
+
u
T
R
u
\ J = x^TQx + u^TRu \
J=xTQx+uTRu
其中
Q
Q
Q和
R
R
R是权重矩阵，人为设定。
需要求解
K
K
K
求解方式
K
=
R
−
1
B
T
P
K = R^{-1}B^TP
K=R−1BTP
离散
K
=
(
R
+
B
T
P
B
)
−
1
B
T
P
A
K = (R + B^TPB)^{-1}B^TPA
K=(R+BTPB)−1BTPA
其中
A
T
P
+
P
A
+
Q
=
P
B
R
−
1
B
T
P
A^TP + PA + Q = PBR^{-1}B^TP
ATP+PA+Q=PBR−1BTP
离散
P
=
A
T
P
A
−
A
T
P
B
(
R
+
B
T
P
B
)
−
1
B
T
P
A
+
Q
P = A^TPA - A^TPB(R + B^TPB)^{-1}B^TPA+Q
P=ATPA−ATPB(R+BTPB)−1BTPA+Q
其中，P的初值可为Q。
P
i
n
i
t
=
Q
P_{init} = Q
Pinit=Q
3.示例及流程
0.写出状态方程
1.写出矩阵A
2.写出矩阵B
3.计算误差
4.计算离散矩阵 A d , B d A_d, B_d Ad,Bd
A
d
=
(
I
−
A
/
2
∗
d
t
)
−
1
(
I
+
A
/
2
∗
d
t
)
A_d = (I-A/2*dt)^{-1}(I+A/2*dt)
Ad=(I−A/2∗dt)−1(I+A/2∗dt)
B
d
=
(
I
−
A
/
2
∗
d
t
)
B
∗
d
t
B_d = (I- A/2*dt)B*dt
Bd=(I−A/2∗dt)B∗dt
式中，
d
t
dt
dt是控制周期，较小，例如0.001，故通常省略
B
d
B_d
Bd中的
(
I
−
A
/
2
∗
d
t
)
(I- A/2*dt)
(I−A/2∗dt),得到：
B
d
=
B
∗
d
t
B_d = B*dt
Bd=B∗dt
5.求解LQR矩阵，计算得到K
P = Q
P
n
e
x
t
=
A
T
P
A
−
A
T
P
B
(
R
+
B
T
P
B
)
−
1
B
T
P
A
+
Q
P_{next} = A^TPA - A^TPB(R + B^TPB)^{-1}B^TPA+Q
Pnext=ATPA−ATPB(R+BTPB)−1BTPA+Q
i
f
(
P
−
P
n
e
x
t
)
<
e
if(P - P_{next}) < e
if(P−Pnext)<e
b
r
e
a
k
break
break
扫一扫
1664
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
