自动驾驶：LQR、ILQR和DDP原理、公式推导以及代码演示（一、LQR篇）

机械心

已于 2024-09-11 23:59:06 修改

阅读量1k

点赞数 16

分类专栏：最优化理论文章标签：自动驾驶人工智能机器学习

于 2024-09-06 17:29:57 首次发布

本文链接：https://blog.csdn.net/a8598671/article/details/141957635

版权

最优化理论专栏收录该内容

9 篇文章 1 订阅

订阅专栏

（一）LQR原理、公式推导以及代码演示

1. LQR 原理

线性二次调节器 (Linear Quadratic Regulator, LQR) 是一种用于线性系统的最优控制器，它的目标是在控制系统的动态过程中，尽可能减少误差和能耗。LQR 的目标是通过最优控制输入，使系统状态最小化某一代价函数（即性能指标），以实现最佳控制。

2. 主要应用场景

LQR 广泛用于工业控制、自动驾驶、航天器姿态控制、无人机控制等场景，特别是那些需要平衡控制精度和能量消耗的系统。其主要应用包括：

自动控制：如机器人控制、无人机控制等。
交通领域：如自动驾驶汽车的路径跟踪。
航天器：如姿态和轨道控制。
机械臂：用于精确控制机械臂的运动。
稳态控制问题：如在电力系统中用于保持系统稳定性。

3. LQR 问题定义

LQR 是基于线性系统和二次型代价函数的最优控制方法，在实际工程应用中，我们一般采用离散时间系统的LQR设计，其目标是通过设计反馈控制律，使系统在离散时间步长内稳定运行，最小化某个二次型代价函数。

给定一个离散时间系统，其状态和控制输入通过以下方程表示：

$x_{k+1} = A x_k + B u_k$

其中：

$x_k \in \mathbb{R}^n$ 是时刻k的系统状态，
$u_k \in \mathbb{R}^m$ 是时刻k的控制输入，
$A \in \mathbb{R}^{n \times n}$ 是状态转移矩阵，
$B \in \mathbb{R}^{n \times m}$ 是输入矩阵。

系统描述的物理意义：系统状态 $x_k$ 通过矩阵A演变到下一时刻，同时控制输入 $u_k$ 通过矩阵B影响系统的状态。

LQR 的目标是设计控制输入 $u_x$ ，使得以下代价函数 $J$ 最小：

$J = \sum_{k=0}^{N-1} \left( x_k^T Q_k x_k + u_k^T R_k u_k \right) + x_N^T Q_N x_N$

其中：

$Q_k \in \mathbb{R}^{n \times n}$ 是对状态偏离的惩罚矩阵，通常为正半定矩阵 $Q_k\geqslant0$ 。
$R_k \in \mathbb{R}^{m \times m}$ 是对控制输入的惩罚矩阵，通常为正定矩阵 $R_k>0$ 。
$Q_N \in \mathbb{R}^{n \times n}$ 是在末状态 $x_N$ 时对状态的终端惩罚矩阵。

LQR 的目标是找到一个反馈控制策略 $u_k=K_kx_k$ ，使得代价函数 $J$ 最小化。

4. LQR 公式推导

1. 价值函数定义

LQR的关键思想是使用动态规划（Dynamic Programming）来递归求解最优控制律。我们首先定义价值函数 $V_k(x_k)$ ，表示从时刻 $k$ 开始直到终点 $N$ 时的最小代价：

$V_k(x_k) = \min_{\{u_k, \dots, u_{N-1}\}} \sum_{i=k}^{N-1} \left( x_i^T Q_i x_i + u_i^T R_i u_i \right) + x_N^T Q_N x_N$

该价值函数 $V_k(x_k)$ 是从时刻 $k$ 开始的最优代价，给定初始状态 $x_k$ 。

在最终时刻 $N$ ，状态已固定，价值函数可以直接表示为：

$V_N(x_N) = x_N^T Q_N x_N$

2. 贝尔曼方程和递推公式

利用贝尔曼最优性原理，任何时刻 $k$ 的最优控制策略都可以通过递推的方式求得。根据动态规划的思想，时刻 $k$ 的价值函数可以表示为：

