LQR求解推导及代码实现

最新推荐文章于 2024-04-28 20:34:10 发布

艰默

最新推荐文章于 2024-04-28 20:34:10 发布

阅读量805

点赞数 24

分类专栏：自动驾驶-控制文章标签：算法线性代数机器学习 lqr python

本文链接：https://blog.csdn.net/jianmo1993/article/details/137045920

版权

自动驾驶-控制专栏收录该内容

12 篇文章 2 订阅

订阅专栏

文章目录

引言

考虑一个离散线性二次型系统的状态空间方程为
$x_{k+1}=Ax_k+Bu_k \tag{1}$
其中， $u_k$ 为 $\times 1$ 的控制向量， $x_k$ 为 $\times 1$ 的状态向量，状态矩阵 $A\in R^{m\times m}$ ，输入矩阵 $\in R^{m\times n}$ ，

离散LQR（Linear Quadratic Regulator）是针对离散时间线性系统的最优控制方法。它是连续时间LQR的离散化版本，用于解决在离散时间步长上系统的最优控制问题。离散LQR的目标是找到一个控制策略，使得从初始状态到最终状态的整个过程中的性能指标（代价函数）最小化。

离散LQR的性能指标通常定义为：

$\sum_{k=0}^{N-1} (x_k^T Q x_k + u_k^T R u_k) + x_N^T F x_N \tag{2}$
其中：

$x_k$ 是在离散时间步 $k$ 的系统状态。
$u_k$ 是在时间步 $k$ 的控制输入。
$Q$ 是状态权重 $\times m$ 的半正定方阵，用于衡量状态的代价，通常将其设计为对角矩阵。
$R$ 是控制权重 $\times n$ 的正定对称矩阵，用于衡量控制输入的代价，通常将其设计为对角矩阵。
$F$ 是末端状态权重 $\times m$ 的半正定方阵，用于衡量最终状态的代价，通常将其设计为对角矩阵。
$N$ 是控制的总时间步数。

LQR求解

归纳通用迭代方程

根据（2）式，我们可以采用逆向分级的思想对其进行分析求解：

当 $k = N$ 时，系统从 $k = N$ 到 $k = N$ 的代价为
$J_{N \to N}=x_N^TFx_N \tag{3}$
这里当 $k = N$ 时候，系统已经达到最终的 $x_N$ 的状态，因此 $J_{N \to N}$ 只存在末端代价，此时对应的代价就是最优代价，即
$J_{N \to N}^* = x_N^TFx_N = x_N^TP_Nx_N\tag{4}$
上式中为了后面方便归纳，我们设 $P_N=F$ 。
当 $k = N - 1$ 时，系统从 $k = N - 1$ 到 $k = N$ 的代价为
$\begin{align*} J_{N-1 \to N}&=x_N^T F x_N + x_{N-1}^T Q x_{N-1} + u_{N-1}^T R u_{N-1}\\ &=J_{N \to N} + x_{N-1}^T Q x_{N-1} + u_{N-1}^T R u_{N-1} \end{align*} \tag{5}$

