自动驾驶控制算法——LQR控制算法

自动驾驶控制算法——LQR控制算法

一、LQR 是什么？

Linear Quadratic Regulator（线性二次型调节器，简称 LQR）是一类基于状态反馈的最优控制算法。
它针对“线性系统 + 二次型代价函数”这对经典组合，自动给出一组最优反馈增益，从而在误差最小化与控制能量最小化之间取得全局最优折中。
在自动驾驶中，LQR 被广泛用于横向/纵向轨迹跟踪、车道保持（LKA）、稳定性控制（VSC）、智能底盘协同控制等场景，兼具实时性、多变量耦合处理能力与理论最优性。

二、LQR 原理

2.1 线性状态空间模型 (State–Space Model)

对连续时间系统，令

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

• $x(t)\in {\mathbb{R}}^n$：状态向量（位置、姿态、速度、误差等）
• $u(t)\in {\mathbb{R}}^m$：控制输出（转向角、驱动力、制动力等）
• $A$、$B$：系统动力学矩阵（可由自行车模型、三自由度模型或实验辨识而得）

离散时间版本（采样周期 $\Delta t$）同理：

$x_{k+1}=A_d{x}_k+B_d{u}_k$

2.2 二次型性能指标 $J$

设计目标：在 $t\in [0,\infty )$ 内最小化

$J={\int }_0^{\infty }(x^{\mathrm{T}}(t)Qx(t)+u^{\mathrm{T}}(t)Ru(t))dt$

2.3 代数黎卡提方程 (ARE)

最优解存在唯一正定矩阵 $P$，满足

$A^{\mathrm{T}}P+PA-PB{R}^{-1}{B}^{\mathrm{T}}P+Q=0$

得到最优反馈增益

$K=R^{-1}{B}^{\mathrm{T}}P⟹u(t)=-Kx(t)$

2.4 特点总结

1. 最优性：对给定 $A,B,Q,R$ 全局最优；无局部极值困扰。
2. 多变量耦合：一次性考虑所有状态与输入，天然处理耦合。
3. 实时性高：上线前离线求 $K$，在线只需矩阵乘法。
4. 线性假设：需在工作点线性化；大幅偏离或强非线性场景下效果下降。
5. 无显式约束：不直接处理饱和与障碍，需要配合限幅或外层规划器。

2.5 一句话总结 LQR 原理：

LQR 就像一个“聪明的爸爸”，手里拽着你（小孩）一根绳子，在奔跑中努力让你既不跑偏，又不会猛拉你太用力。

2.5.1 场景类比：小孩牵绳跑步

假设你（小孩）在一个广场上跑步，有一个目标路线（比如一条蓝色的直线轨迹），你爸（控制器）拽着你一根弹力绳，任务是让你尽量沿着这条轨迹跑。你爸不能直接控制你，但可以通过拉扯绳子的方式调整你的方向。

现在他面临两个问题：

1. 你偏离轨迹越多（状态误差大），他就想越用力拉你回来
2. 但用力过猛你会摔倒（控制动作大 → 不舒适），他也不想那样

于是他就想着：

“我要最优地拉这根绳子，让我家孩子既快速回到轨迹，又不至于每次都摔个大跟头。”

这时候他脑子里其实就运行着一个 LQR 控制器。

2.5.2 数学意义下的类比映射：

2.5.3 更直白一点的解释：

LQR 的工作流程可以理解为以下四步：

① 系统动态是什么？

你是个小车，你能跑、能转弯，爸爸知道你每跑一步、每拐一个弯，会跑到什么位置 → 系统模型：

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

② 什么样的轨迹才叫“好”？

当然是既偏差小，又别太用力操控你：

$J={\int }_0^{\infty }\left[x^{T}Qx+u^{T}Ru\right]dt$偏差太大 $x^{T}Qx$ 会被罚

• 控制太猛 $u^{T}Ru$也要罚
• 怎么罚？用矩阵 $Q,R$ 来定义，自己调！

③ 怎么做才最优？

解一个叫“黎卡提方程”的东西，得到一个增益 $K$，然后只要你一偏离，控制器就自动拉回：

