浅谈线性二次型调节器（LQR）算法（四）—— 非零期望点静态误差的解决思路

---

---

前言

在前面第三篇的博客中，叙述了关于$LQR$非零期望的轨迹跟踪推导和实现。在最后的仿真实践中发现一个现象，当代价函数中的输入权重矩阵$R$非0时，会使得状态向量与期望状态向量之间存在静态误差。当把输入权重矩阵$R$置零时，则可以将静态误差消除，但此时系统输入$U$将会出现非常大的起伏变化。这篇博客将围绕这个现象来讨论，如何以合适的方式来解决非零期望点静态误差的问题。

盘根溯源

线性系统的平衡点

$$\vec{X}(k+1)=A\vec{X}(k)$$
显然在无输入的情况下，线性系统的平衡点只有$\vec{X}=0$。因此当我们希望系统状态向量稳定在其他位置时，则需要有外部输入来保持。即
$$\vec{X}(k+1)=A\vec{X}(k)+B\vec{U}(k)$$
在外部输入使得系统保持在某个非零稳定点后，系统的输入也将稳定在某个位置，使系统处于平衡状态，即使得
$$\vec{X}(k+1)=\vec{X}(k)$$
且
$$\vec{U}(k+1)=\vec{U}(k)={\vec{U}}_{const}$$

代价函数的形式

$$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}}_{e=0}$所对应的代价$J_{e=0}$并不是最优的（即不是最小的代价）。
因此，经过$LQR$计算的控制律$\vec{U}$产生的控制结果就会出现静态误差（因为它认为这就是最优的）。

消除静态误差的思想

系统输入增量控制

既然系统输入会影响代价函数，进而使误差向量无法收敛为0，那么就不对它计算代价值，取代它的是系统输入的增量（即变化量）。
即增量输入满足以下定义：
$$\vec{U}(k)=\vec{U}(k-1)+\Delta \vec{U}(k)$$
其中，$\Delta \vec{U}(k)$是系统在$k$时刻的输入增量。
而我们希望的是在代价函数中由$k$时刻的输入增量$\Delta \vec{U}(k)$取代$k$时刻的系统输入$\vec{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)+\Delta \vec{U}(k{)}^{T}R\Delta \vec{U}(k))$$
与上一章中的推导思路相同，我们还需要找到$\Delta \vec{U}(k)$如何影响误差向量$\vec{e}$，即用状态空间方程的形式来描述他们之间的数学关系，才能使得代价函数的最优化计算成立。
方法也与上一章的方法相似，也是通过状态向量增广变化的方式来实现。

• 状态空间方程
  $$\vec{X}(k+1)=A\vec{X}(k)+B\vec{U}(k)=A\vec{X}(k)+B\vec{U}(k-1)+B\Delta \vec{U}(k)$$
  很显然，状态空间方程中多出了一项 $B\vec{U}(k-1)$，需要将这一项进行增广变换来重新构造状态空间方程。
• 对向量进行增广变换
  保留上一章轨迹跟踪的增广变换部分，新加入多出来的$B\vec{U}(k-1)$，则有
  $${\vec{X}}_b(k)=\left[\begin{array}{c}\vec{X}(k)\\ \overrightarrow{X_d}(k)\\ \vec{U}(k-1)\end{array}\right]$$
• 增广变换后的状态空间方程
  $$\begin{gathered} \overrightarrow{X_b}(k+1)=\left[\begin{array}{ccc}A& 0& B\\ 0& A_D& 0\\ 0& 0& I\end{array}\right]\left[\begin{array}{c}\vec{X}(k)\\ \overrightarrow{X_d}(k)\\ \vec{U}(k-1)\end{array}\right]+\left[\begin{array}{c}B\\ 0\\ I\end{array}\right]\Delta \vec{U}(k) \\ \Rightarrow \overrightarrow{X_b}(k+1)=A_b\overrightarrow{X_b}(k)+B_b\Delta \vec{U}(k) \end{gathered}$$
