浅谈线性二次型调节器（LQR）算法（二）—— 离散系统及仿真代码

争取35岁退休

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

阅读量2k

点赞数 24

分类专栏：数据融合和控制算法文章标签：算法线性代数矩阵 matlab

于 2024-01-09 16:39:29 首次发布

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

版权

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

16 篇文章 20 订阅

订阅专栏

传送门

前言
线性二次型控制器
动手实践
结论
- 优势
- 劣势
后续

前言

之前已经写过一篇关于LQR控制器的理解，但是在看了一些资料，重新思考复盘过，认为原先关于LQR的理解有一定的投机取巧成分。并且只阐述了连续系统下的相关公式推导，并没有对离散系统进行推导叙述。
但为了能够体现出两次思考的差别，便没有将原有博客删除或直接修改，而是新起一篇博客进行重新描述整个理解和公式推导过程，最后会给出仿真过程，并将仿真文件上传分享。
为了不做一些重复性赘述，默认读者掌握相关的基础知识

线性二次型控制器

状态空间方程
$\vec{X}' = A\vec{X} + B\vec{U}\\ Y=C\vec{X}$
对于LQR来说，只需要讨论其状态转移方程，并且将连续系统转换成离散系统。对应的状态转移方程为
$\vec{X}_{k+1} = A_{D}\vec{X}_{k}+B_{D}\vec{U}_{k}$

这里简单讨论一下 $A$ 和 $A_{D}$ ， $B$ 和 $B_{D}$ 之间的关系，由于离散系统还与离散周期有关系，因此需要假定离散周期为 $\Delta T$ ，根据连续状态转移方程
$\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}$
即可以得到
$A_{D} = E+\Delta T A \\ B_{D}=\Delta T B$

代价函数（Cost Function）
代价函数是用来描述LQR控制倾向的函数，通过调节权重矩阵来修改控制特性。另外，代价函数用于计算控制律，其中的权重矩阵最终都会影响控制律，进而影响控制效果。
代价函数的形式为
$\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}$
其中， $t_f$ 为目标时刻， $t_0$ 为初始时刻。矩阵 $F$ 为末端代价权重矩阵，用于描述末端状态向量的权重大小。矩阵 $Q$ 和矩阵 $R$ 分别状态向量 $\vec{X}$ 和系统输入 $\vec{U}$ 在过程代价中的权重。
由于是对离散系统进行LQR控制，因此也需要对代价函数进行离散化描述，可以得到
$\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)) }$

这里我想详细阐述一些关于LQR特性的内容。根据这个离散代价函数，需要思考几个问题

$J$ 的含义
代价函数 $J$ 代表了从 $0$ 时刻到 $N$ 时刻，这个过程中状态向量以及输入的累积值。
$J$ 的最优化代表什么
从代价函数的形式可以看出，其都是半正定矩阵，即 $\geq 0$ 。因此对 $J$ 求最优化，会使得状态向量向零向量收敛。
$J$ 的最优化结果是什么
用 $J$ 对输入 $\vec{U}$ 进行求导，可以得到从一个输入向量的时间序列 $\{\vec{U_0},\vec{U_1},\vec{U_2}...,\vec{U_N-1} \}$ 。但是这显然不好求，太多自变量了，求偏导都得求到头大，因此需要使用一些数学方法来巧妙地实现最优求解。

递归特性
当 $k = N$ 时
$J_{k=N} = \frac{1}{2} \vec{X}(N)^{T}F\vec{X}(N)$
很明显，最后一个时刻的代价函数值就是末端代价，末端代价即为N时刻代价函数的最优值 $J^{*}_{k=N}$ 。
当 $k = 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))$
当 $k = 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))$
很明显，不同时刻间的代价函数包含了下一时刻的代价函数，体现了非常强烈的递归特性。
根据贝尔曼最优理论，如果 $k$ 时刻的代价函数 $J_{k}$ 是最优的，那么他包含的 $J_{k+1}$ 一定是最优的。
从 $k = N$ 时刻开始，计算 $N$ 时刻的最优代价函数
$J^{*}_{k=N} = J_{k=N} = \frac{1}{2} \vec{X}(N)^{T}F\vec{X}(N)$
当 $k = 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)) }$
又有
$\vec{X}(N)=A\vec{X}(N-1)+B\vec{U}(N-1)$
对 $\vec{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)}$
经过矩阵求导得到
$\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)$
令其求导为0，求极值
$B^{T}\cdot F \cdot (A\vec{X}(N-1)+B\vec{U}(N-1))+R\vec{U}(N-1)=0$
可以得到
$-B^{T}FA \cdot \vec{X}(N-1)=(B^{T}FB+R) \cdot \vec{U}(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)$