$u(t)=-Kx(t)$

这个反馈矩阵 $K$ 就是控制器的“聪明劲儿”。

④ 最终结果如何？

• 如果你设计得好，系统运行起来就像小车沿轨道平稳前进，轻松纠偏
• 如果你太注重精度（$Q$太大），控制器可能疯狂操作
• 如果你太怕费力（$R$太大），小车就变得慵懒、不追轨了

🖼️ 图像化理解你可以把 LQR 理解为：“我告诉系统什么是‘好’，它就自己计算出‘怎么做’”。

总结关键思想

2.6 案例分析 —— 车辆轨迹跟踪

2.6.1 任务介绍

• 目标：让车辆以 15 m/s 的恒速精准沿参考曲线（S 形）行驶，最小化横向误差 $e_y$ 和航向误差 $e_{\psi }$。
• 模型：采用简化自行车模型并在小角度、恒速条件下线性化。

2.6.2 控制流程

1. 实时感知车辆位置与姿态 → 计算误差向量 $x=[e_y,{\dot{e}}_y,e_{\psi },{\dot{e}}_{\psi }{]}^{\mathrm{T}}$。

2. 状态反馈：控制器输出前轮转角

   $\delta =-Kx$

3. 车辆执行 → 新状态回馈 → 进入下一控制周期。

2.6.3 参数调优现象观察

• 增大 $Q_{(e_y,e_{\psi })}$：曲线跟随更紧，响应更快，但控制动作更激进，可能出现抖动。
• 增大 R：转向更平滑、舒适，跟踪误差增大，可能在急弯处外偏。
• 平衡策略：先用 Bryson’s Rule 给初值，再基于仿真/现场试验迭代微调。

2.6.4 不同 Q/R 组合的典型效果

图中红色曲线展示了在**非线性车辆模型（Kinematic Bicycle Model）**下，LQR 控制器如何调整前轮转角，使车辆从偏离状态逐步回到并跟踪 S 型参考轨迹（黑色虚线）。

这说明即使模型是非线性的，基于线性近似设计的 LQR 控制器也能取得良好的控制效果，尤其在中等速度、路径曲率适中的场景下非常适用。

2.7 LQR 参数解读

2.7.1 常用术语

2.7.2 权重矩阵 $Q,R$对性能的影响

• 大 $Q$ / 小 $R$：强调跟踪精度 → 响应快、可能过冲。
• 小 $Q$ / 大 $R$：强调控制平滑 → 误差大、舒适性高。

2.7.3 关键性能指标

• 稳态误差
• 超调量 / 峰值转角
• 90% 调整时间
• 控制能量 $\int u^{2}dt$

2.7.4 计算与信号流程图

感知 → 计算状态误差 → LQR 内核（读 $K$ 表 → 乘以 −$K$）→ 输出控制 → 车辆执行 → 反馈环。 （工程实现中常封装成 ROS 控制节点或嵌入式任务）

2.7.5 典型作用总结

• 在紧急避障前提供平稳高精度跟踪。
• 可作为 MPC 的终端控制器或非线性控制器的局部线性近似补偿器。
• 在低速或大曲率场景下，可结合速度规划降低侧向加速度峰值。

2.7.6 设计准则与物理量对应关系

三、离散 LQR 推导（DLQR）

3.1 离散状态空间模型

$x_{k+1}=A_d{x}_k+B_d{u}_k,k=0,1,\dots$

• $x_k\in {\mathbb{R}}^n$：第 $k$ 步系统状态
• $u_k\in {\mathbb{R}}^m$：控制输入
• $A_d,B_d$：离散化后的系统矩阵（可由零阶保持 $A_d=e^{A\Delta t},B_d={\int }_0^{\Delta t}{e}^{A\tau }d\tau B$ 获得，或直接辨识/数值近似）

3.2 无限时域二次型性能指标

$J={\sum }_{k=0}^{\infty }(x_k^{\top }Q{x}_k+u_k^{\top }R{u}_k),Q⪰0,R\succ 0$

