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

争取35岁退休

已于 2024-04-17 23:56:51 修改

阅读量2.3k

点赞数 23

分类专栏：数据融合和控制算法文章标签：算法概率论机器学习

于 2024-01-14 00:52:23 首次发布

本文链接：https://blog.csdn.net/m0_37835056/article/details/135572162

版权

数据融合和控制算法专栏收录该内容

16 篇文章 21 订阅

订阅专栏

传送门

前言
再看 $L QR$
如何实现非0期望的控制
实践仿真
- 增广系统模型
- 仿真代码
运行结果
结论
后续

前言

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

再看 $L QR$

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

状态向量全能控
对于系统状态向量全能控的线性系统而言，经过 $L QR$ 控制器调节的系统稳定点一定为 $0$ 向量。通过以下推导得到：
系统状态转移方程
$\vec{X}(k+1)=A \vec{X}(k)+B\vec{U}(k)$
又 $L QR$ 的控制律为
$\vec{U}(k)=-K\vec{X}(k)$
代入系统后得到
$\vec{X}(k+1) = (A-BK)\vec{X}(k)$
若 $k$ 时刻的状态向量不为 $\vec{0}$ ，则经过 $L QR$ 最优计算得到的矩阵 $K$ ，会进一步将 $\vec{X}'(k)$ 调整为使 $\vec{X}$ 趋近于 $\vec{0}$ 的方向，直到状态向量为 $\vec{X}=\vec{0}$ ，此时系统将处于稳定状态，因为 $\vec{X}'=0$ ，系统状态向量将不再变化。
因此可以看出，以这种代价函数
$\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$ 时，若没有外力干涉，则无法稳定下来，即期望的状态向量 $\vec{X_{d}}=\begin{bmatrix} x_{d} \\0 \end{bmatrix}$ 不是系统稳定向量。
那么应该如何计算最优的系统输入，使系统快速收敛于期望的系统状态向量。
狸猫换太子
上面关于代价函数的描述可以总结为，利用以下形式
$\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}^{*}$ 不断趋近于0，那么如果将系统中的状态向量替换为误差向量 $\vec{e}=\vec{X} - \vec{X_{d}}$ (反着写也可以，推导过程同理)，则可以使误差向量趋近于0，即状态向量 $\vec{X}$ 趋近于期望向量 $\vec{X_{d}}$ 。
则代价函数更改为：
$\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)-\vec{X_{d}}(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)$
其中， $D=\begin{bmatrix}I &-I\end{bmatrix}$ 为 $n\times2n$ 矩阵， $\vec{X_{A}}= \begin{bmatrix}\vec{X}(k) \\ \vec{X_{d}}(k)\end{bmatrix}$ 为 $2n\times 1$ 列向量。
根据系统状态转移方程关系，可以得到
$\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}$
至此，便得到了用于描述误差向量 $\vec{e}$ 的增广状态向量 $\vec{X}_{A}$ 的状态转移方程，总结为以下结论。
$\vec{e}(k)=D\vec{X_{A}}(k)\\ \vec{X_{A}}(k+1)=A_{D}\vec{X_{A}}(k)+B_{D}\vec{U}$
可以看出，当增广后的状态向量 $\vec{X}_{A}$ 趋近于0，误差向量 $\vec{e}$ 也会趋近于0，因此我们便将误差向量趋于0的问题转化为了增广状态向量趋于0的问题。
即
$\vec{e}\to 0 \Rightarrow D\vec{X}_{A}\to 0$
狸猫当太子
当我们将问题转化后，问题便回归到之前的讨论范畴，即利用代价函数求解最优控制律的过程。
代价函数为
$\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)) }$
整理为：
$\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)) }$
其中， $P_{A} = D^{T}P(0)D$ ， $Q_{A}=D^{T}QD$ 。 $P (0)$ 是对误差向量的末端代价权重矩阵， $Q$ 是对过程误差向量的权重矩阵。
并且系统状态转移方程为
$\vec{X_{A}}(k+1)=A_{D}\vec{X_{A}}(k)+B_{D}\vec{U}$
利用上一章推导得到的结论，
$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)$
下标写的好乱，大家伙儿看到有下标的就是增广后的就行，大家将就看，我也写的头昏脑涨，在这给大家拜个早年。
最后将计算得到的反馈增益矩阵以全状态反馈的形式代入系统，
$\vec{U}_{k}=-K\vec{X}_{k}$

实践仿真

增广系统模型

以同样以弹簧阻尼系统来搞，增广后的系统状态转移方程如下：
在这里插入图片描述
$\vec{X_{A}}(k+1)=A_{D}\vec{X_{A}}(k)+B_{D}\vec{U}$
其中， $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}$ ， $B_{D}=\begin{bmatrix} 0 \\ \frac{T}{m} \\0\\0\end{bmatrix}$ 。

仿真代码

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 QR$ 计算的最优，并不一定是期望点为最优。

躺平的系统
由于位置 $x = 1$ 并不是系统的稳定点，因此需要外部输入来保持这种不稳定的状态，因此会出现一种情况——系统不愿意做太多的输入，为了保证数值上的最低代价，所以系统选择半躺平或者说半摆烂的状态，导致控制器存在静态误差。
并且可以看出代价和代价的导数情况，系统调控过程中，代价导数长期处于0，即他认为当前的系统代价是最优的。系统也觉得这样就够了也开始摆了。
励志的系统
那么如何让系统奋发向上，积极努力的完全达到设定的目标呢。
可以让输入权重矩阵 $R$ 为0试试。

可以看到，位置确实稳定在了 $x = 1$ 处，但是也可以看到系统输入 $\vec{U}$ 也付出了很大的代价，原先他花费的输入并不大，但是为了达到完全的期望，花费了数十倍的努力，也确实让它达到目标了。
努力和结果的平衡
这次实验仿真说明了，要平衡好努力和回报，设定合理的期望，做出适当的付出，对不够理想的结果要能够接受。累了就好好休息，不要硬抗!。