这两周一直在调一台基于LQR横向控制的小车，说实话，LQR这个算法在自动驾驶规划控制岗的面试和实战里出现的频率，高到有点离谱。但很多人对它的理解停留在“查一下Riccati方程，调一下Q和R矩阵，然后调出来一个K矩阵”这种黑盒层面。作为“最优化理论与自动驾驶”系列的第三篇，这篇我直接把它彻底拆开：原理怎么来的、公式每一步为什么长这样、以及代码到底怎么写才能用于实际的路径跟踪，一次性说清楚。
如果你是想入门自动驾驶控制方向的学生，或者是正在用LQR做平衡车、无人机、STM32小车项目的实践型选手，这篇文章的公式推导和Python代码都可以直接参考。我不打算灌水，只讲在工程里真正会被用到的东西，以及一些教科书里没有明说的坑。
1. LQR在自动驾驶规划控制中到底扮演什么角色
1.1 一个两轮车的横向控制例子
先说一个最简单的场景。一辆车在一条弯曲的道路上行驶，车上装了GPS或者视觉定位系统，我们可以实时知道车辆当前的位置、航向角，也知道前方参考路径的几何信息。现在的问题是：方向盘该怎么打，才能让车辆又快又稳地贴合这条路径？
如果你第一反应是PID，那没错，很多工程初版方案确实这么干。但PID在处理路径曲率连续变化、车速变化、车身横摆动力学耦合这些情况时，参数会调得非常痛苦。你可能会发现：低速调好的参数，一到高速就开始画龙；弯道松油门的时候没事，一加油门就震荡。
LQR解决的就是这个问题。它把“车身状态误差”和“控制输入代价”放在同一个数学框架里，通过最优化理论直接算出每个控制周期里最优的方向盘/前轮转角增量。不需要人工去试一大堆增益，只需要定义好“车偏离路径多少算严重”“打方向盘多猛算过分”，剩下的交给矩阵运算。
上面这个例子，其实就是LQR在自动驾驶路径跟踪中的典型应用场景。我圈内朋友做LQR平衡车、LQR无人机控制，底层数学框架也完全一样，只是状态矩阵和控制矩阵的维度和物理含义不同。学会一个，一通百通。
1.2 和PID、MPC的定位差异
我在带团队的时候，经常被问一个问题：既然MPC那么强，为什么很多量产项目里还在用LQR？这个问题特别值得单独拉出来说。
自动驾驶规划控制栈里的控制器通常分三层：全局路径规划、局部轨迹规划、底层跟踪控制。LQR和MPC主要都在“跟踪控制”这一层发挥作用，但两者计算量和性能边界差别很大。
| 维度 | PID | LQR | MPC |
|---|---|---|---|
| 模型利用 | 不用模型 | 需要线性模型 | 需要模型，可支持非线性 |
| 约束处理 | 不支持 | 不支持（或后期加限制） | 原生支持 |
| 计算量 | 极小 | 小，矩阵求解即可 | 大，需在线优化 |
| 最优性 | 无 | 局部最优（无限时域） | 有限时域最优 |
| 典型场景 | 简单跟随、底层稳速 | 横向控制、姿态控制 | 复杂避障、轨迹规划 |
我的建议是：如果约束不复杂、模型可以用线性微分方程近似，LQR永远是工程性价比最高的选择。MPC确实能处理转向饱和、加速度限制这类硬约束，但调MPC的权重矩阵和预测时域，那你得多准备几周的调试时间。所以这篇文章选的方案是LQR，它既是控制理论的最优控制入门核心，也是自动驾驶横向控制里落地最广的算法之一。
2. 从自行车模型到误差状态方程
2.1 为什么选自行车模型
要设计LQR，第一步不是写代码，而是建模。车辆是一个复杂的多体系统，四个轮胎各有侧偏特性，悬架有运动学约束。但如果把这些全建进去，LQR的线性系统框架根本招架不住。实际工程里，我们基本都假设左右车轮的转向角相同、车辆做平面运动，于是四轮车辆可以等效成一个“前轮转向、后轮固定”的自行车模型。
这个模型的运动学方程是：
dot{x} = v·cos(psi) dot{y} = v·sin(psi) dot{psi} = (v / L) · tan(delta)其中x、y是车辆后轴中心位置，psi是航向角，v是纵向速度，L是轴距，delta是前轮转角。这个模型的物理直觉很明显：前轮转角决定车辆的旋转快慢，轴距越短，车转得越灵活。
2.2 误差状态的定义与线性化
LQR设计的是状态反馈控制器，所以我们要把“车辆坐标”转换成“车辆相对参考路径的误差”。定义两个核心误差量：
- 横向误差 e_y：车辆后轴中心到参考路径最近点的垂直距离
- 航向误差 e_psi：车身航向角与参考路径切线方向的夹角
这里假设速度 v 恒定，参考路径曲率为 kappa_ref。不考虑轮胎侧偏时，横向误差和航向误差可以构成一个线性时变系统。为了让LQR能够求解，我们在每个控制周期把参考路径当前点的状态当作工作点做线性化。
误差状态的一阶动态可以写成：
dot{e}_y = v·e_psi dot{e}_psi = (v / L)·delta - v·kappa_ref把参考曲率项当作已知扰动或者前馈量，忽略掉它之后，状态方程变成：
[dot{e}_y] [0 v] [e_y] [ 0 ] [dot{e}_psi] [0 0] [e_psi] [ v/L ] * delta这一下就清爽了。A矩阵、B矩阵全是常数或只跟车速相关，完全符合LQR的适用条件。
2.3 线性时变模型的处理思路
看到A矩阵里带一个速度v，可能马上会有人问：车速在变化怎么办？这不是一个定常系统啊。工程上有两种处理办法。
第一种，低速或者速度变化缓慢的场景，直接把当前车速当成常数代入A矩阵，每个控制周期重新计算一次Riccati方程，速度变了，K矩阵就跟着变。这种叫自适应增益调度，小车项目和不少实车方案都是这么做的。
第二种，把状态扩维，把纵向加速度也加进状态方程建耦合模型。这种方法更精确但推导量上去了，一般只有做极限工况控制才会用到。我自己的实践体会是：在常规速度变化范围内，第一种方法已经完全够用，而且鲁棒性更好。把模型弄复杂了，参数标定难度反而成倍增加。
3. 代价函数：LQR的“最优化”灵魂
3.1 二次型代价函数怎么读
LQR的全称是Linear Quadratic Regulator，前面“Linear”指线性系统，“Quadratic”指的就是代价函数是二次型。控制目标被定义为：
J = ∫ ( x^T · Q · x + u^T · R · u ) dt离散形式：
J = Σ ( x_k^T · Q · x_k + u_k^T · R · u_k )别被这个数学符号吓住。你可以把 x^T Q x 理解成“当前状态偏离理想状态有多远”，把 u^T R u 理解成“当前控制动作有多激烈”。LQR要做的事情，就是在整个运行时间里，找到一条控制序列，让这两部分的累计代价最小。
举个生活化的类比：你想让一辆车贴着一根线走，Q就是你对“贴线精度”的执念，R就是你对“方向盘打得太猛”的排斥。如果你完美主义，想贴着线分毫不差，那就把Q调大；如果你开车风格激进，不太在意偶尔偏一点，但受不了车来回晃，那就把R调大。
3.2 Q和R的物理解读
Q和R不是随便拍脑袋定的，它们的每一项都有明确的物理量纲和工程语义。
Q矩阵维度等于状态维度，对应对角线上的数值表示“各个状态误差的重要程度”。在横向控制这个[ e_y, e_psi ]的两状态系统里：
- Q[0][0]越大，说明越在意横向偏差，车会更快贴回参考线
- Q[1][1]越大，说明越在意航向偏差，车会更快摆正车身方向
R矩阵维度等于控制输入的个数，在方向盘转角系统里就是单一数值。R越大，转角增量就会被压得越小，控制action越温柔。Q和R的比值Q/R才是关键，而不是绝对值。你同时把Q和R放大100倍，解出来的K矩阵完全一样。
3.3 为什么是二次型而不是四次型
这是很多学最优化理论的人会卡住的地方。其实原因很朴素：二次型是“凸的”，这个二次型最优化问题存在唯一的全局最优解，而且在线性系统约束下可以解析求解。如果换成四次型，虽然惩罚大误差更狠，但求解难度指数级上升，在实车上每个控制周期只有几毫秒的预算，根本跑不动。
更重要的是，二次型代价函数下求出来的控制律有一个绝佳的性质：它是一个线性状态反馈 u = -Kx。这个性质保证了工程实现极其简单，实时性极高。这就是LQR能在车载ECU、STM32这类算力有限的硬件上跑的底气。
4. Riccati方程的推导脉络与数值求解
4.1 从最优性条件到Riccati方程
网上关于LQR的帖子很多，但大多数只告诉你“最后要解Riccati方程”，不讲这个方程从哪来的。这里我把推导逻辑用最直白的方式捋一遍。
把系统的状态动态 x_dot = Ax + Bu 当成等式约束，和代价函数放到一起，构造哈密顿函数。根据变分法和最优性条件，可以推出最优控制 u 必须满足 u = -R^{-1} B^T λ，其中 λ 是协态变量。再假设协态变量与状态之间满足线性关系 λ = Px，把这个假设代回最优性条件，就能消除 λ 和 u，只剩下关于P的方程。
这个方程就是大名鼎鼎的代数Riccati方程：
A^T·P + P·A - P·B·R^{-1}·B^T·P + Q = 0解出P之后，最优反馈增益就是：
K = R^{-1}·B^T·P这条推导链路的每一步，本质都是在回答同一个问题：在“状态要稳”和“控制要省”之间，最优解恰好长成线性反馈的形式。这是最优化理论里最优控制思想最优雅的体现。
4.2 离散Riccati方程与迭代解法
工程上我们通常离散化系统，在每个采样周期k上计算控制输入。离散形式的Riccati方程是：
P = A_d^T·P·A_d - A_d^T·P·B_d·(R + B_d^T·P·B_d)^{-1}·B_d^T·P·A_d + Q这个方程没有手算的必要，代码里用迭代法求解即可：从 P = Q 开始，反复代入上式右侧，直到P矩阵变化量小于阈值，P就收敛了。迭代次数一般几十次到上百次就能收敛，在嵌入式硬件上完全跑得动。
如果不想自己写迭代，Python里直接用 scipy.linalg.solve_discrete_are 一行就能求解。但我更建议你自己实现一遍迭代法，一能加深理解，二是在没有科学计算库的环境里也能用。
4.3 增益矩阵K的含义
得到K之后，控制量就是 u_k = -K·x_k。K矩阵的每一行对应一个控制输入，每一列对应一个状态量。以横向控制为例，K = [ K1, K2 ]，控制量 delta = -K1·e_y - K2·e_psi。
拆开看，这个控制律和PD控制长得非常像：K1相当于比例项，盯着横向偏差打方向；K2相当于微分项，盯着航向偏差抑制振荡。但LQR里的K不是拍脑袋调的，它是在最优化意义下、根据Q和R自动算出的最优比例。这也就是为什么很多人说“LQR是PID的进阶版”，从数学上讲确实如此。
5. Python代码实现：从路径跟踪到可视化
5.1 代码结构总览
代码我用Python实现，依赖numpy和matplotlib，不需要装ROS，也不需要实车。整个过程拆成四个模块：车辆运动学模型、参考路径生成、LQR求解器、主循环可视化。这样的结构方便你移植到STM32或者自己改造成无人机版本。
import numpy as np import matplotlib.pyplot as plt class BicycleModel: def __init__(self, L=2.5, dt=0.05): self.L = L self.dt = dt self.x = 0.0 self.y = 0.0 self.psi = 0.0 self.v = 5.0 def update(self, delta, v=None): if v is not None: self.v = v self.psi += self.v / self.L * np.tan(delta) * self.dt self.x += self.v * np.cos(self.psi) * self.dt self.y += self.v * np.sin(self.psi) * self.dtBicycleModel类用来模拟被控车辆。这里把dt固定为0.05秒，对应20Hz的控制频率，这个频率比较接近实际底盘控制器的运行频率。
5.2 参考路径与最近点匹配
参考路径我用一条正弦曲线，因为曲率连续变化，最能检验跟踪算法好坏，比纯直线有说服力得多。
def generate_reference_path(): s = np.linspace(0, 200, 2000) x_ref = s y_ref = 3.0 * np.sin(0.15 * s) psi_ref = np.arctan2(np.gradient(y_ref), np.gradient(x_ref)) return np.stack([x_ref, y_ref, psi_ref], axis=1)主循环里每一帧都要做一件关键的事：找参考路径上离车辆当前位置最近的点。这个操作叫“最近点匹配”。有了最近点，才能计算横向误差 e_y 和航向误差 e_psi。
def find_nearest_point(path, x, y): dist = (path[:, 0] - x) ** 2 + (path[:, 1] - y) ** 2 idx = int(np.argmin(dist)) return path[idx], idx不要小看这段代码，它藏着很多工程问题。后面我会专门讲“最近点跳变”这个坑。
5.3 离散LQR求解器
核心求解器如下：
def solve_lqr(A, B, Q, R, dt, max_iter=10000, tol=1e-6): n = A.shape[0] m = B.shape[1] # 零阶保持离散化 A_d = np.eye(n) + A * dt B_d = B * dt # 迭代求解离散Riccati方程 P = Q.copy() for _ in range(max_iter): tmp = B_d.T @ P @ B_d + R P_new = A_d.T @ P @ A_d \ - A_d.T @ P @ B_d @ np.linalg.inv(tmp) @ B_d.T @ P @ A_d \ + Q if np.max(np.abs(P_new - P)) < tol: P = P_new break P = P_new K = np.linalg.inv(B_d.T @ P @ B_d + R) @ B_d.T @ P @ A_d return K离散化这里我用了零阶保持（ZOH）近似，也就是 A_d = I + A·dt，B_d = B·dt。这个近似在采样时间远小于系统最小时间常数时精度足够。然后迭代到P收敛，再算K矩阵。
如果你图省事，也可以这样：
from scipy.linalg import solve_discrete_are P = solve_discrete_are(A_d, B_d, Q, R) K = np.linalg.inv(R + B_d.T @ P @ B_d) @ B_d.T @ P @ A_d两种方法结果一致，我建议你把迭代版自己敲一遍，跑通之后再用scipy版本替代，理解深度完全不同。
5.4 主循环与仿真结果
这里是完整的主循环，包含控制、状态更新和数据记录：
def run_simulation(): dt = 0.05 v = 5.0 L = 2.5 vehicle = BicycleModel(L=L, dt=dt) path = generate_reference_path() Q = np.diag([1.0, 1.0]) R = np.array([[1.0]]) x_history, y_history = [], [] for _ in range(int(20 / dt)): nearest_pt, idx = find_nearest_point(path, vehicle.x, vehicle.y) dx = vehicle.x - nearest_pt[0] dy = vehicle.y - nearest_pt[1] path_psi = nearest_pt[2] e_y = -dx * np.sin(path_psi) + dy * np.cos(path_psi) e_psi = vehicle.psi - path_psi e_psi = np.arctan2(np.sin(e_psi), np.cos(e_psi)) state = np.array([e_y, e_psi]) A = np.array([[0.0, v], [0.0, 0.0]]) B = np.array([[0.0], [v / L]]) K = solve_lqr(A, B, Q, R, dt) delta = float(-K @ state) delta = np.clip(delta, -0.6, 0.6) vehicle.update(delta, v) x_history.append(vehicle.x) y_history.append(vehicle.y) plt.figure(figsize=(10, 6)) plt.plot(path[:, 0], path[:, 1], 'b--', label='reference') plt.plot(x_history, y_history, 'r-', label='LQR tracking') plt.legend() plt.grid(True) plt.axis('equal') plt.show() run_simulation()跑出来的结果，能明显看到车辆从初始偏差位置逐渐收敛到参考路径，之后全程贴线行驶，没有超调振荡，也没有稳态误差。这就是LQR效果的直观体验。
6. 调参、坑点与工程化建议
6.1 不同Q/R参数的响应对比
很多人拿到代码之后第一件事就是乱调Q和R，发现效果不对就怀疑算法出了问题。其实LQR的参数调节有明确的方向，我贴三组典型实验对比，你看完就懂了。
| 参数设置 | 实际表现 | 决策建议 |
|---|---|---|
| Q=diag(1,1), R=1 | 响应适中，无超调，稳态误差小 | 初级项目推荐 |
| Q=diag(50,1), R=1 | 横向偏差快速归零，但航向角容易振荡 | 需要快速纠偏时用 |
| Q=diag(1,1), R=10 | 方向盘动作平缓，但弯道跟踪滞后明显 | 舒适性优先场景 |
第一组参数是很好的起步点。如果你发现车贴着线来回晃，多半是R太小，把R调大；如果你发现车入弯慢、总是切弯，多半是Q[0][0]不够大，把横向偏差权重调大。这个规律几乎适用于所有用LQR做控制的项目。
6.2 最近点匹配跳变的问题
这是我在实际项目里踩过最大的坑，必须单独拎出来讲。
当路径曲率比较急，或者车辆偏离路径较远时，车辆当前位置到参考路径的最近点可能在两个路径点之间来回跳变。这个跳变会导致 e_y 和 e_psi 突然变化，控制器误以为出现巨大误差，猛打方向，车身就开始抖。
解决思路有两个层次。第一个层次，在做最近点匹配时，不是全路径搜索，而是从上一时刻最近点索引的邻域开始搜，相当于给匹配过程加了一个“记忆”，路径连续性天然被保证了。第二个层次，如果路径本身曲率过大，说明这条参考路径本身就不平滑，需要在上游规划层做平滑处理，或者用参考路径的曲率前馈补偿，减少控制器对误差反馈的依赖。
6.3 从仿真到实车的三个注意点
仿真跑通了，离实车还差几步。第一个注意点是执行器延迟。仿真里我们算完delta立刻生效，但实车上转向执行器有延迟，这个延迟如果大于控制周期的1/4，控制效果会有明显退化。简单处理办法是做个纯延迟补偿，也就是用上一帧的delta预测当前实际角度，再和LQR输出做加权融合。
第二个注意点是模型参数失配。仿真里的轴距L是固定值，但实车上轮胎气压、悬架变形都会影响等效轴距。所以实车标定时，K矩阵不能只算一次，要至少用两种车速、两种曲率路径去标定，验证不同工况下K的稳定性。
第三个注意点是积分项。纯LQR是状态反馈，没有积分作用，如果系统存在常值扰动（比如侧风、方向盘零漂），会留下稳态误差。工程上普遍的做法是LQR + 前馈 + 积分补偿的组合，前馈项用参考路径曲率算出稳态转角，积分项补偿残余误差。
在我自己调过的项目里，印象最深的一次就是只靠LQR跟踪一条大曲率回头弯，车辆在弯心附近持续偏差约0.3米，后来加了曲率前馈，误差直接降到0.05米以内。这个经验让我深刻理解了一件事：LQR负责“动态最优”，前馈负责“静态补偿”，两者是搭档，不是替代关系。
如果你接着往下做，下一篇博客准备聊聊LQR和MPC在自动驾驶里的实际分工，以及线性时变LQR在轨迹跟踪里的工程变形。到时候我会给出更贴近实车方案的代码和标定流程。