有限时域（Horizon NNN）只需将求和上限改为 $N-1$，末端加终端权 $x_N^{\top }S{x}_N$。

3.3 动态规划推导

1. 定义成本到终点（Cost-to-Go）函数

   $J_k(x_k)={\sum }_{i=k}^{\infty }(x_i^{\top }Q{x}_i+u_i^{\top }R{u}_i)$

2. Bellman 最优性原理

   $J_k(x)={\min }_u\left[x^{\top }Qx+u^{\top }Ru+J_{k+1}(A_{d}x+B_{d}u)\right]$

3. 二次型猜想 设$J_k(x)=x^{\top }{P}_{k}x$，则递推得到$P_k=Q+A_d^{\top }{P}_{k+1}{A}_d-A_d^{\top }{P}_{k+1}{B}_d(R+B_d^{\top }{P}_{k+1}{B}_d{)}^{-1}{B}_d^{\top }{P}_{k+1}{A}_d$_

4. 最优控制律**$u_k^{\star }=-K_k{x}_k,K_k=(R+B_d^{\top }Pk+1B_d{)}^{-1}{B}_d^{\top }Pk+1A_d$

5. 无限时域收敛：若 $(A_d,B_d)$ 稳定可控且$(A_d,Q^{1/2})$ 可观，则存在极限

   $P_k\underset{k\to \infty }{\to }{P}_{\infty }$

   其满足离散代数黎卡提方程（DARE）

   $P=A_d^{\top }P{A}_d-A_d^{\top }P{B}_d(R+B_d^{\top }P{B}_d{)}^{-1}{B}_d^{\top }P{A}_d+Q$

   得到 恒定最优增益

   $K=(R+B_d^{\top }P{B}_d{)}^{-1}{B}_d^{\top }P{A}_d$

数值求解

• Scipy：scipy.linalg.solve_discrete_are(A_d,B_d,Q,R)
• MATLAB：[K,P,~] = dlqr(A_d,B_d,Q,R)
• 若做有限时域控制，可从终端条件 $P_N=S$ 向前递推（Backward Riccati Sweep）。

3.4 DLQR 与 CLQR 的对应关系

四、iLQR（Iterative LQR）扩展

动机：LQR 只适用于线性近似；真实车辆模型$x_{k+1}=f(x_k,u_k)$ 往往非线性。
思路：在给定控制序列附近迭代线性化 + 二次化，每轮都做一次“时变 LQR” → 前向滚动更新 → 直至收敛。
本质上是 DDP（Differential Dynamic Programming）的简化实现，计算量更轻，且易于嵌入 MPC/规划器。

4.1 问题定义

• 非线性动力学：$x_{k+1}=f(x_k,u_k)$
• 分段代价：${\ell }_k(x_k,u_k)$，终端代价 ${\ell }_f(x_N)$
• 目标：$\underset{u_{0:N-1}}{\min }\sum _{k=0}^{N-1}{\ell }_k+{\ell }_{f}u0:N-1$

4.2 算法框架

1. 初始化：选一条可行控制序列 { u k ( 0 ) } \{u_k^{(0)}\} {uk(0)​}，正向仿真得到轨迹 { x k ( 0 ) } \{x_k^{(0)}\} {xk(0)​}。

2. 迭代循环 ($t=0,1,2,\dots$)：

   • 线性化：对每个步长 $k$

     $\delta x_{k+1}=A_k\delta x_k+B_k\delta u_k,A_k={\frac{\partial f}{\partial x}|}_{(x_k^{(t)},u_k^{(t)})},B_k={\frac{\partial f}{\partial u}|}_{(\cdot )}$

   • 二次化代价：泰勒展开 ${\ell }_k$ 得二次型 $\delta x,\delta u$。

   • 后向遍历（LQR backward pass）：求时变增益 $K_k^{(t)},k_k^{(t)}$（反馈 + 前馈）。

   • 前向滚动：更新控制

     $u_k^{(t+1)}=u_k^{(t)}+{\alpha }^{(t)}{k}_k^{(t)}+K_k^{(t)}(x_k^{(t+1)}-x_k^{(t)})$

     并重新仿真得 { x k ( t + 1 ) } \{x_k^{(t+1)}\} {xk(t+1)​}

   • 线搜索：调节 ${\alpha }^{(t)}\in (0,1]$ 满足代价下降。

   • 收敛判据：$|J^{(t+1)}-J^{(t)}|<\epsilon$ 或迭代次数达上限。