这里令 $K = (B^{T}FB+R) ^{-1} B^{T}FA$ ，可以得到全状态反馈的控制律，即 $\vec{U} = -K \vec{X}$
矩阵运算之前在卡尔曼滤波器里讲过，有兴趣可以翻看，不重复赘述。卡尔曼滤波器

为了验证其是否为极小值点，二次求导。
$\frac{\partial^{2} J_{k=N-1}}{\partial \vec{U}^{2}(N-1)} =\ B^{T} FB +R$
显然，由于矩阵 $R$ 正定， $B^{T}FB$ 为半正定矩阵，因此
$\frac{\partial^{2} J_{k=N-1}}{\partial \vec{U}^{2}(N-1)} >0$
因此该极值点为极小值点。
将极值点 $\vec{U}(N-1)=-(B^{T}FB+R) ^{-1} B^{T}FA \cdot \vec{X}(N-1)$ 代入，得到 $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)$ 还是 $J^{*}(N-1)$ 都是以 $\vec{X}^{T}P\vec{X}$ 的形式成立。
因此总结其规律，令
$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)$
则可以得到增益矩阵( $N - 1$ 时刻)
$K(N-1) = (B^{T}P(0)B+R) ^{-1} B^{T}P(0)A$

总结一下
当 $k = N$ 时刻
$J^{*}(N) = \vec{X}^{T}(N) P(0) \vec{X}(N)$
当 $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\ge1$ )
$(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)$

动手实践

系统模型

以弹簧阻尼系统为例，设质量块质量为m，以静力平衡点为位移0点，其受力情况为：
在这里插入图片描述

这里列出受力情况，主要是向一些新入门的同学展示一些状态空间方程是如何来的，已知可跳过。
$ma = - k x - c v$
其中， $a$ 为质量块的加速度， $v$ 为质量块的速度， $x$ 为质量块的位移， $k$ 为弹簧系数， $c$ 为阻尼系数。
接下来，建立状态空间方程，设状态向量为 $\vec{X}$ 为
$\vec{X} =\begin{bmatrix} x_1\\x_2\end{bmatrix} = \begin{bmatrix} x\\v\end{bmatrix}$
则状态转移方程为
$\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}$
为了在matlab中将其运动过程仿真出来，将其转换成离散方程，设采样时间为 $T$ ，则可以得到
$\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}$

仿真代码

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 % 添加网格线

运行结果

在这里插入图片描述

可以看出，这是一个无论初始状态如何，最终都能够振荡收敛的系统。此时在这个系统里加入一个力 $\vec{F}$ ，作为系统输入 $u$ ，根据 $L QR$ 计算输入序列，来使得系统状态更快速收敛为0。

带输入的系统模型

在这里插入图片描述

带输入的系统模型的状态空间方程为
$\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$

LQR控制器设计

定义代价函数 $J$
$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)) }$
其中，矩阵 $P (0), Q, R$ 分别是根据控制需求定义的末端代价权重矩阵，过程状态代价矩阵，过程输入代价矩阵。
计算全状态矩阵 $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)$
其中，
$A=\begin{bmatrix} 1&T\\-\frac{kT}{m}&1-\frac{cT}{m}\end{bmatrix},B=\begin{bmatrix} 0\\\frac{T}{m}\end{bmatrix}$

控制器仿真

clear all;

lqr_flag = 1;

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;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+1

P=zeros(n,4);
%P迭代矩阵，用于计算K
P0 = [1 0;0 1];
%末端状态代价矩阵2x2
Q = [1 0;0 1];
%过程状态代价矩阵2x2
R = 1;
%过程输入代价矩阵1x1
K = zeros(n,2);
%全状态反馈矩阵 1x2
P(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;
    end
else        % 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    
end

plot(time, x,'r-',time,v,'g-',time,u,'b-') % 绘制曲线
xlabel('x') % 添加x轴标签
ylabel('y') % 添加y轴标签
title('lqr') % 添加标题
legend('x','v','u');
grid on % 添加网格线