线性二次调节器（LQR）：理论解析与车道保持辅助系统（LKA）应用案例

CCC：CarCrazeCurator

已于 2024-08-12 15:50:37 修改

阅读量824

点赞数 27

文章标签：算法

于 2024-08-12 15:46:26 首次发布

本文链接：https://blog.csdn.net/liumao99/article/details/141124814

版权

LQR概述

线性二次调节器（Linear Quadratic Regulator，LQR）是一种用于线性系统的优化控制策略，旨在通过选择合适的控制律来最小化系统的性能指标，该指标通常是一个二次型函数，反映了状态偏差和控制输入的加权平方。LQR控制不仅适用于稳定系统，还可以用来稳定不稳定系统，并且可以处理多变量控制问题。

LQR基本原理

1、线性系统模型

线性系统模型是LQR方法的出发点。它假设系统的状态和控制输入之间的关系可以用线性微分方程来描述。这意味着系统的行为可以用一个线性映射来表达，其中状态向量 $x(t)$ 和控制输入向量 $u(t)$ 的线性组合决定了状态的演化。这种线性假设简化了问题的复杂性，使得我们可以使用线性代数和解析方法来求解最优控制问题。线性系统模型通常具有以下形式：

$\dot{x}(t)=Ax(t)+Bu(t)$

其中， $\dot{x}(t)$ 是状态向量 $x(t)$ 的导数，表示系统状态随时间的变化率。在控制系统理论中，状态向量 $x(t)$ 描述了系统在任何给定时间点的完整状态，包括位置、速度、角度等系统可能关心的所有状态变量。 $A$ 是状态矩阵，描述了系统状态之间的内部动力学关系。 $B$ 是控制矩阵，描述了控制输入向量 $u(t)$ 如何影响系统的状态，它们都是常数矩阵。

2、二次性能指标

性能指标是衡量系统性能的关键，它通常由两个部分组成：状态惩罚项和控制惩罚项。状态惩罚项 $x^{T}(t)Qx(t)$ 衡量了系统状态偏离期望值的程度，而控制惩罚项 $u^{T}(t)Ru(t)$ 衡量了控制输入的能量。这两项的加权和构成了性能指标，其目的是在系统运行过程中最小化这个指标。权重矩阵 $Q$ 和 $R$ 决定了状态和控制输入在性能指标中的相对重要性。例如，如果系统对某些状态变量的偏差更加敏感，那么相应的 $Q$ 矩阵的对应元素就会更大。性能指标的形式如下：

$J(x,u)=\int_{0}^{T}(x^{T}(t)Qx(t)+u^{T}(t)Ru(t))dt$

其中， $J(x,u)$ 是性能指标，衡量系统性能的函数。 $T$ 是积分的终止时间。

3、最优化问题

LQR问题可以看作是一个最优化问题，目标是在满足系统动态约束的条件下，找到一组控制输入序列 $u(t)$ ，使得性能指标 $J(x,u)$ 达到最小。这个问题可以通过多种数学方法来解决，包括变分法、动态规划和现代优化算法。最优化问题的核心是权衡状态偏差和控制输入之间的成本，以达到全局最优。

4、解析解

LQR问题的一个关键特点是它有一个解析解，这意味着我们可以通过求解一个代数方程来得到最优控制律。这个方程就是代数Riccati方程（ARE），它是一个关于正定矩阵 $P$ 的方程。求解ARE的难点在于它是一个非线性方程，但一旦求解成功，我们就可以得到性能指标的最小值以及最优控制律。ARE的形式如下：

$A^{T}P+PA-PBR^{-1}B^{T}P+Q=0$

求解ARE是LQR设计过程中的关键步骤，因为它提供了性能指标的最小值。

5、最优控制律

最优控制律是LQR设计中的最终产物，它是状态向量 $x(t)$ 的线性函数。这个控制律的形式是：

$u(t)=-R^{-1}B^{T}Px(t)$

它表明最优控制输入与状态向量成线性关系。这个控制律的特点是它能够保证在给定的权重矩阵 $Q$ 和 $R$ 下，性能指标 $J(x,u)$ 被最小化。这种状态反馈控制律是LQR方法的核心。

6、稳定性

LQR设计的一个重要特性是它保证了闭环系统的稳定性。如果权重矩阵 $Q$ 和 $R$ 都是正定的，那么闭环系统是渐近稳定的。这意味着系统状态最终会收敛到一个平衡点，而这个平衡点是性能指标最小化的地方。稳定性是控制系统设计中的一个关键要求，LQR方法通过其设计过程自然地满足了这一要求。

