前言
上一篇博客简单推导了离散系统下的
L
Q
R
LQR
LQR计算公式,并且留下了一个伏笔——如何使系统状态收敛到非0状态。这篇博客将围绕这个问题来展开,并秉持着想到什么写什么的原则来进行一系列freestyle。
skr~
再看 L Q R LQR 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))
其中是代价
J
J
J用于衡量控制律性能的指标,由于函数中每一项都是半正定的,因此可以得到
J
≥
0
J \ge0
J≥0
另外,代价函数中包括了末端向量
X
⃗
(
N
)
\vec{X}(N)
X(N),过程向量
X
⃗
(
k
)
\vec{X}(k)
X(k),系统输入
U
⃗
(
k
)
\vec{U}(k)
U(k)。很明显,这其中唯一能由我们决定的是系统输入
U
⃗
(
k
)
\vec{U}(k)
U(k)。因此我们对于代价函数进行最优计算的时候,实际上是设计一组系统输入序列
U
⃗
\vec{U}
U,使得代价
J
J
J尽可能的小(因为J存在下限0,无上限
)。
- 状态向量全能控
对于系统状态向量全能控的线性系统而言,经过 L Q R LQR LQR控制器调节的系统稳定点一定为 0 0 0向量。通过以下推导得到:
系统状态转移方程
X ⃗ ( k + 1 ) = A X ⃗ ( k ) + B U ⃗ ( k ) \vec{X}(k+1)=A \vec{X}(k)+B\vec{U}(k) X(k+1)=AX(k)+BU(k)
又 L Q R LQR LQR的控制律为
U ⃗ ( k ) = − K X ⃗ ( k ) \vec{U}(k)=-K\vec{X}(k) U(k)=−KX(k)
代入系统后得到
X ⃗ ( k + 1 ) = ( A − B K ) X ⃗ ( k ) \vec{X}(k+1) = (A-BK)\vec{X}(k) X(k+1)=(A−BK)X(k)
若 k k k时刻的状态向量不为 0 ⃗ \vec{0} 0,则经过 L Q R LQR LQR最优计算得到的矩阵 K K K,会进一步将 X ⃗ ′ ( k ) \vec{X}'(k) X′(k)调整为使 X ⃗ \vec{X} X趋近于 0 ⃗ \vec{0} 0的方向,直到状态向量为 X ⃗ = 0 ⃗ \vec{X}=\vec{0} X=0,此时系统将处于稳定状态,因为 X ⃗ ′ = 0 \vec{X}'=0 X′=0,系统状态向量将不再变化。
因此可以看出,以这种代价函数
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))
得到的最优控制律,它总是将状态向量 X ⃗ \vec{X} X往 0 ⃗ \vec{0} 0趋近。
上述的描述说明了,这在数学上是一条可以实现的最优路径。
如何实现非0期望的控制
- 以之前的弹簧阻尼系统为例
假设现在希望质量块在某个期望的位置 x = x d x=x_{d} x=xd稳定下来 v = 0 v=0 v=0,应该如何做?
可以知道,若 x d ≠ 0 x_{d} \ne 0 xd=0时,若没有外力干涉,则无法稳定下来,即期望的状态向量 X d ⃗ = [ x d 0 ] \vec{X_{d}}=\begin{bmatrix} x_{d} \\0 \end{bmatrix} Xd=[xd0]不是系统稳定向量。
那么应该如何计算最优的系统输入,使系统快速收敛于期望的系统状态向量。
- 狸猫换太子
上面关于代价函数的描述可以总结为,利用以下形式
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))
求解得到的系统输入 U ⃗ \vec{U} U,会使得系统可控的状态 X ⃗ ∗ \vec{X}^{*} X∗不断趋近于0,那么如果将系统中的状态向量替换为误差向量 e ⃗ = X ⃗ − X d ⃗ \vec{e}=\vec{X} - \vec{X_{d}} e=X−Xd(反着写也可以,推导过程同理),则可以使误差向量趋近于0,即状态向量 X ⃗ \vec{X} X趋近于期望向量 X d ⃗ \vec{X_{d}} Xd。
则代价函数更改为:
J = 1 2 e ⃗ ( N ) T P ( 0 ) e ⃗ ( N ) + 1 2 ∑ k = 0 N − 1 ( e ⃗ ( k ) T Q e ⃗ ( k ) + U ⃗ ( k ) T R U ⃗ ( k ) ) J = \frac{1}{2} \vec{e}(N)^{T}P(0)\vec{e}(N)+\frac{1}{2}\sum_{k=0}^{N-1}{ (\vec{e}(k)^{T}Q\vec{e}(k) + \vec{U}(k)^{T}R\vec{U}(k)) } J=21e(N)TP(0)e(N)+21k=0∑N−1(e(k)TQe(k)+U(k)TRU(k))
这个时候会产生一个问题,即原先的代价函数中,能通过对系统输入 U ⃗ \vec{U} U求解最优代价,需要具备一个重要的前提,即对于系统状态向量 X ⃗ \vec{X} X,能受 U ⃗ \vec{U} U直接或间接影响的。
这里其实就是系统能控性的判定。
根据系统状态转移方程,系统输入 U ⃗ \vec{U} U直接参与状态向量的计算。
X ⃗ ( k + 1 ) = A X ⃗ ( k ) + B U ⃗ ( k ) \vec{X}(k+1)=A \vec{X}(k)+B\vec{U}(k) X(k+1)=AX(k)+BU(k)
因此当我们将代价函数修改成以误差向量 e ⃗ \vec{e} e为基准时,需要补充描述系统输入 U ⃗ \vec{U} U是如何影响 e ⃗ \vec{e} e的,只要完成这一步,新的代价函数的最优求解在数学上也能够成立。 - 狸猫变太子
由误差向量表达式
e ⃗ ( k ) = X ⃗ ( k ) − X d ⃗ ( k ) \vec{e}(k)=\vec{X}(k)-\vec{X_{d}}(k) e(k)=X(k)−Xd(k)
写成矩阵形式
e ⃗ ( k ) = [ I − I ] [ X ⃗ ( k ) X d ⃗ ( k ) ] = D X A ⃗ ( k ) \vec{e}(k)=\begin{bmatrix}I &-I \end{bmatrix} \begin{bmatrix}\vec{X}(k) \\ \vec{X_{d}}(k)\end{bmatrix} =D\vec{X_{A}}(k) e(k)=[I−I][X(k)Xd(k)]=DXA(k)
其中, D = [ I − I ] D=\begin{bmatrix}I &-I\end{bmatrix} D=[I−I]为 n × 2 n n\times2n n×2n矩阵, X A ⃗ = [ X ⃗ ( k ) X d ⃗ ( k ) ] \vec{X_{A}}= \begin{bmatrix}\vec{X}(k) \\ \vec{X_{d}}(k)\end{bmatrix} XA=[X(k)Xd(k)]为 2 n × 1 2n\times 1 2n×1列向量。
根据系统状态转移方程关系,可以得到
X A ⃗ ( k + 1 ) = [ X ⃗ ( k + 1 ) X d ⃗ ( k + 1 ) ] = [ A 0 0 A d ] [ X ⃗ ( k ) X d ⃗ ( k ) ] + [ B 0 ] U ⃗ ( k ) = A D X A ⃗ ( k ) + B D U ⃗ \vec{X_{A}}(k+1)= \begin{bmatrix}\vec{X}(k+1) \\ \vec{X_{d}}(k+1)\end{bmatrix}= \begin{bmatrix}A&0\\0\ & A_{d}\end{bmatrix} \begin{bmatrix}\vec{X}(k) \\ \vec{X_{d}}(k)\end{bmatrix}+ \begin{bmatrix}B \\0\end{bmatrix}\vec{U}(k)=A_{D}\vec{X_{A}}(k)+B_{D}\vec{U} XA(k+1)=[X(k+1)Xd(k+1)]=[A0 0Ad][X(k)Xd(k)]+[B0]U(k)=ADXA(k)+BDU
至此,便得到了用于描述误差向量 e ⃗ \vec{e} e的增广状态向量 X ⃗ A \vec{X}_{A} XA的状态转移方程,总结为以下结论。
e ⃗ ( k ) = D X A ⃗ ( k ) X A ⃗ ( k + 1 ) = A D X A ⃗ ( k ) + B D U ⃗ \vec{e}(k)=D\vec{X_{A}}(k)\\ \vec{X_{A}}(k+1)=A_{D}\vec{X_{A}}(k)+B_{D}\vec{U} e(k)=DXA(k)XA(k+1)=ADXA(k)+BDU
可以看出,当增广后的状态向量 X ⃗ A \vec{X}_{A} XA趋近于0,误差向量 e ⃗ \vec{e} e也会趋近于0,因此我们便将误差向量趋于0的问题转化为了增广状态向量趋于0的问题。
即
e ⃗ → 0 ⇒ D X ⃗ A → 0 \vec{e}\to 0 \Rightarrow D\vec{X}_{A}\to 0 e→0⇒DXA→0 - 狸猫当太子
当我们将问题转化后,问题便回归到之前的讨论范畴,即利用代价函数求解最优控制律的过程。
代价函数为
J = 1 2 ( D X A ⃗ ( N ) ) T P ( 0 ) D X A ⃗ ( N ) + 1 2 ∑ k = 0 N − 1 ( ( D X A ⃗ ( k ) ) T Q D X A ⃗ ( k ) + U ⃗ ( k ) T R U ⃗ ( k ) ) J = \frac{1}{2} (D\vec{X_{A}}(N))^{T}P(0)D\vec{X_A}(N)+\frac{1}{2}\sum_{k=0}^{N-1}{ ((D\vec{X_A}(k))^{T}QD\vec{X_A}(k) + \vec{U}(k)^{T}R\vec{U}(k)) } J=21(DXA(N))TP(0)DXA(N)+21k=0∑N−1((DXA(k))TQDXA(k)+U(k)TRU(k))
整理为:
J = 1 2 X A ⃗ ( N ) T [ D T P ( 0 ) D ] X A ⃗ ( N ) + 1 2 ∑ k = 0 N − 1 ( X A ⃗ ( k ) ) T [ D T Q D ] X A ⃗ ( k ) + U ⃗ ( k ) T R U ⃗ ( k ) ) ⇒ J = 1 2 X A ⃗ ( N ) T P A ( 0 ) X A ⃗ ( N ) + 1 2 ∑ k = 0 N − 1 ( X A ⃗ ( k ) ) T Q A X A ⃗ ( k ) + U ⃗ ( k ) T R U ⃗ ( k ) ) J = \frac{1}{2} \vec{X_{A}}(N)^{T}[D^{T}P(0)D]\vec{X_A}(N)+\frac{1}{2}\sum_{k=0}^{N-1}{ (\vec{X_A}(k))^{T}[D^{T}QD]\vec{X_A}(k) + \vec{U}(k)^{T}R\vec{U}(k)) }\\ \Rightarrow J= \frac{1}{2} \vec{X_{A}}(N)^{T}P_{A}(0)\vec{X_A}(N)+\frac{1}{2}\sum_{k=0}^{N-1}{ (\vec{X_A}(k))^{T}Q_{A}\vec{X_A}(k) + \vec{U}(k)^{T}R\vec{U}(k)) } J=21XA(N)T[DTP(0)D]XA(N)+21k=0∑N−1(XA(k))T[DTQD]XA(k)+U(k)TRU(k))⇒J=21XA(N)TPA(0)XA(N)+21k=0∑N−1(XA(k))TQAXA(k)+U(k)TRU(k))
其中, P A = D T P ( 0 ) D P_{A} = D^{T}P(0)D PA=DTP(0)D, Q A = D T Q D Q_{A}=D^{T}QD QA=DTQD。 P ( 0 ) P(0) P(0)是对误差向量的末端代价权重矩阵, Q Q Q是对过程误差向量的权重矩阵。
并且系统状态转移方程为
X A ⃗ ( k + 1 ) = A D X A ⃗ ( k ) + B D U ⃗ \vec{X_{A}}(k+1)=A_{D}\vec{X_{A}}(k)+B_{D}\vec{U} XA(k+1)=ADXA(k)+BDU
利用上一章推导得到的结论,
K D ( N − k ) = ( B D T P A ( k − 1 ) B D + R ) − 1 B D T P A ( k − 1 ) A D P A ( k ) = ( [ A D − B D K D ( N − k ) ] T ⋅ P A ( k − 1 ) ⋅ [ A D − B D K D ( N − k ) ] + K D ( N − k ) T R K D ( N − k ) + Q A ) J D ∗ ( N − k ) = X ⃗ A T ( N − k ) P A ( k ) X ⃗ A ( N − k ) K_{D}(N-k) = (B_{D}^{T}P_{A}(k-1)B_{D}+R) ^{-1} B_{D}^{T}P_{A}(k-1)A_{D}\\ P_{A}(k) = ( [A_{D}-B_{D}K_{D}(N-k)]^{T} \cdot P_{A}(k-1) \cdot [A_{D}-B_{D}K_{D}(N-k)] + K_{D}(N-k)^{T} RK_{D}(N-k) + Q_{A}) \\ J_{D}^{*}(N-k) = \vec{X}_{A}^{T}(N-k) P_{A}(k) \vec{X}_{A}(N-k) KD(N−k)=(BDTPA(k−1)BD+R)−1BDTPA(k−1)ADPA(k)=([AD−BDKD(N−k)]T⋅PA(k−1)⋅[AD−BDKD(N−k)]+KD(N−k)TRKD(N−k)+QA)JD∗(N−k)=XAT(N−k)PA(k)XA(N−k)
下标写的好乱,大家伙儿看到有下标的就是增广后的就行,大家将就看,我也写的头昏脑涨,在这给大家拜个早年。
最后将计算得到的反馈增益矩阵以全状态反馈的形式代入系统,
U ⃗ k = − K X ⃗ k \vec{U}_{k}=-K\vec{X}_{k} Uk=−KXk
实践仿真
增广系统模型
以同样以弹簧阻尼系统来搞,增广后的系统状态转移方程如下:
X
A
⃗
(
k
+
1
)
=
A
D
X
A
⃗
(
k
)
+
B
D
U
⃗
\vec{X_{A}}(k+1)=A_{D}\vec{X_{A}}(k)+B_{D}\vec{U}
XA(k+1)=ADXA(k)+BDU
其中,
A
D
=
[
1
T
0
0
−
k
T
m
1
−
c
T
m
0
0
0
0
1
0
0
0
0
1
]
A_{D}=\begin{bmatrix} 1&T &0&0\\-\frac{kT}{m}&1-\frac{cT}{m}&0&0\\0&0&1&0\\0&0&0&1\end{bmatrix}
AD=
1−mkT00T1−mcT0000100001
,
B
D
=
[
0
T
m
0
0
]
B_{D}=\begin{bmatrix} 0 \\ \frac{T}{m} \\0\\0\end{bmatrix}
BD=
0mT00
。
仿真代码
clear all;
T = 0.1;
%离散周期位1ms
m = 1;
%重量块质量为1kg
c = 0.2;
k = 0.5;
%阻尼系数和弹簧系数
A = [1 T;-k*T/m 1-c*T/m];
B = [0;T/m];
A_D = [A,zeros(2,2);zeros(2,2),eye(2)];
B_D = [B;zeros(2,1)];
%系统状态空间方程
n = 1000;
x = zeros(n,1);%位置
v = zeros(n,1);%速度
time = zeros(n,1); %时间
u = zeros(n,1); %系统输入
J = zeros(n,1); %代价
JT = zeros(n,1);%代价的导数
%记录状态数据,用来绘图的
X0 = [3;0];
Xd0 = [1;0];
Xa0 = [X0;Xd0];
%系统初始状态向量
Xk = X0;
XAk = Xa0;
%状态向量Xk和增广状态向量
P=zeros(n,16);
%P迭代矩阵,用于计算K
Ca = [eye(2),-eye(2)];
% erro = D A_D
P0 = Ca' * [1 0;0 1] * Ca;
%末端状态代价矩阵2x2
Q = Ca' * [1 0;0 1] * Ca;
%过程状态代价矩阵2x2
R = 1;
%过程输入代价矩阵1x1
K = zeros(n,4);
%全状态反馈矩阵 1x4
P(1,:) = P0(:)';
%初始化
for i = 2:n
tmpP = reshape(P(i-1,:),4,4);
K(n-i+1,:) = reshape( (B_D'*tmpP*B_D+R)\B_D'*tmpP*A_D,1,4);
tmpK = reshape(K(n-i+1,:),1,4);
P(i,:)= reshape( (A_D-B_D * tmpK)'* tmpP *(A_D-B_D * tmpK) + tmpK'*R*tmpK+Q ,1,16);
end
%从最后一个往前算P(k)
for i = 1:n
Kmatrix = reshape(K(i,:),1,4);
uk = - Kmatrix*XAk;
Xk = A*Xk + B*uk;
x(i) = Xk(1);
v(i) = Xk(2);
time(i) = i*T;
u(i) = uk;
XAk = A_D*XAk + B_D*uk;
end
%k->N时刻的最优代价
Xn = [x(n);v(n);Xd0];
Jn = 0.5 * Xn' * P0 * Xn;
J(n-1) = Jn;
for i = 2:n
tmpP = reshape(P(i-1,:),4,4);
tmpX = [x(n-i+1);v(n-i+1);Xd0];
J(n-i+1) = J(n-i+2) + 0.5 * tmpX' * Q * tmpX + u(n-i+1)' * R * u(n-i+1);
JT(n-i+1) = B_D'* tmpP * (A_D * tmpX + B_D * u(n-i+1)) + R * u(n-i+1);
end
plot_row = 5;
plot_column = 1;
subplot(plot_row,plot_column,1);
plot(time, x) % 绘制曲线
xlabel('t') % 添加x轴标签
ylabel('x') % 添加y轴标签
title('x-t') % 添加标题
grid on % 添加网格线
subplot(plot_row,plot_column,2);
plot(time,v) % 绘制曲线
xlabel('t') % 添加x轴标签
ylabel('v') % 添加y轴标签
title('v-t') % 添加标题
grid on % 添加网格线
subplot(plot_row,plot_column,3);
plot(time,u) % 绘制曲线
xlabel('t') % 添加x轴标签
ylabel('u') % 添加y轴标签
title('u-t') % 添加标题
grid on % 添加网格线
subplot(plot_row,plot_column,4);
plot(time,J) % 绘制曲线
xlabel('t') % 添加x轴标签
ylabel('J') % 添加y轴标签
title('J-t') % 添加标题
grid on % 添加网格线
subplot(plot_row,plot_column,5);
plot(time,JT) % 绘制曲线
xlabel('t') % 添加x轴标签
ylabel('JT ') % 添加y轴标签
title('JT-t') % 添加标题
grid on % 添加网格线
运行结果
可以清楚的看到,位置并没有按照期望的停在1处,一开始我也以为是因为代码或者公式推导出现错误导致的。后来我才反应过来,这是因为
L
Q
R
LQR
LQR计算的最优,并不一定是期望点为最优。
- 躺平的系统
由于位置 x = 1 x=1 x=1并不是系统的稳定点,因此需要外部输入来保持这种不稳定的状态,因此会出现一种情况——系统不愿意做太多的输入,为了保证数值上的最低代价,所以系统选择半躺平或者说半摆烂的状态,导致控制器存在静态误差。
并且可以看出代价和代价的导数情况,系统调控过程中,代价导数长期处于0,即他认为当前的系统代价是最优的。系统也觉得这样就够了也开始摆了
。 - 励志的系统
那么如何让系统奋发向上,积极努力的完全达到设定的目标呢。
可以让输入权重矩阵 R R R为0试试。
可以看到,位置确实稳定在了 x = 1 x=1 x=1处,但是也可以看到系统输入 U ⃗ \vec{U} U也付出了很大的代价,原先他花费的输入并不大,但是为了达到完全的期望,花费了数十倍的努力,也确实让它达到目标了。 - 努力和结果的平衡
这次实验仿真说明了,要平衡好努力和回报,设定合理的期望,做出适当的付出,对不够理想的结果要能够接受。累了就好好休息,不要硬抗!。
结论
- 从这一次实验中可以看出,当期望向量处于系统的非稳定点时,容易出现一定的静态误差。
- 当控制器参数不理想时,可能会导致系统‘摆烂’。
- L Q R LQR LQR适用于常数期望,读者可以将期望做成正弦曲线来观察它的跟踪性能,个人尝试效果不好。
- 个人希望你们不要仅仅停留在看完就算了,多动手,有什么疑问都去实践观察一下,再结合现象思考,帮助理解。
后续
后续的话可能会继续围绕今天提到的 L Q R LQR LQR的几个问题继续去讨论,也可能写一些其他控制算法的博客,可能是 M P C MPC MPC,也可能是 S M C SMC SMC,或者出一些实战的控制系列博客。感谢阅读,敬请期待!