根据贝尔曼优化理论，结合（4）式可得 $k = N - 1$ 时最优代价：
$J_{N-1 \to N}^* (x_{N-1}, x_{N}, u_{N-1})= \mathop{\min}\limits_{x_N} (J_{N \to N}^*) + \mathop{\min}\limits_{u_{N-1}}(x_{N-1}^T Q x_{N-1} + u_{N-1}^T R u_{N-1}) \tag{6}$
根据（1）式可得 $x_N=Ax_{N-1}+Bu_{n-1}$ ，将其和 $P_N=F$ 代入可得
$\begin{align*} J^*_{N-1 \to N}&=\mathop{\min}\limits_{u_{N-1}}((Ax_{N-1}+Bu_{n-1})^T P_N (Ax_{N-1}+Bu_{n-1})) + \mathop{\min}\limits_{u_{N-1}}(x_{N-1}^T Q x_{N-1} + u_{N-1}^T R u_{N-1})\\ &=\mathop{\min}\limits_{u_{N-1}}((Ax_{N-1}+Bu_{n-1})^T P_N (Ax_{N-1}+Bu_{n-1}) + (x_{N-1}^T Q x_{N-1} + u_{N-1}^T R u_{N-1}))\\ &=\mathop{\min}\limits_{u_{N-1}}(J_{N-1 \to N}) \end{align*} \tag{7}$
对其在输入 $u_{N-1}$ 上求导得
$\begin{align*} \frac{\partial J_{N-1 \to N}}{\partial u_{N-1}}&=\frac{\partial ((Ax_{N-1}+Bu_{n-1})^T P_N (Ax_{N-1}+Bu_{N-1}) )}{\partial u_{N-1}} + \frac{\partial (x_{N-1}^T Q x_{N-1} + u_{N-1}^T R u_{N-1}) }{\partial u_{N-1}}\\ &=\frac{\partial (Ax_{N-1}+Bu_{n-1}) }{\partial u_{N-1}} \frac{\partial (Ax_{N-1}+Bu_{n-1})^T P_N (Ax_{N-1}+Bu_{N-1}) }{\partial (Ax_{N-1}+Bu_{n-1})} + \frac{\partial (u_{N-1}^T R u_{N-1}) }{\partial u_{N-1}}\\ &=B^TP_N(Ax_{N-1}+Bu_{N-1})+Ru_{N-1} \end{align*} \tag{8}$
对其在输入 $u_{N-1}$ 上二次求导得
$\begin{align*} \frac{\partial^2 J_{N-1 \to N}}{\partial u_{N-1}}&=\frac{\partial B^TP_N(Ax_{N-1}+Bu_{N-1})+Ru_{N-1}}{\partial u_{N-1}} \\ &=B^TP_NB+R \end{align*} \tag{9}$
由于 $P_N=F$ 为半定矩阵，因此 $B^TP_NB$ 也是半正定的，同时 $R$ 是正定的，因此（9）式的结果也是正定矩阵，因此令（8）式等于0：
$\begin{align*} \frac{\partial J_{{N-1} \to N}}{\partial u_{N-1}} &= 0\\ B^TP_N(Ax_{N-1}+Bu_{N-1})+Ru_{N-1}&=0\\ B^TP_NBu_{N-1} + B^TP_NAx_{N-1}+ Ru_{N-1} &= 0\\ (B^TP_NB + R)u_{N-1} + B^TP_NAx_{N-1}&= 0\\ u_{N-1} + (B^TP_NB + R)^{-1}B^TP_NAx_{N-1}&= 0 \end{align*} \tag{10}$

求出来的 $u_{N-1}$ 为 $J_{N-1 \to N}$ 的最小值。即最优输入
$u^*_{N-1} = -(B^TP_NB + R)^{-1}B^TP_NAx_{N-1} \tag{11}$

定义反馈矩阵 $K_{N-1}=(B^TP_NB + R)^{-1}B^TP_NA$ ，上式变为
$u^*_{N-1} =-K_{N-1}x_{N-1} \tag{12}$
结合（7）式可得系统从 $k = N - 1$ 到 $k = N$ 的最优代价为
$\begin{align*} J^*_{N-1 \to N}&=(Ax_{N-1}-BK_{N-1}x_{N-1})^T P_N (Ax_{N-1}-BK_{N-1}x_{N-1}) + x_{N-1}^T Q x_{N-1} + (K_{N-1}x_{N-1})^T R (K_{N-1}x_{N-1})\\ &=((A-BK_{N-1})x_{N-1})^T P_N ((A-BK_{N-1})x_{N-1}) + x_{N-1}^T Q x_{N-1} + x_{N-1}^TK_{N-1}^T R K_{N-1}x_{N-1}\\ &=x_{N-1}^T(A-BK_{N-1})^T P_N (A-BK_{N-1})x_{N-1} + x_{N-1}^T Q x_{N-1} + x_{N-1}^TK_{N-1}^T R K_{N-1}x_{N-1}\\ &=x_{N-1}^T [(A-BK_{N-1})^T P_N (A-BK_{N-1}) + Q + K_{N-1}^T R K_{N-1}] x_{N-1} \end{align*} \tag{13}$