$V_k(x_k) = \min_{u_k} \left( x_k^T Q_k x_k + u_k^T R_k u_k + V_{k+1}(x_{k+1}) \right)$

这里 $x_{k+1}$ 是通过系统动态方程确定的，即 $x_{k+1} = A x_k + B u_k$ 。将该表达式代入贝尔曼方程：

$V_k(x_k) = \min_{u_k} \left( x_k^T Q_k x_k + u_k^T R_k u_k + V_{k+1}(A_k x_k + B_k u_k) \right)$

假设 $V_{k+1}(x_{k+1})$ 可以表示为二次型函数：

$V_{k+1}(x_{k+1}) = x_{k+1}^T P_{k+1} x_{k+1} + c_{k+1}$

其中， $P_{k+1}$ 是对称正定矩阵， $c_{k+1}$ 是常数项。将此带入上式，得到：

$V_k(x_k) = \min_{u_k} \left( x_k^T Q_k x_k + u_k^T R_k u_k + (A_k x_k + B_k u_k)^T P_{k+1} (A_k x_k + B_k u_k) + c_{k+1} \right)$

展开后得到：

$V_k(x_k) = \min_{u_k} \left( x_k^T (Q_k + A_k^T P_{k+1} A_k) x_k + u_k^T (R_k + B_k^T P_{k+1} B_k) u_k + 2 x_k^T A_k^T P_{k+1} B_k u_k + c_{k+1} \right)$

3. 最优控制律

为了最小化价值函数，我们需要对控制输入 $u_k$ 求导，并将其设为零：

$\frac{\partial V_k}{\partial u_k} = 2 (R_k + B_k^T P_{k+1} B_k) u_k + 2 B_k^T P_{k+1} A_k x_k = 0$

求解最优控制输入 $u_k^*$ ：

$u_k^* = -(R_k + B_k^T P_{k+1} B_k)^{-1} B_k^T P_{k+1} A_k x_k$

我们可以引入反馈增益矩阵 $K_k$ 表示为：

$K_k = -(R_k + B_k^T P_{k+1} B_k)^{-1} B_k^T P_{k+1} A_k$

于是最优控制律为：

$u_k^* = K_k x_k$

这个反馈控制律 $u_k^* = K_k x_k$ 表明最优的控制输入是当前状态 $x_k$ 的线性反馈。

5. 价值函数的更新

将最优控制输入 $u_k^*$ 代入到价值函数表达式中，更新时刻 $k$ 的价值函数。首先代入 $u_k^*$ ：

$V_k(x_k) = x_k^T (Q_k + A_k^T P_{k+1} A_k - A_k^T P_{k+1} B_k (R_k + B_k^T P_{k+1} B_k)^{-1} B_k^T P_{k+1} A_k) x_k + c_{k+1}$

令矩阵 $P_k$ 为：

$P_k = Q_k + A_k^T P_{k+1} A_k - A_k^T P_{k+1} B_k (R_k + B_k^T P_{k+1} B_k)^{-1} B_k^T P_{k+1} A_k$

这是一个关于矩阵 $P_k$ 的递推公式，它从末时刻 $N$ 开始反向递推。最终时刻的条件为：

$P_N = Q_N$

至此，我们通过递推公式求解每个时刻的 $P_k$ 和反馈增益矩阵 $K_k$ ，从而获得最优的线性反馈控制策略。

6. 动态规划递推算法

基于以上推导，LQR的求解过程可以总结为以下步骤：

1. 初始化终端条件：设定最终状态的权重矩阵 $P_N = Q_N$ 。

2. 反向递推更新矩阵：从 $k = N-1$ 开始，递推计算每个时刻的 $P_k$ ：

$P_k = Q_k + A_k^T P_{k+1} A_k - A_k^T P_{k+1} B_k (R_k + B_k^T P_{k+1} B_k)^{-1} B_k^T P_{k+1} A_k$

3. 计算反馈增益矩阵：对于每个时刻 $k$ ，计算反馈增益矩阵：