• 误差向量与增广状态向量之间的转换
  $$\vec{e}(k)=\vec{X}(k)-\overrightarrow{X_d}(k)+0\times \vec{U}(k-1)=\left[\begin{array}{ccc}I& -I& 0\end{array}\right]\left[\begin{array}{c}\vec{X}(k)\\ \overrightarrow{X_d}(k)\\ \vec{U}(k-1)\end{array}\right]=C_b{\vec{X}}_b(k)$$
• 整理后得到新的代价函数形式
  $$\begin{gathered} J=\frac{1}{2}{\vec{X}}_b(N{)}^T[C_b^{T}P(0)C_b]{\vec{X}}_b(N)+\frac{1}{2}\sum _{k=0}^{N-1}({\vec{X}}_b(k{)}^T[C_b^{T}Q{C}_b]{\vec{X}}_b(k)+\Delta \vec{U}(k{)}^{T}R\Delta \vec{U}(k)) \\ \Rightarrow J=\frac{1}{2}{\vec{X}}_b(N{)}^T{P}_b(0){\vec{X}}_b(N)+\frac{1}{2}\sum _{k=0}^{N-1}({\vec{X}}_b(k{)}^T{Q}_b{\vec{X}}_b(k)+\Delta \vec{U}(k{)}^{T}R\Delta \vec{U}(k)) \end{gathered}$$
• 根据$LQR$计算公式
  $$\begin{gathered} K_b(N-k)=(B_b^T{P}_b(k-1)B_b+R{)}^{-1}{B}_b^T{P}_b(k-1)A_b \\ {P}_b(k)=([A_b-B_b{K}_b(N-k){]}^T\cdot P_b(k-1)\cdot [A_b-B_b{K}_b(N-k)]+K_b(N-k{)}^{T}R{K}_b(N-k)+Q_b) \\ {J}_b^{\ast }(N-k)={\vec{X}}_b^T(N-k)P_b(k){\vec{X}}_b(N-k) \end{gathered}$$
• 计算控制律
  $$\vec{U}(k)=\vec{U}(k-1)-K_b(k)X_b(k)$$

稳态输入参考控制

在前面也提到过，假设在系统输入的作用下，系统状态向量保持在一个非零点$\overrightarrow{X_d}$，将这种稳态输入称之为稳态输入向量$\overrightarrow{U_d}$，且满足以下关系
$$\begin{gathered} \vec{X}(k+1)=\overrightarrow{X_d}=A\vec{X}(k)+B\vec{U}(k)=A\overrightarrow{X_d}+B\overrightarrow{U_d} \\ ①\Rightarrow (I-A){\vec{X}}_d=B{\vec{U}}_d \end{gathered}$$
定义稳态输入误差 $\delta \vec{U}(k)$，其满足以下关系
$$②\vec{U}(k)=\delta \vec{U}(k)+\overrightarrow{U_d}$$
代入系统状态空间方程
$$③\vec{X}(k+1)=A\vec{X}(k)+B\overrightarrow{U_d}+B\delta \vec{U}(k)$$
将①代入③中，得到
$$④\vec{X}(k+1)=A\vec{X}(k)+(I-A){\vec{X}}_d+B\delta \vec{U}(k)$$
又根据轨迹跟踪推导的增广状态向量$\overrightarrow{X_c}(k)=\left[\begin{array}{c}\vec{X}(k)\\ \overrightarrow{X_d}(k)\end{array}\right]$，因此④可以增广变换为
$$\begin{gathered} {\vec{X}}_c(k+1)=\left[\begin{array}{cc}A& I-A\\ 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]\delta \vec{U}(k) \\ ⑤\Rightarrow {\vec{X}}_c(k+1)=A_c{\vec{X}}_c(k)+B_c\delta \vec{U}(k) \end{gathered}$$
并且误差向量满足以下关系：
$$\vec{e}(k)=\vec{X}(k)-\overrightarrow{X_d}(k)=\left[\begin{array}{cc}I& -I\end{array}\right]\left[\begin{array}{c}\vec{X}(k)\\ \overrightarrow{X_d}(k)\end{array}\right]=C_c{\vec{X}}_c(k)$$
推导出这个新的增广状态空间方程，则可以将代价函数中的系统输入向量替换为稳态输入误差向量，以此达到消除静态误差的目的：
$$\begin{gathered} J=\frac{1}{2}{\vec{X}}_c(N{)}^T[C_c^{T}P(0)C_c]{\vec{X}}_c(N)+\frac{1}{2}\sum _{k=0}^{N-1}({\vec{X}}_c(k{)}^T[C_c^{T}Q{C}_c]{\vec{X}}_c(k)+\delta \vec{U}(k{)}^{T}R\delta \vec{U}(k)) \\ \Rightarrow J=\frac{1}{2}{\vec{X}}_c(N{)}^T{P}_c(0){\vec{X}}_c(N)+\frac{1}{2}\sum _{k=0}^{N-1}({\vec{X}}_c(k{)}^T{Q}_c{\vec{X}}_c(k)+\delta \vec{U}(k{)}^{T}R\delta \vec{U}(k)) \end{gathered}$$
$$