定义 $P_{N-1}=(A-BK_{N-1})^T P_N (A-BK_{N-1}) + Q + K_{N-1}^T R K_{N-1}$ ，则上式可记为
$\begin{align*} J^*_{N-1 \to N}=x_{N-1}^T P_{N-1}x_{N-1} \end{align*} \tag{14}$
当 $k = N - 2$ 时，同理可得系统从 $k = N - 2$ 到 $k = N - 1$ 的代价为
$\begin{align*} J_{N-2 \to N-1}&=x_N^T F x_N + x_{N-1}^T Q x_{N-1} + u_{N-1}^T R u_{N-1}+ x_{N-2}^T Q x_{N-2} + u_{N-2}^T R u_{N-2}\\ &=J_{N-1 \to N}+ x_{N-2}^T Q x_{N-2} + u_{N-2}^T R u_{N-2}\\ \end{align*} \tag{15}$
可得 $k = N - 2$ 时最优代价：
$\begin{align*} J_{N-2 \to N-1}^* &= \mathop{\min}\limits_{x_{N-1}} (J_{N-1 \to N}^*) + \mathop{\min}\limits_{u_{N-2}}(x_{N-2}^T Q x_{N-2} + u_{N-2}^T R u_{N-2})\\ &= \mathop{\min}\limits_{u_(N-2)} (J_{N-1 \to N}^*) + \mathop{\min}\limits_{u_{N-2}}(x_{N-2}^T Q x_{N-2} + u_{N-2}^T R u_{N-2})\\ &= \mathop{\min}\limits_{u_(N-2)} (x_{N-1}^T P_{N-1}x_{N-1} ) + \mathop{\min}\limits_{u_{N-2}}(x_{N-2}^T Q x_{N-2} + u_{N-2}^T R u_{N-2})\\ &= \mathop{\min}\limits_{u_(N-2)} ((Ax_{N-2}+Bu_{N-2})^T P_{N-1}(Ax_{N-2}+Bu_{N-2})) + \mathop{\min}\limits_{u_{N-2}}(x_{N-2}^T Q x_{N-2} + u_{N-2}^T R u_{N-2}) \\ &= \mathop{\min}\limits_{u_(N-2)} ((Ax_{N-2}+Bu_{N-2})^T P_{N-1}(Ax_{N-2}+Bu_{N-2}) + x_{N-2}^T Q x_{N-2} + u_{N-2}^T R u_{N-2}) \end{align*} \tag{16}$

对上式括号部分对 $u_{N-2}$ 求导并令其等于0，可得到
$\begin{align*} \frac{\partial ((Ax_{N-2}+Bu_{N-2})^T P_{N-1}(Ax_{N-2}+Bu_{N-2}) + x_{N-2}^T Q x_{N-2} + u_{N-2}^T R u_{N-2})}{\partial u_{N-2}} &= 0\\ B^TP_{N-1}(Ax_{N-2}+Bu_{N-2})+Ru_{N-2}&=0\\ B^TP_{N-1}Bu_{N-2} + B^TP_{N-1}Ax_{N-2}+ Ru_{N-2} &= 0\\ (B^TP_{N-1}B + R)u_{N-2} + B^TP_{N-1}Ax_{N-2}&= 0\\ u_{N-2} + (B^TP_{N-1}B + R)^{-1}B^TP_{N-1}Ax_{N-2}&= 0 \end{align*} \tag{17}$
令 $K_{N-2}= (B^TP_{N-1}B + R)^{-1}B^TP_{N-1}A$ ，可得
$u^*_{N-2} =-K_{N-2}x_{N-2} \tag{18}$
结合（16）式整理可得系统从 $k = N - 2$ 到 $k = N - 1$ 的最优代价为
$\begin{align*} J^*_{N-2 \to N-1} &=x_{N-2}^T P_{N-2} x_{N-2} \end{align*} \tag{19}$

其中 $P_{N-2}=(A-BK_{N-2})^T P_{N-1} (A-BK_{N-2}) + Q + K_{N-2}^T R K_{N-2}$ 。
当 $k = N - 3$ 时，。。。。。。

同理我们可以继续往下计算得到 $u_{N-1},u_{N-2},\dots, u_{0}$ ，归纳整理可得到通用的迭代方程：

$k = N$ 时，
$\begin{align*} J_{N \to N}^* &= x_{N}^T P_{N} x_{N} \\ P_N&=F \end{align*} \tag{20}$

$k=N-1,N-2,\dots,0$ 时，
$\begin{align*} u^*_{k}&=-K_{k}x_{k} \\ J_{k \to k+1}^* &= x_{k}^T P_{k} x_{k} \\ K_{k}&=(B^TP_{k+1}B + R)^{-1}B^TP_{k+1}A\\ P_{k}&=(A-BK_{k})^T P_{k+1} (A-BK_{k}) + Q + K_{k}^T R K_{k} \end{align*} \tag{21}$

迭代求解过程

如果（1）式的线性时不变系统完全可控且稳定，则当最优控制末端的时间 $N$ 无穷大（即反向迭代次数足够大），则反馈矩阵 $K_k$ 则将趋于常数矩阵 $K$ ，即
$\lim\limits_{N\to\infty}K_k = K$

出现这种情况的原因可以解释为：

先从 $N$ 开始反向迭代，求出系统每一次的最优反馈 $K_k$ 和最优输入 $u^*_k$ ，从（21）式可以看出，这几个值只跟系统的问题定义（即系统状态方程和Q 、R）有关，与初始状态无关。因此，对于任意一个不同的初始状态，均存在一个通用的最优控制反馈 $K$ 。