LQR控制策略的应用案例

车道保持辅助（Lane Keeping Assist, LKA）是实现安全、高效驾驶的核心技术之一，特别是在车辆的横向控制方面。通过一个的LKA的例子来详细说明如何使用LQR进行设计。这个例子将包括系统建模、性能指标的定义、求解代数Riccati方程、以及设计最优控制律。

1、系统建模

在LKA系统中，我们的目标是控制车辆沿着车道中心行驶。假设车辆的纵向速度是恒定的，并且只考虑横向控制。车辆的横向动态可以由以下状态空间方程描述：

$\dot{x}(t)=Ax(t)+Bu(t)$

其中：

- $\delta$ ：车辆偏离车道中心线的距离。
- $\dot{\delta }$ ：车辆偏离速度。
- $\psi$ ：车辆相对于车道中心线的转角。
- $\dot{\psi }$ ：车辆的横摆角速度。
$u(t)$ ：转向角或者转向力矩。
$A$ 和 $B$ 是系统矩阵，描述了车辆的动态行为。

2、性能指标定义

LKA系统的性能指标通常由以下二次型表达式给出：

$J=\int_{0}^{\infty }(x^{T}(t)Qx(t)+u^{T}(t)Ru(t))dt$

这里：

$Q$ ：是状态权重矩阵，反映了对车辆偏离车道中心的惩罚。
$R$ ：是控制权重，反映了对控制输入的惩罚。

3、求解代数Riccati方程

LQR方法要求我们求解以下代数Riccati方程（ARE）来找到最优增益矩阵 $P$ ：

$A^{T}P+PA-PBR^{-1}B^{T}P+Q=0$

求解ARE可以得到一个正定矩阵 $P$ ，这个矩阵将用于计算最优控制律。

4、最优控制律设计

根据LQR理论，最优控制律 $u(t)$ 可以通过以下公式给出：

$u(t)=-R^{-1}B^{T}Px(t)$

$u(t)=-\frac{1}{R}B^{T}P\begin{bmatrix} \dot{\delta (t)}\\ \delta (t)\\ \psi (t)\\ \dot{\psi (t)} \end{bmatrix}$

这个控制律确保了性能指标 $J$ 的最小化。

$Q$ 和 $R$ 矩阵的选择对控制器的性能有很大影响。较大的 $Q$ 值会使控制器更注重减少车辆偏离车道中心，而较大的 $R$ 值会使控制器更注重减少控制输入的幅度，以避免过度的转向动作。

LQR控制策略的代码

#include <iostream>
#include <Eigen/Dense>

int main() {
    // 定义状态变量
    // x = [车辆偏离车道中心线的距离，车辆偏离速度，车辆相对于车道中心线的转角，车辆的横摆角速度]^T
    Eigen::Vector4d x;
    // 初始状态假设为 [偏离距离, 偏离速度, 转角, 横摆角速度]
    x << 0.5, 0.0, 0.0, 0.0;

    // 系统矩阵 A 和 B（这里仅为示例，实际参数需要根据车辆动力学确定）
    Eigen::Matrix4d A;
    A << 0, 1, 0, 0,
         0, 0, -9.81 / 2, 0,
         0, 0, 0, 1,
         0, 0, 1, 0;

    Eigen::Matrix<double, 4, 1> B;
    B << 0, 0, 0, 1; // 假设控制输入直接影响横摆角速度

    // 定义权重矩阵 Q 和 R
    Eigen::Matrix4d Q = Eigen::Matrix4d::Identity(); // 状态权重矩阵
    double R = 1.0; // 控制权重

    // 使用Eigen求解连续时间代数Riccati方程
    Eigen::Matrix4d P = (A - B * (B.transpose() * P * A) * R) * Q * (A - B * (B.transpose() * P * A)).transpose() + B * (B.transpose() * P * A) * R;

    // 检查P是否为正定矩阵
    if (P.eigenvalues().minReal() <= 0) {
        std::cerr << "Matrix P is not positive definite." << std::endl;
        return -1;
    }

    // 计算增益矩阵 K
    Eigen::Vector4d K = (B.transpose() * P * A).inverse() * (B.transpose() * P);

    // 打印增益矩阵 K
    std::cout << "Gain matrix K:\n" << K << std::endl;

    // 模拟控制输入
    double u = -K.dot(x); // LQR控制律
    std::cout << "Control input u: " << u << std::endl;

    return 0;
}