浅谈线性二次型调节器（LQR）算法（三）—— 轨迹跟踪公式推导及仿真代码_lqr 轨迹跟踪

---

---

前言

上一篇博客简单推导了离散系统下的$LQR$计算公式，并且留下了一个伏笔——如何使系统状态收敛到非0状态。这篇博客将围绕这个问题来展开，并秉持着想到什么写什么的原则来进行一系列freestyle。
skr~

再看$LQR$

经过了上篇博客的推导，可以理解我们得到的控制律，是根据代价函数的最优问题求解的。假设代价函数如下：
$$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$用于衡量控制律性能的指标，由于函数中每一项都是半正定的，因此可以得到
$$J\ge 0$$
另外，代价函数中包括了末端向量$\vec{X}(N)$，过程向量$\vec{X}(k)$，系统输入$\vec{U}(k)$。很明显，这其中唯一能由我们决定的是系统输入$\vec{U}(k)$。因此我们对于代价函数进行最优计算的时候，实际上是设计一组系统输入序列$\vec{U}$，使得代价$J$尽可能的小（因为J存在下限0，无上限）。

• 状态向量全能控
  对于系统状态向量全能控的线性系统而言，经过$LQR$控制器调节的系统稳定点一定为$0$向量。通过以下推导得到：
  系统状态转移方程
  $$\vec{X}(k+1)=A\vec{X}(k)+B\vec{U}(k)$$
  又$LQR$的控制律为
  $$\vec{U}(k)=-K\vec{X}(k)$$
  代入系统后得到
  $$\vec{X}(k+1)=(A-BK)\vec{X}(k)$$
  若$k$时刻的状态向量不为$\vec{0}$，则经过$LQR$最优计算得到的矩阵$K$，会进一步将${\vec{X}}^{\prime }(k)$调整为使$\vec{X}$趋近于$\vec{0}$的方向，直到状态向量为$\vec{X}=\vec{0}$，此时系统将处于稳定状态，因为${\vec{X}}^{\prime }=0$，系统状态向量将不再变化。
  因此可以看出，以这种代价函数
  $$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))$$
  得到的最优控制律，它总是将状态向量$\vec{X}$往$\vec{0}$趋近。
  上述的描述说明了，这在数学上是一条可以实现的最优路径。

如何实现非0期望的控制

• 以之前的弹簧阻尼系统为例
  假设现在希望质量块在某个期望的位置$x=x_d$稳定下来$v=0$，应该如何做？
  可以知道，若$x_d\ne 0$时，若没有外力干涉，则无法稳定下来，即期望的状态向量$\overrightarrow{X_d}=\left[\begin{array}{c}{x}_d\\ 0\end{array}\right]$不是系统稳定向量。
  那么应该如何计算最优的系统输入，使系统快速收敛于期望的系统状态向量。
• 狸猫换太子
  上面关于代价函数的描述可以总结为，利用以下形式
  $$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))$$
  求解得到的系统输入$\vec{U}$，会使得系统可控的状态${\vec{X}}^{\ast }$不断趋近于0，那么如果将系统中的状态向量替换为误差向量$\vec{e}=\vec{X}-\overrightarrow{X_d}$(反着写也可以，推导过程同理)，则可以使误差向量趋近于0，即状态向量$\vec{X}$趋近于期望向量$\overrightarrow{X_d}$。
  则代价函数更改为：
  $$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))$$
  这个时候会产生一个问题，即原先的代价函数中，能通过对系统输入$\vec{U}$求解最优代价，需要具备一个重要的前提，即对于系统状态向量$\vec{X}$，能受$\vec{U}$直接或间接影响的。
  这里其实就是系统能控性的判定。
  根据系统状态转移方程，系统输入$\vec{U}$直接参与状态向量的计算。
  $$\vec{X}(k+1)=A\vec{X}(k)+B\vec{U}(k)$$
  因此当我们将代价函数修改成以误差向量$\vec{e}$为基准时，需要补充描述系统输入$\vec{U}$是如何影响$\vec{e}$的，只要完成这一步，新的代价函数的最优求解在数学上也能够成立。
• 狸猫变太子
  由误差向量表达式
  $$\vec{e}(k)=\vec{X}(k)-\overrightarrow{X_d}(k)$$
  写成矩阵形式
  $$\vec{e}(k)=\left[\begin{array}{cc}I& -I\end{array}\right]\left[\begin{array}{c}\vec{X}(k)\\ \overrightarrow{X_d}(k)\end{array}\right]=D\overrightarrow{X_A}(k)$$
  其中，$D=\left[\begin{array}{cc}I& -I\end{array}\right]$为$n\times 2n$矩阵，$\overrightarrow{X_A}=\left[\begin{array}{c}\vec{X}(k)\\ \overrightarrow{X_d}(k)\end{array}\right]$为$2n\times 1$列向量。
  根据系统状态转移方程关系，可以得到
  $$\overrightarrow{X_A}(k+1)=\left[\begin{array}{c}\vec{X}(k+1)\\ \overrightarrow{X_d}(k+1)\end{array}\right]=\left[\begin{array}{cc}A& 0\\ 0& A_d\end{array}\right]\left[\begin{array}{c}\vec{X}(k)\\ \overrightarrow{X_d}(k)\end{array}\right]+\left[\begin{array}{c}B\\ 0\end{array}\right]\vec{U}(k)=A_D\overrightarrow{X_A}(k)+B_D\vec{U}$$
  至此，便得到了用于描述误差向量$\vec{e}$的增广状态向量${\vec{X}}_A$的状态转移方程，总结为以下结论。
  $$\begin{gathered} \vec{e}(k)=D\overrightarrow{X_A}(k) \\ \overrightarrow{X_A}(k+1)=A_D\overrightarrow{X_A}(k)+B_D\vec{U} \end{gathered}$$
  可以看出，当增广后的状态向量${\vec{X}}_A$趋近于0，误差向量$\vec{e}$也会趋近于0，因此我们便将误差向量趋于0的问题转化为了增广状态向量趋于0的问题。
  即
  $$\vec{e}\to 0\Rightarrow D{\vec{X}}_A\to 0$$