4.3 简化伪代码

Input: f, ℓ, ℓ_f, horizon N, x0
Initialize U ← {u0,...,u_{N-1}} (zeros or heuristic)
while not Converged do
    X ← ForwardRollout(f, x0, U)
    {A_k,B_k,ℓ_x,ℓ_u,ℓ_xx,ℓ_uu,ℓ_ux} ← LinearizeAndQuadratize(X,U)
    {K_k,k_k} ← BackwardPass(A,B,ℓ_x,ℓ_u,ℓ_xx,ℓ_uu,ℓ_ux)
    U_new ← LineSearchAndUpdate(U,K,k)
    if CostDropSmall then break
    U ← U_new
end while
return U, X

4.4 iLQR 与其它方法对比

4.5 工程实践要点

• 采样周期：iLQR 得到的是开环控制序列；可每 τ\tauτ 秒重解一次，外层加反馈稳健性更佳（Receding Horizon iLQR）。
• 权重与正则：常在二次化阶段对 Hessian 加 $\lambda I$ 保证正定。
• 约束：可在代价中加入 barrier / soft-constraint，也可外层投影。
• 软件：
  • ilqr（Python）、traja/ilqr (基于 JAX)
  • acados 非线性 MPC 内含 iLQR kernel
  • C++ 实时实现可参考 mpc-lqr、pinocchio+croccodile

五、LQR 权重整定方法（参数选择）

LQR 的设计核心在于选择合适的权重矩阵 $Q$ 与 $R$，它们反映了控制器对误差状态与控制输入的“偏好程度”。调得好，可以实现高精度+高舒适性；调不好则可能导致过度抖动、饱和、甚至失稳。本节介绍三种常用的权重调参策略。

5.1 ✅ Bryson’s Rule

📌 基本思想

将每个状态量/控制量的“最大允许值”设为代价函数中的单位惩罚基准。

即：对每个分量设定其最大允许值 $x_{i,\max }$、$u_{\max }$，令：

$Q_{ii}=\frac{1}{x_{i,\max }^2},R=\frac{1}{u_{\max }^2}$

🧠 背景原理

• LQR 性能指标函数本质上是：

  $J=\int x^{\top }Qx+u^{\top }Rudt=\sum \text{(状态惩罚)}+\text{(控制惩罚)}$

• 若我们将每个量归一化到它的最大可接受值，那么所有项的权重就统一了 → 得到一个物理量解释清晰的权重矩阵。

🔧 操作步骤

1. 明确各状态量最大可接受偏差（如位置偏差 ≤ 0.2 m，偏航角 ≤ 3° 等）

2. 明确控制量最大允许范围（如最大转角 ≤ 25°）

3. 代入公式构建权重矩阵

   $Q=\mathrm{diag}\left(\frac{1}{x_1^2},\frac{1}{x_2^2},\dots \right),R=\frac{1}{u_{\max }^2}$

4. 若还需强调某些状态量，可在此基础上乘上放大系数（例如横向位置乘以 5）。

🚗 自动驾驶应用案例

5.2 🛠️ Trial-and-Error 调参法

📌 基本思想

基于仿真/试验观察系统响应曲线，手动增减 $Q$/$R$ 中元素逐步调整控制行为。

🧠 背景原理

• LQR 是解析型反馈控制，其性能高度依赖于代价函数的构型
• 直接通过增大/减小某一项权重，可以预测性地影响系统的响应曲线特征（如超调、响应时间、震荡等）

🔧 操作步骤

1. 初始设置（例如单位矩阵）：

   $Q=I,R=1$

2. 仿真运行系统，看如下响应指标：

   • 超调量
   • 稳态误差
   • 峰值控制量
   • 抖动/频繁操作