• 根据$LQR$计算公式
  $$\begin{gathered} K_c(N-k)=(B_c^T{P}_c(k-1)B_c+R{)}^{-1}{B}_c^T{P}_c(k-1)A_c \\ {P}_c(k)=([A_c-B_c{K}_c(N-k){]}^T\cdot P_c(k-1)\cdot [A_c-B_c{K}_c(N-k)]+K_c(N-k{)}^{T}R{K}_c(N-k)+Q_c) \\ {J}_c^{\ast }(N-k)={\vec{X}}_c^T(N-k)P_c(k){\vec{X}}_c(N-k) \end{gathered}$$
• 计算控制律
  $$\vec{U}(k)={\vec{U}}_d-K_c(k)X_c(k)$$

实践仿真

仿真代码 —— 系统输入增量控制

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_b = [A,zeros(2,2),B;zeros(2,2),eye(2),zeros(2,1);zeros(1,2),zeros(1,2),1];
B_b = [B;zeros(2,1);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];
u0 = 0;
Xb0 = [X0;Xd0;u0];
%系统初始状态向量
Xk = X0;
Xbk = Xb0;
uk = 0;
duk = 0;
%状态向量Xk和增广状态向量
P=zeros(n,25);
%P 5x5
Cb = [eye(2),-eye(2),zeros(2,1)];
% Cb 2x5
P0 = Cb' * [1 0;0 1] * Cb;
%末端状态代价矩阵2x2
Q = Cb' * [1 0;0 1] * Cb;
%过程状态代价矩阵2x2
R = 1;
%过程输入代价矩阵1x1
K = zeros(n,5);
%全状态反馈矩阵 1x5
P(1,:) = P0(:)';
%初始化