• 狸猫当太子
  当我们将问题转化后，问题便回归到之前的讨论范畴，即利用代价函数求解最优控制律的过程。
  代价函数为
  $$J=\frac{1}{2}(D\overrightarrow{X_A}(N){)}^{T}P(0)D\overrightarrow{X_A}(N)+\frac{1}{2}\sum _{k=0}^{N-1}((D\overrightarrow{X_A}(k){)}^{T}QD\overrightarrow{X_A}(k)+\vec{U}(k{)}^{T}R\vec{U}(k))$$
  整理为：
  $$\begin{gathered} J=\frac{1}{2}\overrightarrow{X_A}(N{)}^T[D^{T}P(0)D]\overrightarrow{X_A}(N)+\frac{1}{2}\sum _{k=0}^{N-1}(\overrightarrow{X_A}(k){)}^T[D^{T}QD]\overrightarrow{X_A}(k)+\vec{U}(k{)}^{T}R\vec{U}(k)) \\ \Rightarrow J=\frac{1}{2}\overrightarrow{X_A}(N{)}^T{P}_A(0)\overrightarrow{X_A}(N)+\frac{1}{2}\sum _{k=0}^{N-1}(\overrightarrow{X_A}(k){)}^T{Q}_A\overrightarrow{X_A}(k)+\vec{U}(k{)}^{T}R\vec{U}(k)) \end{gathered}$$
  其中，$P_A=D^{T}P(0)D$，$Q_A=D^{T}QD$。$P(0)$是对误差向量的末端代价权重矩阵，$Q$是对过程误差向量的权重矩阵。
  并且系统状态转移方程为
  $$\overrightarrow{X_A}(k+1)=A_D\overrightarrow{X_A}(k)+B_D\vec{U}$$
  利用上一章推导得到的结论，
  $$\begin{gathered} 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}R{K}_D(N-k)+Q_A) \\ {J}_D^{\ast }(N-k)={\vec{X}}_A^T(N-k)P_A(k){\vec{X}}_A(N-k) \end{gathered}$$
  下标写的好乱，大家伙儿看到有下标的就是增广后的就行，大家将就看，我也写的头昏脑涨，在这给大家拜个早年。
  最后将计算得到的反馈增益矩阵以全状态反馈的形式代入系统，
  $${\vec{U}}_k=-K{\vec{X}}_k$$

实践仿真

增广系统模型

以同样以弹簧阻尼系统来搞，增广后的系统状态转移方程如下：

$$\overrightarrow{X_A}(k+1)=A_D\overrightarrow{X_A}(k)+B_D\vec{U}$$
其中， 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−mkT​00​T1−mcT​00​0010​0001​ ​，$B_D=\left[\begin{array}{c}0\\ \frac{T}{m}\\ 0\\ 0\end{array}\right]$。

仿真代码

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
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106

运行结果

可以清楚的看到，位置并没有按照期望的停在1处，一开始我也以为是因为代码或者公式推导出现错误导致的。后来我才反应过来，这是因为$LQR$计算的最优，并不一定是期望点为最优。

• 躺平的系统
  由于位置$x=1$并不是系统的稳定点，因此需要外部输入来保持这种不稳定的状态，因此会出现一种情况——系统不愿意做太多的输入，为了保证数值上的最低代价，所以系统选择半躺平或者说半摆烂的状态，导致控制器存在静态误差。
  并且可以看出代价和代价的导数情况，系统调控过程中，代价导数长期处于0，即他认为当前的系统代价是最优的。系统也觉得这样就够了也开始摆了。
• 励志的系统
  那么如何让系统奋发向上，积极努力的完全达到设定的目标呢。
  可以让输入权重矩阵$R$为0试试。
  
  可以看到，位置确实稳定在了$x=1$处，但是也可以看到系统输入$\vec{U}$也付出了很大的代价，原先他花费的输入并不大，但是为了达到完全的期望，花费了数十倍的努力，也确实让它达到目标了。
• 努力和结果的平衡
  这次实验仿真说明了，要平衡好努力和回报，设定合理的期望，做出适当的付出，对不够理想的结果要能够接受。累了就好好休息，不要硬抗!。

结论

• 从这一次实验中可以看出，当期望向量处于系统的非稳定点时，容易出现一定的静态误差。
• 当控制器参数不理想时，可能会导致系统‘摆烂’。
• $LQR$适用于常数期望，读者可以将期望做成正弦曲线来观察它的跟踪性能，个人尝试效果不好。
• 个人希望你们不要仅仅停留在看完就算了，多动手，有什么疑问都去实践观察一下，再结合现象思考，帮助理解。

后续

后续的话可能会继续围绕今天提到的$LQR$的几个问题继续去讨论，也可能写一些其他控制算法的博客，可能是$MPC$，也可能是$SMC$，或者出一些实战的控制系列博客。感谢阅读，敬请期待！