在从 $N$ 开始反向迭代过程中，我们实际上是在计算这个代数Riccati方程的一个解序列，这个解序列对应于从最终时刻N向初始时刻递推的过程。这个过程可以理解为是在逐步构建最优反馈控制律，使得系统从任意初始状态出发，能够在有限时间内以最优的方式达到性能指标的最小化。由于LQR的设计是基于最优控制理论的，这个最优控制律具有一些很好的性质。其中之一就是，当系统接近稳态时，最优控制律应该保持相对稳定，不再随时间发生大的变化。这是因为，在稳态附近，系统的动态行为已经相对稳定，不再需要大的控制输入来调整系统状态。

总结来说，对于一个有限时域N的LQR系统来说，矩阵在从 $N$ 开始反向迭代的过程中快速收敛到常值的原因是由于系统逐渐逼近最优解的过程中，系统状态和控制输入进入稳态区域，导致Riccati方程中的各项逐渐趋于常数。这个收敛过程反映了LQR设计的有效性和最优控制理论的稳定性。

因此，我们可以通过不停的迭代求解 $K_k$ ，直到该反馈矩阵收敛于一个常数矩阵 $K$ 为止，将该常数矩阵 $K$ 作为系统最优的反馈矩阵。

综上，采用LQR算法进行控制率求解的步骤概括为：

确定迭代范围 $N$ 。
设置迭代初始值 $P_{N}=F$ ，其中 $Q_f=Q$
循环迭代，从后往前 $\ldots, 0$ ，
$\begin{align*} K_{k}&=(B^TP_{k+1}B + R)^{-1}B^TP_{k+1}A\\ P_{k}&=(A-BK_{k})^T P_{k+1} (A-BK_{k}) + Q + K_{k}^T R K_{k} \end{align*}$

判断 $K_k$ 和 $K_{k+1}$ 每个对应元素的差值是否小于 $\epsilon$ （这里 $\epsilon$ 代表迭代精度，一般是非常小的数字），如果都小于则跳出循环，此时的 $K_t$ 即为最终的最优反馈矩阵，否则继续循环。
最终得优化的控制量 $u_{t}^{*}=-K_{t} x_{t}$

代码实现

python代码的实现如下

import numpy as np
from scipy.linalg import inv

N = 200     # 迭代范围
EPS = 1e-4  # 迭代精度

def cal_lqr_k(A, B, Q, R, F):
    """计算LQR反馈矩阵K
    Args:
        A : mxm状态矩阵A
        B : mxn状态矩阵B
        Q : Q是状态权重mxm的半正定方阵，用于衡量状态的代价，通常将其设计为对角矩阵。
        R : R是控制权重nxn的正定对称矩阵，用于衡量控制输入的代价，通常将其设计为对角矩阵。
        F : F是末端状态权重mxm的半正定方阵，用于衡量最终状态的代价，通常将其设计为对角矩阵。
    Returns:
        K : 反馈矩阵K
    """
    # 设置迭代初始值
    P = F
    # 循环迭代
    for t in range(N):
        K_t = inv(B.T @ P @ B + R) @ B.T @ P @ A
        P_t = (A - B @ K_t).T @ P @ (A - B @ K_t) + Q + K_t.T @ R @ K_t
        if(abs(P_t- P).max()<EPS):
            break
        P = P_t
    return K_t


def main():
    A = np.array([[0, 1], [-1, -0.5]])
    B = np.array([[0], [1]])
    Q = np.array([[1, 0], [0, 1]])
    F = np.array([[1, 0], [0, 1]])
    R = np.array([[1]])
    K = cal_lqr_k(A, B, Q, R, F)
    print(K)

if __name__ == '__main__':
    main()

艰默

关注

24
点赞
踩
19

收藏

觉得还不错? 一键收藏
打赏
4
评论
LQR求解推导及代码实现

考虑一个离散线性二次型系统的状态空间方程为xk+1=Axk+Buk(1)x_{k+1}=Ax_k+Bu_k \tag{1}xk+1=Axk+Buk(1)其中，uku_kuk为n×1n \times 1n×1的控制向量，xkx_kxk为m×1m \times 1m×1的状态向量，状态矩阵A∈Rm×mA\in R^{m\times m}A∈Rm×m ，输入矩阵B∈Rm×nB \in R^{m\times n}B∈Rm×n，离散LQR（Linear Quadratic Regulator）是针对
复制链接

扫一扫