for i = 2:n
    tmpP = reshape(P(i-1,:),5,5);   
    K(n-i+1,:) = reshape( (B_b'*tmpP*B_b+R)\B_b'*tmpP*A_b,1,5);
    tmpK = reshape(K(n-i+1,:),1,5);
    P(i,:)= reshape( (A_b-B_b * tmpK)'* tmpP *(A_b-B_b * tmpK) + tmpK'*R*tmpK+Q ,1,25);
end
%从最后一个往前算P(k)

for i = 1:n
    Kmatrix = reshape(K(i,:),1,5);
    duk = - Kmatrix*Xbk;     
    uk = uk+duk;
    Xk = A*Xk + B*uk;
    x(i) = Xk(1);
    v(i) = Xk(2);
    time(i) = i*T;
    u(i) = uk;
    Xbk = A_b*Xbk + B_b*duk;    
end    

figure(2);
plot_row = 3;
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 % 添加网格线

运行结果 —— 系统输入增量控制

• 对比实验，图左为输入增量式，图右为常规式。
  在所有权重矩阵，系统参数都一样的情况下，输入增量方式的$LQR$能够消除静态误差，并且以几乎不变的系统输入完成这个目标。
  

仿真代码 —— 稳态输入参考控制

根据系统模型为
$$\left[\begin{array}{c}{x}_1(k+1)\\ x_2(k+1)\end{array}\right]=\left[\begin{array}{cc}1& T\\ -\frac{kT}{m}& 1-\frac{cT}{m}\end{array}\right]\left[\begin{array}{c}{x}_1(k)\\ x_2(k)\end{array}\right]+\left[\begin{array}{c}0\\ \frac{T}{m}\end{array}\right]u$$
其中，$m=1$，$c=0.2$，$k=0.5$，$T=0.1$。
另$\overrightarrow{X_d}=\left[\begin{array}{c}1\\ 0\end{array}\right]$，代入后可以得到
$$\begin{gathered} 1=1 \\ 0=-\frac{0.5\ast 0.1}{1}\times 1+0+0.1\times \overrightarrow{U_d} \end{gathered}$$
得到$\overrightarrow{U_d}=0.5$。

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_c = [A,eye(2)-A;zeros(2,2),eye(2)];
B_c = [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];
u0 = 0;
Xc0 = [X0;Xd0];
%系统初始状态向量
Xk = X0;
Xck = Xc0;
ud = 0.5;
uk = 0;
duk = 0;
%状态向量Xk和增广状态向量
P=zeros(n,16);
%P 5x5
Cb = [eye(2),-eye(2)];
% Cb 2x5
P0 = Cb' * [1 0;0 1] * Cb;
%末端状态代价矩阵2x2
Q = Cb' * [1 0;0 1] * Cb;
%过程状态代价矩阵2x2
R = 1;
%过程输入代价矩阵1x1
K = zeros(n,4);
%全状态反馈矩阵 1x5
P(1,:) = P0(:)';
%初始化

for i = 2:n
    tmpP = reshape(P(i-1,:),4,4);   
    K(n-i+1,:) = reshape( (B_c'*tmpP*B_c+R)\B_c'*tmpP*A_c,1,4);
    tmpK = reshape(K(n-i+1,:),1,4);
    P(i,:)= reshape( (A_c-B_c * tmpK)'* tmpP *(A_c-B_c * tmpK) + tmpK'*R*tmpK+Q ,1,16);
end
%从最后一个往前算P(k)

for i = 1:n
    Kmatrix = reshape(K(i,:),1,4);
    duk = - Kmatrix*Xck;     
    uk = ud+duk;
    Xk = A*Xk + B*uk;
    x(i) = Xk(1);
    v(i) = Xk(2);
    time(i) = i*T;
    u(i) = uk;
    Xck = A_c*Xck + B_c*duk;    
end    

figure(3);
plot_row = 3;
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 % 添加网格线

运行结果 —— 稳态输入参考控制

• 右边是稳态输入参考控制，左边是无特殊操作
  

结论

• 提出了关于消除$LQR$静态误差的设计思想，并且通过仿真验证这一结论。
• 无论哪种思路，要记住是如何推导的，也要理解他的思想。
• 两种思路都是替换掉代价函数系统输入项来消除静态误差，告诉我们一个道理，如果她真的不适合你，趁早换了。（毒鸡汤）
• 输入增量控制的思路适用于期望可能频繁变换的场景，告诉我们一个道理，如果有时候不能一步到位，慢慢积累也不失为一种办法。（毒鸡汤）
• 稳态输入参考的思路适用于期望点恒定的场景，告诉我们一个道理，尽管一眼望到终点，但也难免路途坎坷。（毒鸡汤）

后续

• 好像关于$LQR$的内容已经没有太多能继续讨论的了，后续大大大大概率会往其他控制器拓展了。
• 后续想做一个系列——以平衡车为案例的控制实践，涉及内容包括机械设计、运动建模、控制仿真、程序编写、调试等，有兴趣的同学关注一波，也可以评论或者私信我交流。
• 敬请期待！