浅谈线性二次型调节器（LQR）算法（二）—— 离散系统及仿真代码
传送门
- 前言
- 线性二次型控制器
- 动手实践
-
- 系统模型
- 仿真代码
- 运行结果
- 带输入的系统模型
- LQR控制器设计
- 控制器仿真
- 运行结果
- 结论
-
- 优势
- 劣势
- 后续
前言
之前已经写过一篇关于LQR控制器的理解，但是在看了一些资料，重新思考复盘过，认为原先关于LQR的理解有一定的投机取巧成分。并且只阐述了连续系统下的相关公式推导，并没有对离散系统进行推导叙述。
但为了能够体现出两次思考的差别，便没有将原有博客删除或直接修改，而是新起一篇博客进行重新描述整个理解和公式推导过程，最后会给出仿真过程，并将仿真文件上传分享。
为了不做一些重复性赘述，默认读者掌握相关的基础知识
线性二次型控制器
- 状态空间方程
X
⃗
′
=
A
X
⃗
+
B
U
⃗
Y
=
C
X
⃗
\vec{X}’ = A\vec{X} + B\vec{U}\\ Y=C\vec{X}
X
′=AX
+BU
Y=CX对于LQR来说，只需要讨论其状态转移方程，并且将连续系统转换成离散系统。对应的状态转移方程为
X
⃗
k
+
1
=
A
D
X
⃗
k
+
B
D
U
⃗
k
\vec{X}_{k+1} = A_{D}\vec{X}_{k}+B_{D}\vec{U}_{k}
X
k+1=ADX
k+BDU
k
这里简单讨论一下
A
A
A和
A
D
A_{D}
AD，
B
B
B和
B
D
B_{D}
BD之间的关系，由于离散系统还与离散周期有关系，因此需要假定离散周期为
Δ
T
\Delta T
ΔT，根据连续状态转移方程
X
⃗
′
=
X
⃗
k
+
1
−
X
⃗
k
Δ
T
=
A
X
⃗
k
+
B
U
⃗
k
X
⃗
k
+
1
−
X
⃗
k
=
Δ
T
A
X
⃗
k
+
Δ
T
B
U
⃗
k
X
⃗
k
+
1
=
(
E
+
Δ
T
A
)
X
⃗
k
+
Δ
T
B
U
⃗
k
\vec{X}’ = \frac{\vec{X}_{k+1} – \vec{X}_{k}}{\Delta T}=A\vec{X}_{k} + B\vec{U}_{k}\\ \vec{X}_{k+1} – \vec{X}_{k}=\Delta T A\vec{X}_{k}+\Delta T B\vec{U}_{k}\\ \vec{X}_{k+1} = (E+\Delta TA)\vec{X}_{k}+\Delta TB\vec{U}_{k}
X
′=ΔTX
k+1−X
k=AX
k+BU
kX
k+1−X
k=ΔTAX
k+ΔTBU
kX
k+1=(E+ΔTA)X
k+ΔTBU
k即可以得到
A
D
=
E
+
Δ
T
A
B
D
=
Δ
T
B
A_{D} = E+\Delta T A \\ B_{D}=\Delta T B
AD=E+ΔTABD=ΔTB
- 代价函数（Cost Function）
代价函数是用来描述LQR控制倾向的函数，通过调节权重矩阵来修改控制特性。另外，代价函数用于计算控制律，其中的权重矩阵最终都会影响控制律，进而影响控制效果。
代价函数的形式为
J
=
1
2
X
⃗
(
t
f
)
T
F
X
⃗
(
t
f
)
+
1
2
∫
t
0
t
f
(
X
⃗
T
Q
X
⃗
+
U
⃗
T
R
U
⃗
)
d
t
J = \frac{1}{2}\vec{X}(t_f)^{T}F\vec{X}(t_f)+\frac{1}{2}\int_{t_0}^{t_f}{(\vec{X}^{T}Q\vec{X} + \vec{U}^{T}R\vec{U})dt}
J=21X
(tf)TFX
(tf)+21∫t0tf(X
TQX
+U
TRU
)dt其中，
t
f
t_f
tf为目标时刻，
t
0
t_0
t0为初始时刻。矩阵
F
F
F为末端代价权重矩阵，用于描述末端状态向量的权重大小。矩阵
Q
Q
Q和矩阵
R
R
R分别状态向量
X
⃗
\vec{X}
X
和系统输入U
⃗
\vec{U}
U
在过程代价中的权重。由于是对离散系统进行LQR控制，因此也需要对代价函数进行离散化描述，可以得到
J
=
1
2
X
⃗
(
N
)
T
F
X
⃗
(
N
)
+
1
2
∑
k
=
0
N
−
1
(
X
⃗
(
k
)
T
Q
X
⃗
(
k
)
+
U
⃗
(
k
)
T
R
U
⃗
(
k
)
)
J = \frac{1}{2} \vec{X}(N)^{T}F\vec{X}(N)+\frac{1}{2}\sum_{k=0}^{N-1}{ (\vec{X}(k)^{T}Q\vec{X}(k) + \vec{U}(k)^{T}R\vec{U}(k)) }
J=21X
(N)TFX
(N)+21k=0∑N−1(X
(k)TQX
(k)+U
(k)TRU
(k))
这里我想详细阐述一些关于LQR特性的内容。根据这个离散代价函数，需要思考几个问题
J
J
J的含义
代价函数
J
J
J代表了从
0
0
0时刻到
N
N
N时刻，这个过程中状态向量以及输入的累积值。
J
J
J的最优化代表什么
从代价函数的形式可以看出，其都是半正定矩阵，即
J
≥
0
J \geq 0
J≥0。因此对
J
J
J求最优化，会使得状态向量向零向量收敛。
J
J
J的最优化结果是什么
用
J
J
J对输入
U
⃗
\vec{U}
U
进行求导，可以得到从一个输入向量的时间序列{
U
0
⃗
,
U
1
⃗
,
U
2
⃗
.
.
.
,
U
N
−
1
⃗
}
\{\vec{U_0},\vec{U_1},\vec{U_2}…,\vec{U_N-1} \}
{U0
,U1
,U2
…,UN−1
}。但是这显然不好求，太多自变量了，求偏导都得求到头大，因此需要使用一些数学方法来巧妙地实现最优求解。
- 递归特性
当
k
=
N
k=N
k=N时
J
k
=
N
=
1
2
X
⃗
(
N
)
T
F
X
⃗
(
N
)
J_{k=N} = \frac{1}{2} \vec{X}(N)^{T}F\vec{X}(N)
Jk=N=21X
(N)TFX
(N)很明显，最后一个时刻的代价函数值就是末端代价，末端代价即为N时刻代价函数的最优值
J
k
=
N
∗
J^{*}_{k=N}
Jk=N∗。
当
k
=
N
−
1
k=N-1
k=N−1时
J
k
=
N
−
1
=
1
2
X
⃗
(
N
)
T
F
X
⃗
(
N
)
+
1
2
(
X
⃗
(
N
−
1
)
T
Q
X
⃗
(
N
−
1
)
+
U
⃗
(
N
−
1
)
T
R
U
⃗
(
N
−
1
)
)
=
J
k
=
N
+
1
2
(
X
⃗
(
N
−
1
)
T
Q
X
⃗
(
N
−
1
)
+
U
⃗
(
N
−
1
)
T
R
U
⃗
(
N
−
1
)
)
J_{k=N-1} = \frac{1}{2} \vec{X}(N)^{T}F\vec{X}(N) + \ \frac{1}{2}(\vec{X}(N-1)^{T}Q\vec{X}(N-1) + \vec{U}(N-1)^{T}R\vec{U}(N-1)) \\ =J_{k=N}+ \frac{1}{2}(\vec{X}(N-1)^{T}Q\vec{X}(N-1) + \vec{U}(N-1)^{T}R\vec{U}(N-1))
Jk=N−1=21X
(N)TFX
(N)+ 21(X
(N−1)TQX
(N−1)+U
(N−1)TRU
(N−1))=Jk=N+21(X
(N−1)TQX
(N−1)+U
(N−1)TRU
(N−1))当
k
=
N
−
2
k=N-2
k=N−2时
J
k
=
N
−
2
=
1
2
X
⃗
(
N
)
T
F
X
⃗
(
N
)
+
1
2
(
X
⃗
(
N
−
1
)
T
Q
X
⃗
(
N
−
1
)
+
U
⃗
(
N
−
1
)
T
R
U
⃗
(
N
−
1
)
)
+
1
2
(
X
⃗
(
N
−
2
)
T
Q
X
⃗
(
N
−
2
)
+
U
⃗
(
N
−
2
)
T
R
U
⃗
(
N
−
2
)
)
=
J
k
=
N
−
1
+
1
2
(
X
⃗
(
N
−
2
)
T
Q
X
⃗
(
N
−
2
)
+
U
⃗
(
N
−
2
)
T
R
U
⃗
(
N
−
2
)
)
J_{k=N-2} = \frac{1}{2} \vec{X}(N)^{T}F\vec{X}(N) + \ \frac{1}{2}(\vec{X}(N-1)^{T}Q\vec{X}(N-1) + \vec{U}(N-1)^{T}R\vec{U}(N-1)) \\ +\frac{1}{2}(\vec{X}(N-2)^{T}Q\vec{X}(N-2) + \vec{U}(N-2)^{T}R\vec{U}(N-2))\\ =J_{k=N-1}+\frac{1}{2}(\vec{X}(N-2)^{T}Q\vec{X}(N-2) + \vec{U}(N-2)^{T}R\vec{U}(N-2))
Jk=N−2=21X
(N)TFX
(N)+ 21(X
(N−1)TQX
(N−1)+U
(N−1)TRU
(N−1))+21(X
(N−2)TQX
(N−2)+U
(N−2)TRU
(N−2))=Jk=N−1+21(X
(N−2)TQX
(N−2)+U
(N−2)TRU
(N−2))很明显，不同时刻间的代价函数包含了下一时刻的代价函数，体现了非常强烈的递归特性。
根据贝尔曼最优理论，如果
k
k
k时刻的代价函数
J
k
J_{k}
Jk是最优的，那么他包含的
J
k
+
1
J_{k+1}
Jk+1一定是最优的。
从
k
=
N
k=N
k=N时刻开始，计算
N
N
N时刻的最优代价函数
J
k
=
N
∗
=
J
k
=
N
=
1
2
X
⃗
(
N
)
T
F
X
⃗
(
N
)
J^{*}_{k=N} = J_{k=N} = \frac{1}{2} \vec{X}(N)^{T}F\vec{X}(N)
Jk=N∗=Jk=N=21X
(N)TFX
(N)当
k
=
N
−
1
k=N-1
k=N−1时，
J
k
=
N
−
1
=
J
k
=
N
+
1
2
(
X
⃗
(
N
−
1
)
T
Q
X
⃗
(
N
−
1
)
+
U
⃗
(
N
−
1
)
T
R
U
⃗
(
N
−
1
)
)
=
1
2
X
⃗
(
N
)
T
F
X
⃗
(
N
)
+
1
2
(
X
⃗
(
N
−
1
)
T
Q
X
⃗
(
N
−
1
)
+
U
⃗
(
N
−
1
)
T
R
U
⃗
(
N
−
1
)
)
J_{k=N-1} =J_{k=N}+\frac{1}{2} { (\vec{X}(N-1)^{T}Q\vec{X}(N-1) + \vec{U}(N-1)^{T}R\vec{U}(N-1)) }\\ =\frac{1}{2} \vec{X}(N)^{T}F\vec{X}(N)+\frac{1}{2} { (\vec{X}(N-1)^{T}Q\vec{X}(N-1) + \vec{U}(N-1)^{T}R\vec{U}(N-1)) }
Jk=N−1=Jk=N+21(X
(N−1)TQX
(N−1)+U
(N−1)TRU
(N−1))=21X
(N)TFX
(N)+21(X
(N−1)TQX
(N−1)+U
(N−1)TRU
(N−1))又有
X
⃗
(
N
)
=
A
X
⃗
(
N
−
1
)
+
B
U
⃗
(
N
−
1
)
\vec{X}(N)=A\vec{X}(N-1)+B\vec{U}(N-1)
X
(N)=AX
(N−1)+BU
(N−1)对
U
⃗
(
N
−
1
)
\vec{U}(N-1)
U
(N−1)求导∂
J
k
=
N
−
1
∂
U
⃗
(
N
−
1
)
=
∂
X
⃗
(
N
)
∂
U
⃗
(
N
−
1
)
⋅
∂
1
2
X
⃗
(
N
)
T
F
X
⃗
(
N
)
∂
X
⃗
(
N
)
+
∂
1
2
U
⃗
(
N
−
1
)
T
R
U
⃗
(
N
−
1
)
)
∂
U
⃗
(
N
−
1
)
\frac{\partial J_{k=N-1}}{\partial \vec{U}(N-1)} \ =\frac{\partial \vec{X}(N)}{\partial \vec{U}(N-1)} \cdot \frac{\partial \frac{1}{2} \vec{X}(N)^{T}F\vec{X}(N)}{\partial \vec{X}(N)}\ +\frac{\partial \frac{1}{2} \vec{U}(N-1)^{T}R\vec{U}(N-1)) }{\partial \vec{U}(N-1)}
∂U
(N−1)∂Jk=N−1 =∂U
(N−1)∂X
(N)⋅∂X
(N)∂21X
(N)TFX
(N) +∂U
(N−1)∂21U
(N−1)TRU
(N−1))经过矩阵求导得到
∂
J
k
=
N
−
1
∂
U
⃗
(
N
−
1
)
=
B
T
⋅
F
⋅
(
A
X
⃗
(
N
−
1
)
+
B
U
⃗
(
N
−
1
)
)
+
R
U
⃗
(
N
−
1
)
\frac{\partial J_{k=N-1}}{\partial \vec{U}(N-1)} =\ B^{T}\cdot F \cdot (A\vec{X}(N-1)+B\vec{U}(N-1))+R\vec{U}(N-1)
∂U
(N−1)∂Jk=N−1= BT⋅F⋅(AX
(N−1)+BU
(N−1))+RU
(N−1)令其求导为0，求极值
B
T
⋅
F
⋅
(
A
X
⃗
(
N
−
1
)
+
B
U
⃗
(
N
−
1
)
)
+
R
U
⃗
(
N
−
1
)
=
0
B^{T}\cdot F \cdot (A\vec{X}(N-1)+B\vec{U}(N-1))+R\vec{U}(N-1)=0
BT⋅F⋅(AX
(N−1)+BU
(N−1))+RU
(N−1)=0可以得到
−
B
T
F
A
⋅
X
⃗
(
N
−
1
)
=
(
B
T
F
B
+
R
)
⋅
U
⃗
(
N
−
1
)
-B^{T}FA \cdot \vec{X}(N-1)=(B^{T}FB+R) \cdot \vec{U}(N-1)
−BTFA⋅X
(N−1)=(BTFB+R)⋅U
(N−1)解得
U
⃗
(
N
−
1
)
=
−
(
B
T
F
B
+
R
)
−
1
B
T
F
A
⋅
X
⃗
(
N
−
1
)
=
−
K
(
N
−
1
)
⋅
X
⃗
(
N
−
1
)
\vec{U}(N-1) = -(B^{T}FB+R) ^{-1} B^{T}FA \cdot \vec{X}(N-1) \ =-K(N-1) \cdot \vec{X}(N-1)
U
(N−1)=−(BTFB+R)−1BTFA⋅X
(N−1) =−K(N−1)⋅X
(N−1)
- 这里令
K
=
(
B
T
F
B
+
R
)
−
1
B
T
F
A
K = (B^{T}FB+R) ^{-1} B^{T}FA
K=(BTFB+R)−1BTFA，可以得到全状态反馈的控制律，即
U
⃗
=
−
K
X
⃗
\vec{U} = -K \vec{X}
U
=−KX- 矩阵运算之前在卡尔曼滤波器里讲过，有兴趣可以翻看，不重复赘述。卡尔曼滤波器
为了验证其是否为极小值点，二次求导。
∂
2
J
k
=
N
−
1
∂
U
⃗
2
(
N
−
1
)
=
B
T
F
B
+
R
\frac{\partial^{2} J_{k=N-1}}{\partial \vec{U}^{2}(N-1)} =\ B^{T} FB +R
∂U
2(N−1)∂2Jk=N−1= BTFB+R
显然，由于矩阵
R
R
R正定，
B
T
F
B
B^{T}FB
BTFB为半正定矩阵，因此
∂
2
J
k
=
N
−
1
∂
U
⃗
2
(
N
−
1
)
>
0
\frac{\partial^{2} J_{k=N-1}}{\partial \vec{U}^{2}(N-1)} >0
∂U
2(N−1)∂2Jk=N−1>0
因此该极值点为极小值点。
将极值点
U
⃗
(
N
−
1
)
=
−
(
B
T
F
B
+
R
)
−
1
B
T
F
A
⋅
X
⃗
(
N
−
1
)
\vec{U}(N-1)=-(B^{T}FB+R) ^{-1} B^{T}FA \cdot \vec{X}(N-1)
U
(N−1)=−(BTFB+R)−1BTFA⋅X
(N−1)代入，得到
N
−
1
N-1
N−1时刻的最优代价函数
J
∗
(
N
−
1
)
=
X
⃗
T
(
N
−
1
)
(
[
A
−
B
K
(
N
−
1
)
]
T
⋅
F
⋅
[
A
−
B
K
(
N
−
1
)
]
+
K
(
N
−
1
)
T
R
K
(
N
−
1
)
+
Q
)
X
⃗
(
N
−
1
)
J^{*}(N-1) = \vec{X}^{T}(N-1) \ ( [A-BK(N-1)]^{T} \cdot F \cdot [A-BK(N-1)] + K(N-1)^{T} R K(N-1) + Q) \ \vec{X}(N-1)
J∗(N−1)=X
T(N−1) ([A−BK(N−1)]T⋅F⋅[A−BK(N−1)]+K(N−1)TRK(N−1)+Q) X
(N−1)
可以看出 无论是
J
∗
(
N
)
J^{*}(N)
J∗(N) 还是
J
∗
(
N
−
1
)
J^{*}(N-1)
J∗(N−1) 都是以
X
⃗
T
P
X
⃗
\vec{X}^{T}P\vec{X}
X
TPX
的形式成立。
因此总结其规律，令
P
(
0
)
=
F
P
(
1
)
=
(
[
A
−
B
K
(
N
−
1
)
]
T
⋅
P
(
0
)
⋅
[
A
−
B
K
(
N
−
1
)
]
+
K
(
N
−
1
)
T
R
K
(
N
−
1
)
+
Q
)
P(0) = F\\ P(1) = ( [A-BK(N-1)]^{T} \cdot P(0) \cdot [A-BK(N-1)] + K(N-1)^{T} R K(N-1) + Q)
P(0)=FP(1)=([A−BK(N−1)]T⋅P(0)⋅[A−BK(N−1)]+K(N−1)TRK(N−1)+Q)
则可以得到增益矩阵(
N
−
1
N-1
N−1时刻)
K
(
N
−
1
)
=
(
B
T
P
(
0
)
B
+
R
)
−
1
B
T
P
(
0
)
A
K(N-1) = (B^{T}P(0)B+R) ^{-1} B^{T}P(0)A
K(N−1)=(BTP(0)B+R)−1BTP(0)A
- 总结一下
当
k
=
N
k=N
k=N时刻
J
∗
(
N
)
=
X
⃗
T
(
N
)
P
(
0
)
X
⃗
(
N
)
J^{*}(N) = \vec{X}^{T}(N) P(0) \vec{X}(N)
J∗(N)=X
T(N)P(0)X
(N)当
k
=
N
−
1
k=N-1
k=N−1时刻
K
(
N
−
1
)
=
(
B
T
P
(
0
)
B
+
R
)
−
1
B
T
P
(
0
)
A
P
(
1
)
=
(
[
A
−
B
K
(
N
−
1
)
]
T
⋅
P
(
0
)
⋅
[
A
−
B
K
(
N
−
1
)
]
+
K
(
N
−
1
)
T
R
K
(
N
−
1
)
+
Q
)
J
∗
(
N
−
1
)
=
X
⃗
T
(
N
−
1
)
P
(
1
)
X
⃗
(
N
−
1
)
K(N-1) = (B^{T}P(0)B+R) ^{-1} B^{T}P(0)A\\ P(1) = ( [A-BK(N-1)]^{T} \cdot P(0) \cdot [A-BK(N-1)] + K(N-1)^{T} R K(N-1) + Q) \\ J^{*}(N-1) = \vec{X}^{T}(N-1) P(1) \vec{X}(N-1)
K(N−1)=(BTP(0)B+R)−1BTP(0)AP(1)=([A−BK(N−1)]T⋅P(0)⋅[A−BK(N−1)]+K(N−1)TRK(N−1)+Q)J∗(N−1)=X
T(N−1)P(1)X
(N−1)根据其递归特性，可以得到通用一般形式(当
k
≥
1
k\ge1
k≥1)
K
(
N
−
k
)
=
(
B
T
P
(
k
−
1
)
B
+
R
)
−
1
B
T
P
(
k
−
1
)
A
P
(
k
)
=
(
[
A
−
B
K
(
N
−
k
)
]
T
⋅
P
(
k
−
1
)
⋅
[
A
−
B
K
(
N
−
k
)
]
+
K
(
N
−
k
)
T
R
K
(
N
−
k
)
+
Q
)
J
∗
(
N
−
k
)
=
X
⃗
T
(
N
−
k
)
P
(
k
)
X
⃗
(
N
−
k
)
K(N-k) = (B^{T}P(k-1)B+R) ^{-1} B^{T}P(k-1)A\\ P(k) = ( [A-BK(N-k)]^{T} \cdot P(k-1) \cdot [A-BK(N-k)] + K(N-k)^{T} R K(N-k) + Q) \\ J^{*}(N-k) = \vec{X}^{T}(N-k) P(k) \vec{X}(N-k)
K(N−k)=(BTP(k−1)B+R)−1BTP(k−1)AP(k)=([A−BK(N−k)]T⋅P(k−1)⋅[A−BK(N−k)]+K(N−k)TRK(N−k)+Q)J∗(N−k)=X
T(N−k)P(k)X
(N−k)
动手实践
系统模型
以弹簧阻尼系统为例，设质量块质量为m，以静力平衡点为位移0点，其受力情况为：
这里列出受力情况，主要是向一些新入门的同学展示一些状态空间方程是如何来的，已知可跳过。
m
a
=
−
k
x
−
c
v
ma = -kx-cv
ma=−kx−cv
其中，
a
a
a为质量块的加速度，
v
v
v为质量块的速度，
x
x
x为质量块的位移，
k
k
k为弹簧系数，
c
c
c为阻尼系数。
接下来，建立状态空间方程，设状态向量为
X
⃗
\vec{X}
X
为
X
⃗
=
[
x
1
x
2
]
=
[
x
v
]
\vec{X} =\begin{bmatrix} x_1\\x_2\end{bmatrix} = \begin{bmatrix} x\\v\end{bmatrix}
X
=[x1x2]=[xv]
则状态转移方程为
X
′
⃗
=
A
X
⃗
→
[
x
1
′
x
2
′
]
=
[
x
′
v
′
]
=
[
v
a
]
=
[
v
−
k
m
x
−
c
m
v
]
=
[
x
2
−
k
m
x
1
−
c
m
x
2
]
=
[
0
1
−
k
m
−
c
m
]
[
x
1
x
2
]
\vec{X’}=A\vec{X}\\ \rightarrow \begin{bmatrix} x_{1}’\\x_{2}’\end{bmatrix}=\begin{bmatrix} x’\\v’\end{bmatrix}=\begin{bmatrix} v\\a\end{bmatrix} =\begin{bmatrix} v\\-\frac{k}{m}x-\frac{c}{m}v\end{bmatrix}=\begin{bmatrix} x_2\\-\frac{k}{m}x_{1}-\frac{c}{m}x_{2}\end{bmatrix}=\begin{bmatrix} 0 & 1\\-\frac{k}{m}&-\frac{c}{m}\end{bmatrix}\begin{bmatrix} x_1\\x_2\end{bmatrix}
X′
=AX
→[x1′x2′]=[x′v′]=[va]=[v−mkx−mcv]=[x2−mkx1−mcx2]=[0−mk1−mc][x1x2]
为了在matlab中将其运动过程仿真出来，将其转换成离散方程，设采样时间为
T
T
T，则可以得到
[
x
1
(
k
+
1
)
x
2
(
k
+
1
)
]
=
[
1
T
−
k
T
m
1
−
c
T
m
]
[
x
1
(
k
)
x
2
(
k
)
]
\begin{bmatrix} x_{1}(k+1)\\x_{2}(k+1)\end{bmatrix}=\begin{bmatrix} 1&T\\-\frac{kT}{m}&1-\frac{cT}{m}\end{bmatrix}\begin{bmatrix} x_{1}(k)\\x_{2}(k)\end{bmatrix}
[x1(k+1)x2(k+1)]=[1−mkTT1−mcT][x1(k)x2(k)]
仿真代码
T = 0.001;
%离散周期位1ms
m = 1;
%重量块质量为1kg
c = 0.2;
k = 0.5;
%阻尼系数和弹簧系数
A = [1 T;-k*T/m 1-c*T/m];
B = [0;1/m];
%系统状态空间方程
x0 = [4;0];
%系统初始状态向量
n = 100000;
%仿真的步数
x = zeros(n,1);
v = zeros(n,1);
time = zeros(n,1);
%记录状态数据，用来绘图的
xk_1 = [0;0];
xk = x0;
%x(k)以及x(k+1)
for i = 1:n
xk_1 = A*xk;
x(i) = xk(1);
v(i) = xk(2);
time(i) = i*T;
xk = xk_1;
end
plot(time, x,'r-',time,v,'b-') % 绘制曲线
xlabel('x') % 添加x轴标签
ylabel('y') % 添加y轴标签
title('lqr') % 添加标题
legend('x','v');
grid on % 添加网格线
运行结果
可以看出，这是一个无论初始状态如何，最终都能够振荡收敛的系统。此时在这个系统里加入一个力
F
⃗
\vec{F}
F
，作为系统输入
u
u
u，根据
L
Q
R
LQR
LQR计算输入序列，来使得系统状态更快速收敛为0。
带输入的系统模型
带输入的系统模型的状态空间方程为
[
x
1
(
k
+
1
)
x
2
(
k
+
1
)
]
=
[
1
T
−
k
T
m
1
−
c
T
m
]
[
x
1
(
k
)
x
2
(
k
)
]
+
[
0
T
m
]
u
\begin{bmatrix} x_{1}(k+1)\\x_{2}(k+1)\end{bmatrix}=\begin{bmatrix} 1&T\\-\frac{kT}{m}&1-\frac{cT}{m}\end{bmatrix}\begin{bmatrix} x_{1}(k)\\x_{2}(k)\end{bmatrix}+\begin{bmatrix} 0\\\frac{T}{m}\end{bmatrix}u
[x1(k+1)x2(k+1)]=[1−mkTT1−mcT][x1(k)x2(k)]+[0mT]u
LQR控制器设计
- 定义代价函数
J
J
J
J
=
1
2
X
⃗
(
N
)
T
P
(
0
)
X
⃗
(
N
)
+
1
2
∑
k
=
0
N
−
1
(
X
⃗
(
k
)
T
Q
X
⃗
(
k
)
+
U
⃗
(
k
)
T
R
U
⃗
(
k
)
)
J=\frac{1}{2} \vec{X}(N)^{T}P(0)\vec{X}(N)+\frac{1}{2}\sum_{k=0}^{N-1}{ (\vec{X}(k)^{T}Q\vec{X}(k) + \vec{U}(k)^{T}R\vec{U}(k)) }
J=21X
(N)TP(0)X
(N)+21k=0∑N−1(X
(k)TQX
(k)+U
(k)TRU
(k))其中，矩阵
P
(
0
)
,
Q
,
R
P(0),Q, R
P(0),Q,R分别是根据控制需求定义的末端代价权重矩阵，过程状态代价矩阵，过程输入代价矩阵。
- 计算全状态矩阵
K
K
K
根据上面的结论可以得到，递推求解公式为
K
(
N
−
k
)
=
(
B
T
P
(
k
−
1
)
B
+
R
)
−
1
B
T
P
(
k
−
1
)
A
P
(
k
)
=
(
[
A
−
B
K
(
N
−
k
)
]
T
⋅
P
(
k
−
1
)
⋅
[
A
−
B
K
(
N
−
k
)
]
+
K
(
N
−
k
)
T
R
K
(
N
−
k
)
+
Q
)
K(N-k) = (B^{T}P(k-1)B+R) ^{-1} B^{T}P(k-1)A\\ P(k) = ( [A-BK(N-k)]^{T} \cdot P(k-1) \cdot [A-BK(N-k)] + K(N-k)^{T} R K(N-k) + Q)
K(N−k)=(BTP(k−1)B+R)−1BTP(k−1)AP(k)=([A−BK(N−k)]T⋅P(k−1)⋅[A−BK(N−k)]+K(N−k)TRK(N−k)+Q)
其中，
A
=
[
1
T
−
k
T
m
1
−
c
T
m
]
,
B
=
[
0
T
m
]
A=\begin{bmatrix} 1&T\\-\frac{kT}{m}&1-\frac{cT}{m}\end{bmatrix},B=\begin{bmatrix} 0\\\frac{T}{m}\end{bmatrix}
A=[1−mkTT1−mcT],B=[0mT]
控制器仿真
clear all;lqr_flag = 1;T = 0.001;%离散周期位1msm = 1;%重量块质量为1kgc = 0.2;k = 0.5;%阻尼系数和弹簧系数A = [1 T;-k*T/m 1-c*T/m];B = [0;T/m];%系统状态空间方程x0 = [4;0];%系统初始状态向量n = 100000;x = zeros(n,1);v = zeros(n,1);time = zeros(n,1);u = zeros(n,1);%记录状态数据，用来绘图的xk_1 = [0;0];xk = x0;%状态向量xk和xk+1P=zeros(n,4);%P迭代矩阵，用于计算KP0 = [1 0;0 1];%末端状态代价矩阵2x2Q = [1 0;0 1];%过程状态代价矩阵2x2R = 1;%过程输入代价矩阵1x1K = zeros(n,2);%全状态反馈矩阵 1x2P(1,:) = P0(:)';%初始化for i = 2:n tmpP = reshape(P(i-1,:),2,2); tmpK = reshape(K(n-i+1,:),1,2); K(n-i+1,:) = reshape( (B'*tmpP*B+R)\B'*tmpP*A,1,2); P(i,:)= reshape( (A-B * tmpK)'* tmpP *(A-B * tmpK) + tmpK'*R*tmpK+Q ,1,4);end%从最后一个往前算P(k)if lqr_flag == 0 %无输入系统 for i = 1:n xk_1 = A*xk; x(i) = xk(1); v(i) = xk(2); time(i) = i*T; xk = xk_1; endelse % lqr控制输入下的系统 for i = 1:n uk = - reshape(K(i,:),1,2)*xk; xk_1 = A*xk + B*uk; x(i) = xk_1(1); v(i) = xk_1(2); time(i) = i*T; u(i) = uk; xk = xk_1; end endplot(time, x,'r-',time,v,'g-',time,u,'b-') % 绘制曲线xlabel('x') % 添加x轴标签ylabel('y') % 添加y轴标签title('lqr') % 添加标题legend('x','v','u');grid on % 添加网格线运行结果
结论
L
Q
R
LQR
LQR控制器是一种通过代价函数最优求解控制序列的算法，以全状态反馈的控制律形式实现控制。并且其反馈矩阵的计算只与状态方程矩阵，代价函数的三个矩阵有关系。
- 参数调试的几种基本原则
若想快速收敛就将矩阵
Q
Q
Q中对应的状态权重调大；
若想减小系统冲激则提高矩阵
R
R
R的值；
建议多动手尝试，调试不同位置参数来观察控制效果。
优势
- 计算简单，并且可离线计算，即已经事先规划好了，无论你的初始状态如何。
- 可以根据控制需求调节参数，以达到不同的控制效果，使用较为灵活。
劣势
- 只适用于线性系统。
- 不引入当前过去状态来修正控制律增益，抗扰动能力较差。
即当前系统受扰动出现较大的状态偏移后无法做出及时应对和修正
后续
当你看到这里，不知道你有没有想过，使用
L
Q
R
LQR
LQR就只能让你的系统状态量归0吗，如果我不希望他不归0，保持为其他的某个常数，亦或者是以某种轨迹进行变化，又应该如何使用
L
Q
R
LQR
LQR来实现呢。这类问题就是轨迹跟踪的问题。这个内容就是我下一篇博客要讨论的东西，敬请期待。
本文来自网络，不代表协通编程立场，如若转载，请注明出处：https://net2asp.com/230165332f.html
相关推荐
-
2024年第四届机器人与人工智能国际会议(JCRAI 2024) | Ei、Scopus双检索
会议简介 Brief Introduction 2024年第四届机器人与人工智能国际会议(JCRAI 2024) 会议时间：2024年8月9日-11日 召开地点：中国·上海 大会官…
-
RuntimeError: No CUDA GPUs are available
在之前运行代码时正常没有任何问题，但是后面莫名其妙突然报错：RuntimeError: No CUDA GPUs are available 一开始还有这个：No CUDA run…
-
2022我的年度总结–AI遮天之路
我是一个普普通通的大学生，我的博客记录了我学习编程以来共计1年多的水平，我希望能把自己大学的经历、选择、困惑等与同样身处大学，选择AI方向不知如何发展的人进行分享，因此写了这篇年终…
-
大数据编程实验四：Spark Streaming
一、目的与要求 1、通过实验掌握Spark Streaming的基本编程方法； 2、熟悉利用Spark Streaming处理来自不同数据源的数据。 3、熟悉DStream的各种转…
-
改变conda虚拟环境的默认路径
anaconda下指定虚拟环境的创建路径 conda环境默认安装在用户目录C:\Users\username.conda\envs下，如果选择默认路径，那么之后创建虚拟环境，也是安…
-
基于大数据的音乐推荐系统的设计与实现
基于大数据的音乐推荐系统是为了给听众推荐符合内心喜好的个性化系统。系统提供的功能有，音乐管理：管理员可以添加删除音乐，音乐查找：用户可以在系统中自行查找想要听的歌曲，音乐推荐：系统…
-
一百六十八、Kettle——用海豚调度器定时调度从Kafka到HDFS的kettle任务脚本（持续更新追踪、持续完善）
一、目的 在实际项目中，从Kafka到HDFS的数据是每天自动生成一个文件，按日期区分。而且Kafka在不断生产数据，因此看看kettle是不是需要时刻运行？能不能按照每日自动生成…
-
分布式一致性算法Paxos、Raft 及 Zookeeper ZAB
国科大学习生活（期末复习资料、课程大作业解析、学习文档等）: 文章专栏（点击跳转） 大数据开发学习文档（分布式文件系统的实现，大数据生态圈学习文档等）: 文章专栏（点击跳转） 文档…
-
GPT4助力Python数据分析与可视化、人工智能建模及论文高效撰写
详情点击链接：GPT4助力Python数据分析与可视化、人工智能建模及论文高效撰写 第一：GPT4基础入门 1、ChatGPT概述（GPT-1、GPT-2、GPT-3、GPT-3….
-
opencv Mat详解
文章目录 一.图像的存储 二.OpenCV中图像坐标系的定义 三.OpenCV中的Mat 四.OpenCV中的图像存储方式 五.显式创建Mat对象 1.使用Mat构造函数 2.使用…