$K_k = -(R_k + B_k^T P_{k+1} B_k)^{-1} B_k^T P_{k+1} A_k$

4. 应用最优控制律：在每个时刻，使用反馈控制律：

$u_k^* = K_k x_k$

5. 注意事项

可控性：LQR要求系统是可控的，即系统的状态能够通过适当的控制输入进行调节。
权重矩阵的选取：状态权重矩阵 $Q_k$ 和控制权重矩阵 $R_k$ 的选取直接影响控制性能。较大的 $Q_k$ 表示对状态偏离目标的惩罚较重，而较大的 $R_k$ 表示对控制输入的惩罚较重。它们的选取往往需要根据实际应用进行调节。
正定性：矩阵 $R_k$ 必须是正定的，否则控制输入的代价函数可能会导致不可解。
系统模型准确性：LQR假设系统是线性的或可以线性化，因此对于强非线性系统，需要先进行线性化处理，或者使用非线性控制方法，如iLQR或DDP。

6. 代码示例

LQR一般用于线性系统，这里为了和后续iLQR以及DDP的方式进行对比，我们用一个简单的非线性系统，其状态 $x_k$ 和控制输入 $u_k$ 满足以下非线性动力学方程：

$x_{k+1} = x_k + \sin(u_k)$

这里的非线性在于控制输入的影响是通过正弦函数进入系统的。我们希望通过控制输入 $u_k$ 使状态 $x_k$ 快速收敛到目标值 0，同时控制量尽可能小，因此二次成本函数：

$J = \sum_{k=0}^{N-1} \left( \frac{1}{2} x_k^2 + \frac{1}{2} u_k^2 \right)$

我们对控制输入 $u_k$ 进行线性化处理，通常在 $u_k=0$ 附近线性化。我们使用泰勒展开对 $sin(u_k)$ 进行线性化：

$sin(u_k)\approx u_k$ (仅当 $u_k$ 较小时)

因此，线性化后的系统动态方程变为：

$x_{k+1} = x_k + u_k$

python代码如下：

import numpy as np


# System dynamics (linearized)
def system_dynamics_lqr(x, u):
    return x + u  # Linearized system: x_{k+1} = x_k + u_k

# Cost function for a single step
def cost_function_lqr(x, u):
    return 0.5 * (x**2 + u**2)

# Backward pass: solve Riccati equation to find P matrices and feedback gains K
def solve_lqr(A, B, Q, R, N):
    P = np.zeros(N+1)
    K = np.zeros(N)

    # Terminal cost (at time N)
    P[N] = Q  # At final time step, P_N = Q

    # Backward pass to compute P matrices and feedback gains K
    for k in range(N-1, -1, -1):
        P[k] = Q + A**2 * P[k+1] - A * P[k+1] * B * (R + B**2 * P[k+1])**(-1) * B * P[k+1] * A
        K[k] = (R + B**2 * P[k+1])**(-1) * B * P[k+1] * A  # Feedback gain

    return P, K

# Forward pass: apply feedback gains K to compute the optimal trajectory
def apply_lqr_control(x0, K, N):
    x = np.zeros(N+1)
    u = np.zeros(N)
    x[0] = x0

    for k in range(N):
        u[k] = -K[k] * x[k]
        x[k+1] = system_dynamics_lqr(x[k], u[k])

    return x, u

if __name__ == "__main__":
    
    # LQR parameters
    N = 3  # Number of time steps
    x0 = 1  # Initial state
    Q = 1  # State cost
    R = 1  # Control cost

    # Matrices for the linearized system
    A = 1  # State transition matrix
    B = 1  # Control matrix

    # Initialize control sequence and state trajectory
    u = np.zeros(N)  # Initial control sequence
    x = np.zeros(N+1)  # State trajectory
    x[0] = x0
    
    # Solve for P and K matrices using LQR
    P, K = solve_lqr(A, B, Q, R, N)

    # Apply LQR control to get the optimal trajectory
    x_final, u_final = apply_lqr_control(x0, K, N)

    # Output the final results
    print(x_final, u_final)

输出结果如下：