LQR控制算法推导-连续与离散形式

余加木

已于 2024-02-19 16:58:44 修改

阅读量2.1k

点赞数 17

分类专栏：控制理论文章标签：最小二乘法线性代数

于 2024-02-19 16:53:34 首次发布

本文链接：https://blog.csdn.net/zjh2883/article/details/136167154

版权

控制理论专栏收录该内容

1 篇文章 0 订阅

订阅专栏

1 前言

线性二次调节（Linear Quadratic Regulator，LQR）是一种经典的控制理论方法，用于设计控制器，使得线性系统在给定的性能指标下表现最优。

LQR的原理基于最小二乘优化问题，它的目标是设计一个状态反馈控制器，以最小化系统的性能指标。

2 连续时间系统

2.1 系统描述

考虑线性时不变系统，用状态空间形式表示：

$\left\{\begin{array}{llllllllll}\dot{x}=Ax+Bu\\ y=Cx+Du\end{array}\right. \tag{1}$
其中 $x$ 是系统的状态向量， $u$ 是系统的控制输入， $y$ 是系统的控制输出， $A$ 是系统状态矩阵，表示状态量之间的变化关系， $B$ 是输入矩阵，表示控制输入与状态变量的关系， $C$ 是输出矩阵，表示状态变量与输出之间的关系， $D$ 是传递矩阵，表示控制输入与输出之间的关系。

2.2 状态反馈控制器

LQR的目标是找到一个状态反馈控制器，根据状态反馈量 $x$ 获得控制输入 $u$ ，即

$\tag{2}$

其中 $K$ 是控制器增益矩阵。

将式(2)代入式(1)的状态方程，可以得到：
$\dot{x}=Ax-BKx=(A-BK)x=A_c x \tag{3}$

因此通过选择合适的控制器增益矩阵 $K$ ，使式(3)的闭环系统矩阵 $A_c$ 的特征值的实部均为负数，从而实现系统稳定控制。

2.3 代价函数

为了设计最优的控制器增益矩阵 $K$ ，LQR定义代价函数：
$J=\int_{0}^{\infty}x^{T}Q x+u^{T}{R u}\,\mathrm{d}t \tag{4}$
其中 $Q$ 是状态向量 $x$ 的半正定加权矩阵， $R$ 是控制输入 $u$ 的正定加权矩阵。加权矩阵中某一元素数值越大，则其对应状态量或输入量越重视。

目标是找到一个最优的控制器增益矩阵 $K^*$ ，使代价函数 $J$ 达到最小值。

2.4 计算推导

将式(2)代入式(4)，可以表达为：
$J=\int_{0}^{\infty}x^{T}(Q+K^TRK)x \mathrm{d}t \tag{5}$

定义一个常量对称矩阵 $P$ ， $P=P^T > 0$ ，假定其满足：
$\frac{d}{dt} (x^{T}Px) = -x^{T}(Q+K^TRK)x \tag{6}$

把式(6)展开化简，可以表示为：
$\dot{x}^T P x + x^T P \dot{x} + x^T Q x + x^TK^TRKx = 0 \tag{7}$

将式(3)代入式(7)，有：
${x}^T A_c^T P x + x^T P A_c{x} + x^T Q x + x^TK^TRKx = 0 \tag{8}$

整理后可得：
${x}^T ( A_c^T P + P A_c + Q + K^TRK )x = 0 \tag{9}$

针对式(9)所示的二次型问题，若式(9)有解，则要求括号内的部分等于0，即：
$A_c^T P + P A_c + Q + K^TRK = 0 \tag{10}$

将 $A_c = A-BK$ 代入上式，得到：

$A^T P + PA+ Q+ K^TRK -PBK - K^TB^TP = 0 \tag{11}$

令 $K=R^{-1}B^TP$ ，则上式可以化简为：
$A^T P + PA+ Q+ K^TR(R^{-1}B^TP)-PB(R^{-1}B^TP) - K^TB^TP \\ =A^T P + PA+ Q+ K^TB^TP-PBR^{-1}B^TP - K^TB^TP \\ = A^T P + PA+ Q -PBR^{-1}B^TP = 0 \tag{12}$

式(12)就是连续时间代数Riccati方程，在 $A$ ， $B$ ， $Q$ ， $R$ 已知的情况下，可以求解出矩阵 $P$ 。

2.5 LQR算法

建立系统状态空间模型，确定 $A$ ， $B$ ， $C$ ， $D$ ；
选择加权矩阵 $Q$ ， $R$ ；
根据式(12)求解Riccati方程，得到矩阵 $P$ ；
根据矩阵 $P$ 计算增益矩阵 $K=R^{-1}B^TP$ ；
计算控制输入 $u = - K x$ 。

3 离散时间系统

3.1 系统描述

考虑线性时不变离散系统：
$\left\{\begin{array}{llllllllll} x_{t+1}=Ax_t+Bu_t\\ y_t=Cx_t+Du_t\end{array}\right. \tag{13}$

3.2 状态反馈控制器

$u_t=-K_t x_t \tag{14}$

3.3 代价函数

离散LQR目标函数：
$J=\sum_{t=0}^{N-1}\left(x_t^{{\mathrm{T}}}\mathbf{Q}x_t+u_t^{{\mathrm{T}}}\mathbf{R}u_t \right) + x_N^T Q_f x_N \tag{15}$
其中 $Q$ 是给定的状态加权矩阵， $Q_f$ 是最终状态加权矩阵，R是输入加权矩阵，满足 $Q=Q^T \ge 0$ ， $Q_f=Q_f^T \ge 0$ ， $R=R^T > 0$ 。 $N$ 是时间范围。

3.4 计算推导

离散系统状态可以表示为：
$x_1 = Ax_0 + Bu_0 \\ x_2 = Ax_1 + Bu_1 \\ \vdots \tag{16}\\ x_n = Ax_{N-1} + Bu_{N-1}$
将上式逐个代入，得到：
$x_1 = Ax_0 + Bu_0 \tag{17}\\ x_2 = A^2 x_0 + ABu_0 + Bu_1 \\ \vdots \\ x_n = A^N x_0 + A^{N-1}Bu_0 + A^{N-2}Bu_1 + \cdots + Bu_{N-1}$
整理化简：
${\left[\begin{array}{l}{x_{0}}\\ {x_{1}}\\ {\vdots}\\ {x_{N}}\end{array}\right]}={\left[\begin{array}{l l l l l}{0}&{\cdots}&{}&{}\\ {B}&{0}&{\cdots}&{}&{}\\ {A B}&{B}&{0}&{\cdots}\\ {\vdots}&{\vdots}&{}\\ {A^{N-1}B}&{A^{N-2}B}&{\cdots}&{B}\end{array}\right]}{\left[\begin{array}{l}{u_{0}}\\ {u_{1}}\\ {\vdots}\\ {u_{N-1}}\end{array}\right]}+{\left[\begin{array}{l}{I}\\ {A}\\ {\vdots}\\ {A^{N}}\end{array}\right] x_{0}} \tag{18}$

定义
${\left[\begin{array}{l l l l l}{0}&{\cdots}&{}&{}\\ {B}&{0}&{\cdots}&{}&{}\\ {A B}&{B}&{0}&{\cdots}\\ {\vdots}&{\vdots}&{}\\ {A^{N-1}B}&{A^{N-2}B}&{\cdots}&{B}\end{array}\right]}$
$\left[\begin{array}{l}{I}\\ {A}\\ {\vdots}\\ {A^{N}}\end{array}\right]$

则式(18)可以简化为：
$Hx_0 \tag{19}$

3.4.1 动态规划法

解决多阶段决策过程最优化问题。

3.4.1.1 值函数

定义值函数 $V_t:R^n\to R$ ，其中 $t=\left(0,\ldots,N\right)$ ：
$V_t(x_t)=\operatorname*{min}_{u_{t,...,u_{N-1}}}\Bigl(\sum_{\tau=t}^{N-1}(x_{\tau}^TQx_{\tau}+u_{\tau}^TRu_{\tau})+x_N^TQ_fx_N\Bigr) \tag{20}$
上式表示在t时刻，从状态 $x_t$ 开始的LQR最小代价值。

令 $z=x_t$ ， $V_t$ 可以表示为二次型的形式 $V_t(z) = z^T P_t z$ 。

当 $t = N$ 时， $P_N = Q_f$ ，代价函数为：
$V_N(z) = z^T Q_f z \tag{21}$
令 $w=u_t$ ，根据动态规划原理，式(8)可以写成如下递归关系式：
$V_t(z)=\operatorname*{min}_{w} (z^TQz + w^TRw + V_{t+1}(Az+Bw)) \tag{22}$
其中 $z^TQz + w^TRw$ 是 $t$ 时刻产生的代价， $V_{t+1}(Az+Bw)$ 是从 $t + 1$ 时刻开始产生的最小代价。

提取式(22)中与 $w$ 无关的项，得到：
$V_t(z)=z^TQz + \operatorname*{min}_{w} (w^TRw + V_{t+1}(Az+Bw)) \tag{23}$

上式建立了 $V_t(z)$ 与 $V_{t+1}(z)$ 之间的递归关系。因此最有控制率 $u_t$ 可以表示为：
$u_t =arg \operatorname*{min}_{w} (w^TRw + V_{t+1}(Az+Bw)) \tag{24}$

3.4.1.2 求极值

根据上述假设 $V_t(z) = z^T P_t z$ ，则式(24)可以进一步转化为：
$u_t =arg \operatorname*{min}_{w} (w^TRw + (Az+Bw)^T P_{t+1} (Az+Bw)) \tag{25}$
对式(25)关于 $w$ 求导，导数为零的点则是极值点：
$2w^TR + 2(Az+Bw)^T P_{t+1} B = 0 \tag{26}$
进一步推导：
$2w^TR + 2(Az+Bw)^T P_{t+1} B = 0\\ w^TR +z^T A^T P_{t+1} B +w^T B^T P_{t+1} B = 0 \\ w^T(R+B^T P_{t+1} B) = -z^T A^T P_{t+1} B \\ (R+B^T P_{t+1} B)^T w = - B^T P_{t+1}^T Az \\ (R+B^T P_{t+1} B) w = - B^T P_{t+1} Az \\ w = -(R+B^T P_{t+1} B) ^{-1}B^T P_{t+1} Az \tag{27}$

因此，最优控制输入为：
$w^* = -(R+B^T P_{t+1} B) ^{-1}B^T P_{t+1} Az \tag{28}$

代入式(23)，有：
$V_t(z)=z^TQz +w^{*T}Rw^* + (Az+Bw^*)^T P_{t+1}(Az+Bw^*) \tag{29}$

进一步化简：
$\begin{array}{l l l} {{V_{t}(z)}} & {{=}} & {{z^{T}Q z+w^{\ast T}R w^{\ast}+(A z+B w^{\ast})^{T}P_{t+1}(A z+B w^{\ast})}} \\ {{}} & {{=}} & {{z^{T}Q z+w^{\ast T}R w^{\ast}+z^{T}A^{T}P_{t+1}A z+2z^{T}A^{T}P_{t+1}B w^{\ast}+w^{\ast T}B^{T}P_{t+1}B w^{\ast}}} \\ {{}} & {{=}} & {{ z^TQz + z^TA^TP_{t+1}Az + w^{*T}(R+B^TP_{t+1}B)w^* + 2z^TA^TP_{t+1}Bw^* }} \\ {{}} & {{=}} & {{ z^T(Q + A^TP_{t+1}A-A^TP_{t+1}B(R+B^TP_{t+1}B)^{-1}B^TP_{t+1}A)z}} \\ \end{array} \tag{30}$

则可以得到：
$P_t= Q + A^TP_{t+1}A-A^TP_{t+1}B(R+B^TP_{t+1}B)^{-1}B^TP_{t+1}A \tag{31}$
$P_t= Q + A^TP_{t+1}A+A^TP_{t+1}BK_t \tag{32}$

3.5 LQR算法

建立系统状态空间模型，确定 $A$ ， $B$ ， $C$ ， $D$ ；
选择加权矩阵 $Q$ ， $Q_f$ ， $R$ ；
确定迭代范围 $N$ ；
迭代初始值 $P_N=Q_f$ ；
循环迭代， $t = N, ..., 1$ ，根据式(32)计算 $P_t$ ；
根据矩阵 $P_t$ 计算增益矩阵 $K_t=-(R+B^T P_{t+1} B) ^{-1}B^T P_{t+1} A$ ，时间 $t = 0, ..., N - 1$ ；
计算控制输入 $u_t=-K_tx_t$ 。

参考文献

基础算法 - LQR - 离散时间有限边界

【控制理论】离散及连续的LQR控制算法原理推导

控制理论基础(LQR)

余加木

关注

17
点赞
踩
36

收藏

觉得还不错? 一键收藏
打赏
1
评论
LQR控制算法推导-连续与离散形式

线性二次调节（Linear Quadratic Regulator，LQR）是一种经典的控制理论方法，用于设计控制器，使得线性系统在给定的性能指标下表现最优。LQR的原理基于最小二乘优化问题，它的目标是设计一个状态反馈控制器，以最小化系统的性能指标。
复制链接

扫一扫