3. 观察控制趋势：

   • 跟踪不准？→ 提高 $Q$ 的关键项
   • 抖动太大？→ 提高 $R$

4. 循环调整至满足性能指标或舒适性要求

🛠 工具推荐

• Python + matplotlib + control 仿真框架
• MATLAB Simulink + scope 曲线可视化
• ROS 中动态配置 K 调参并采集驾驶行为数据

🚗 自动驾驶应用案例

5.3 📐 LMI-Based 自动整定

📌 基本思想

将 LQR 权重选择问题转化为一个线性矩阵不等式（LMI）优化问题，自动寻找满足性能指标下的最优权重。

🧠 背景原理

• LQR 本质是稳定性约束下的最优控制 → 可在 Lyapunov 函数约束下构建如下 LMI：

  $\{\begin{array}{l}{A}_c=A-BK\text{稳定}\\ V(x)=x^{\top }Px,P\succ 0\\ \text{最小化 }\mathrm{tr}(QP)\end{array}$

• 通过控制 Lyapunov 函数下降速率 + 状态/控制限幅约束，可自动搜索 $Q,R$ 满足性能指标

🔧 操作步骤

1. 明确系统模型 $(A,B)$ 与性能指标（如响应时间、能耗上限等）
2. 将问题转化为优化形式（minimize 某种 cost, subject to LMI）
3. 使用 LMI 优化工具（如 CVX, YALMIP）进行数值求解
4. 从求得的 $P$ 与优化变量中反推 $K$、$Q,R$

🛠 工具推荐

• MATLAB + YALMIP + SeDuMi
• Python + CVXPY（支持 SDP）
• SciML Ecosystem（Julia）也支持非线性 SDP

🚗 自动驾驶应用案例

• 自动整定用于满足“最大横向加速度不超过 2.5m/s²”约束
• 控制能量最小化 → 节能路径跟踪控制器
• 系统稳定裕度 ${\lambda }_{\min }(P)$ 最大化设计

六、python实现代码

import numpy as np
import matplotlib.pyplot as plt
from scipy.linalg import solve_continuous_are

# ----------------------------
# 1. 定义线性系统模型
# ----------------------------
# 状态空间模型：dx/dt = A x + B u
A = np.array([[0, 1],
              [0, 0]])              # 双积分器模型（位置+速度）
B = np.array([[0],
              [1]])                # 控制输入为加速度

# ----------------------------
# 2. 设置 LQR 权重矩阵
# ----------------------------
Q = np.array([[10, 0],             # 惩罚位置误差更重
              [0, 1]])             # 惩罚速度误差次之
R = np.array([[1]])                # 控制成本

# ----------------------------
# 3. 求解黎卡提方程并计算最优反馈增益 K
# ----------------------------
P = solve_continuous_are(A, B, Q, R)         # 解代数黎卡提方程
K = np.linalg.inv(R) @ B.T @ P               # LQR 最优增益

print("LQR Gain K =", K)

# ----------------------------
# 4. 仿真控制系统响应
# ----------------------------
dt = 0.01                     # 时间步长
T = 5                         # 仿真总时间
steps = int(T / dt)
x = np.array([[1.0], [0.0]])  # 初始状态：偏离1m，速度为0
trajectory = [x.flatten()]

for _ in range(steps):
    u = -K @ x                # 状态反馈控制律
    x_dot = A @ x + B @ u     # 系统动态
    x += x_dot * dt
    trajectory.append(x.flatten())

trajectory = np.array(trajectory)

# ----------------------------
# 5. 可视化结果
# ----------------------------
plt.figure(figsize=(8, 4))
plt.plot(trajectory[:, 0], label="Position $x_1$ (m)")
plt.plot(trajectory[:, 1], label="Velocity $x_2$ (m/s)")
plt.axhline(0, color='gray', linestyle='--')
plt.xlabel("Time Step")
plt.ylabel("State Value")
plt.title("LQR Control Response of a Double Integrator")
plt.legend()
plt.grid(True)
plt.tight_layout()
plt.show()

小结对比