这两周一直在调一台基于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主要都在“跟踪控制”这一层发挥作用但两者计算量和性能边界差别很大。维度PIDLQRMPC模型利用不用模型需要线性模型需要模型可支持非线性约束处理不支持不支持或后期加限制原生支持计算量极小小矩阵求解即可大需在线优化最优性无局部最优无限时域有限时域最优典型场景简单跟随、底层稳速横向控制、姿态控制复杂避障、轨迹规划我的建议是如果约束不复杂、模型可以用线性微分方程近似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, L2.5, dt0.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, vNone): 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], axis1)主循环里每一帧都要做一件关键的事找参考路径上离车辆当前位置最近的点。这个操作叫“最近点匹配”。有了最近点才能计算横向误差 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_iter10000, tol1e-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·dtB_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(LL, dtdt) 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--, labelreference) plt.plot(x_history, y_history, r-, labelLQR tracking) plt.legend() plt.grid(True) plt.axis(equal) plt.show() run_simulation()跑出来的结果能明显看到车辆从初始偏差位置逐渐收敛到参考路径之后全程贴线行驶没有超调振荡也没有稳态误差。这就是LQR效果的直观体验。6. 调参、坑点与工程化建议6.1 不同Q/R参数的响应对比很多人拿到代码之后第一件事就是乱调Q和R发现效果不对就怀疑算法出了问题。其实LQR的参数调节有明确的方向我贴三组典型实验对比你看完就懂了。参数设置实际表现决策建议Qdiag(1,1), R1响应适中无超调稳态误差小初级项目推荐Qdiag(50,1), R1横向偏差快速归零但航向角容易振荡需要快速纠偏时用Qdiag(1,1), R10方向盘动作平缓但弯道跟踪滞后明显舒适性优先场景第一组参数是很好的起步点。如果你发现车贴着线来回晃多半是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在轨迹跟踪里的工程变形。到时候我会给出更贴近实车方案的代码和标定流程。