如果只选一个自动驾驶横向控制里最经典、最容易被问到的算法LQR一定排前三。很多做规划控制的朋友入行的第一个落地方案就是LQR它不像MPC那样吃算力也不像PID那样全靠试凑它在“最优性”和“工程可实现性”之间踩在了一个非常舒服的位置上。这篇文章就把LQR的原理、公式推导和代码实现串一遍我会尽量按我们平时做实际项目的思路来讲而不是照搬教材。1. LQR解决什么问题从一条虚线说起1.1 自动驾驶横向控制中的核心需求自动驾驶车辆在道路上行驶路径规划模块会给出目标参考线——一条由离散点组成的中心线。控制模块的任务就是让车辆的实际行驶轨迹无限接近这条参考线。这听起来简单但实际做起来牵扯到两个层面一是车辆本身是一个非线性、强耦合的系统二是控制目标需要同时兼顾“跟得准”和“跑得稳”。跟得准意思是横向偏差和航向偏差都要尽量小。跑得稳意思是方向盘不能来回猛打否则车上的人会晕车执行器也受不了。这两个目标在数学上是矛盾的偏差要小控制动作就得大控制动作一大舒适性和稳定性就下降。任何横向控制算法本质上都是在平衡这对矛盾。PID把偏差按比例、积分、微分压制下去但参数的物理意义不直观而且很难做到多目标权衡。MPC能显式处理约束但计算量大车上算力紧张的时候容易掉链子。LQR的定位刚好卡在中间它基于状态空间模型通过最小化一个带权重的二次型代价函数来设计反馈控制器既能处理多变量耦合又有扎实的最优性保证而且计算量小到可以跑在微控制器上。1.2 为什么“线性”和“二次型”这两个词这么关键LQR的全称是Linear Quadratic Regulator线性二次型调节器。线性指的是被控对象模型用线性状态方程描述二次型指的是代价函数是状态和控制输入的二次型函数。这两个限定条件看着局限实际上在工程中非常好用。线性模型的优势在于计算效率极高。围绕工作点做泰勒展开把非线性的车辆动力学在当前状态附近近似成线性方程控制器设计就能用成熟的线性系统理论反馈矩阵K的求解变成一个代数问题——解Riccati方程不需要在线优化。相比之下MPC每个周期都要在线求解一个带约束的优化问题对算力要求高一个量级。二次型的代价函数也有讲究。二次型函数是凸函数全局最优解一定存在且唯一这让“最优”这件事变得可计算。而且二次型的权重矩阵Q和R里每个元素都有明确的物理含义“这个状态偏差我给多高的权重”“控制量我抠门多少”工程师可以直接通过调Q和R来改变控制行为这也给了LQR极强的工程实用性。1.3 这篇适合谁看如果你刚开始接触自动驾驶控制正在纠结学LQR还是学MPC我建议先把LQR吃透。它适合这几类读者做横向控制但还不清楚状态空间模型怎么建立的在调LQR时只知道套公式、不懂Riccati方程是怎么来的需要部署一个快速可行的路径跟踪控制器到实车或嵌入式平台上的。这篇我会把原理、公式、代码演示三块都覆盖到代码是Python写的结构也很简单方便直接改造成自己的仿真模块。2. 车辆动力学与状态空间建模LQR的地基2.1 用“自行车模型”来描述车辆实车上做横向控制99%的团队起步都会用自行车模型也叫二自由度模型。它把车辆简化成一个前轮一个后轮的刚性体忽略侧倾、俯仰和载荷转移只关心横摆运动和侧向运动。这个模型在侧向加速度不超过0.4g的场景下精度够用城市道路和高速巡航都满足这个条件。两个自由度分别是横向速度v_y和横摆角速度r。再加上车辆在大地坐标系下的横向位置偏差和航向偏差就构成了横向控制的状态量。工程上最常用的做法是把误差作为状态量而不是用绝对位置。为什么因为控制目标就是消除相对于参考线的偏差用误差状态可以直接把参考轨迹的影响融入模型。2.2 误差动力学方程的推导参考线在车辆前方每个距离上都有参考曲率κ_ref以及对应的目标横向加速度a_y,ref和参考横摆角速度r_ref。把车辆动力学方程写成误差形式会得到如下线性化后的模型x_dot A x B u E d这里的x是我们的状态向量通常取 [e_y, e_psi, e_psi_dot, e_y_dot] 或者 [e_y, e_psi, v_y, r] 这种形式。u是前轮转角或者方向盘转角d代表曲率干扰项。不同资料里状态定义略有差别关键是状态量的物理意义要清楚。拿取[e_y, e_psi, v_y, r]这组状态为例e_y是车辆质心到参考线的横向偏差e_psi是车辆航向与参考线航向的夹角偏差。线性化后的A矩阵里有几个元素是车速的显式函数所以LQR的反馈增益K算出来之后实际工程中往往是做成“车速-增益”查表的形式车速变就切换增益这也是LQR在实车上落地最常见的做法。2.3 为什么要做线性化线性化误差怎么处理LQR要求系统是线性的但车辆动力学天生非线性。实际的做法是在每个工作点主要是当前车速附近做泰勒展开忽略高阶项得到线性模型。车速越高非线性越强线性化误差越大。所以高速工况下要么提高建模精度把轮胎侧偏刚度随载荷变化的因素也建模进去要么用更保守的权重设计让控制器别那么激进。还有一个工程细节线性化误差并不会让系统崩溃因为LQR本身就是闭环控制器反馈本身就在不断修正模型失配带来的偏差。哪怕模型不准只要输入方向对系统也能稳定下来只是控制品质会打折扣。这也是为什么LQR在工程上被大量使用的原因——它不完美但足够皮实。3. 代价函数与Riccati方程LQR公式是怎么来的3.1 代价函数里的每一项在表达什么LQR的代价函数写作J ∫(x^T Q x u^T R u) dt这里x^T Q x把所有状态偏差的加权平方和累加起来u^T R u把控制量的加权平方和累加起来。Q是半正定矩阵R是正定矩阵。Q的取值决定系统对“状态偏离”的容忍度R的取值决定系统对“控制能耗”的敏感度。举个例子比如把Q中对横向偏差e_y的权重设得很大那么控制器会拼命把车拉回参考线横向偏差迅速收敛代价是方向盘会更猛。R设得大控制器就不愿意打方向盘偏差收敛会变慢但转向更平顺。这个权衡关系在调参时是核心中的核心。二次型还有两个隐含属性一偏差越大惩罚增长越快所以控制器天然带有“先猛后缓”的特性二正定的R保证了代价函数对u是严格凸的最优解存在且唯一。这些性质保证了LQR不会出现“目标打架”导致的振荡或者无解。3.2 最优条件从Hamilton函数到Riccati方程求解LQR的核心思路是用变分法把约束最优化问题转化为无约束问题。构造Hamilton函数H x^T Q x u^T R u λ^T (A x B u)其中λ是协状态向量也叫拉格朗日乘子。最优性条件要求对u求偏导等于零∂H/∂u 2R u B^T λ 0得到u -0.5 R^{-1} B^T λ同时λ满足协状态方程λ_dot -∂H/∂x -2Qx - A^T λ。最优控制里有一个经典假设λ 2P x其中P是一个对称正定矩阵。代入协状态方程和状态方程经过一番代数化简就能得到连续时间Riccati代数方程A^T P P A - P B R^{-1} B^T P Q 0解出P之后反馈增益就是K R^{-1} B^T P最终的控制器形式是u -K x。整个过程推导下来核心就是找一个合适的P矩阵让闭环系统稳定同时代价函数取最小值。3.3 连续系统与离散系统的差别实际写代码用的是离散形式。离散LQR的代价函数写成求和形式J Σ(x_k^T Q x_k u_k^T R u_k)对应的是离散Riccati方程P A^T P A - A^T P B (R B^T P B)^{-1} B^T P A Q这个方程在代码里直接用迭代法求解每轮把P代入右侧算一次直到P收敛。也可以用Python的scipy库直接调solve_discrete_are一步到位。用库函数方便但建议初学者自己手写一遍迭代过程对理解P矩阵的收敛特性非常有帮助。3.4 稳定性保证为什么闭环一定稳定LQR一个最优秀的性质是只要Q是半正定、R是正定解出来的反馈阵K一定能保证闭环系统A-BK的特征值都在复平面左半平面也就是闭环稳定。这个结论有严格的数学证明。工程上不用关心完整证明但要记住这个性质的成立条件模型A、B必须与实际系统匹配否则稳定性保证就不存在了。这也是LQR调参时“模型越准控制越稳”的原因所在。4. Python代码演示实现一个车辆横向LQR控制器4.1 仿真场景设计下面这段代码演示一个简单的车辆横向跟踪场景。车辆沿着一条直线参考路径行驶初始时刻有0.5米的横向偏差和3度的航向偏差LQR控制器的任务是让车辆尽快回到参考线上并保持稳定。我用了一个简化的线性车辆模型状态量x [e_y, e_psi, v_y, r]控制量u 前轮转角δ车速v_x 10 m/s36 km/h固定不变车辆参数质量m1500kg质心到前轴距离lf1.2m质心到后轴距离lr1.5m前轮侧偏刚度Cf80000N/rad后轮侧偏刚度Cr80000N/rad这是非常典型的参数组合跑起来效果直观。4.2 完整代码与逐段注释import numpy as np from scipy.linalg import solve_discrete_are import matplotlib.pyplot as plt # 车辆参数 m 1500.0 # 质量kg lf 1.2 # 质心到前轴距离m lr 1.5 # 质心到后轴距离m Cf 80000.0 # 前轮侧偏刚度N/rad Cr 80000.0 # 后轮侧偏刚度N/rad vx 10.0 # 纵向车速m/s (固定) dt 0.02 # 控制周期s # 状态矩阵 A对应状态 [ey, epsi, vy, r] A np.array([ [0.0, vx, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0], [0.0, -(Cf Cr) / (m * vx), -(Cf Cr) / m, (lf * Cf - lr * Cr) / (m * vx)], [0.0, -(lf * Cf - lr * Cr) / (m * vx), -(lf * Cf - lr * Cr) / m, -(lf**2 * Cf lr**2 * Cr) / (m * vx)] ]) # 控制矩阵 B B np.array([ [0.0], [0.0], [Cf / m], [lf * Cf / m] ]) # 离散化零阶保持法 A_d np.eye(4) A * dt B_d B * dt # 权重矩阵 Q 和 R Q np.diag([10.0, 5.0, 1.0, 0.1]) # 状态惩罚横向偏差、航向偏差、侧向速度、横摆角速度 R np.array([[1.0]]) # 控制量惩罚前轮转角 # 求解离散Riccati方程 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) print(K矩阵:, K) # 仿真初始状态 x np.array([0.5, 3.0 * np.pi / 180.0, 0.0, 0.0]) # 初始横向偏差0.5m航向偏差3度 time_total 5.0 steps int(time_total / dt) history [] for i in range(steps): u -K x # LQR控制律 u np.clip(u, -0.5, 0.5) # 前轮转角限幅约±28.6度 x A_d x B_d u[0] # 状态更新 history.append([x[0], x[1], u[0]]) history np.array(history) t np.arange(steps) * dt # 作图 plt.figure(figsize(10, 6)) plt.subplot(3, 1, 1) plt.plot(t, history[:, 0], labeley) plt.legend() plt.ylabel(横向偏差 (m)) plt.subplot(3, 1, 2) plt.plot(t, history[:, 1], labelepsi) plt.legend() plt.ylabel(航向偏差 (rad)) plt.subplot(3, 1, 3) plt.plot(t, history[:, 2], labelsteering) plt.legend() plt.ylabel(前轮转角 (rad)) plt.xlabel(时间 (s)) plt.tight_layout() plt.show()4.3 仿真结果分析跑完这段代码你会看到横向偏差在1秒附近就能收敛到0附近航向偏差也是类似趋势。前轮转角最开始的输出比较大大概在0.1到0.2弧度之间也就是五六度到十几度随后迅速衰减到0。整个过程没有振荡没有超调这是LQR调参调得比较温和时的典型表现。如果把Q的横向偏差权重从10调到100你会看到两个变化第一个是偏差收敛速度明显变快但是前轮转角的峰值会变大第二个是如果权重拉得过高可能会出现轻微的超调因为系统在追求“极速归零”时会产生惯性冲过头。这是所有带积分效果的控制器都会遇到的问题。4.4 从仿真到实车部署的差距在哪里仿真代码跑通只是第一步。实车部署时至少要多考虑四件事一是模型参数的辨识轮胎侧偏刚度C_f、C_r在不同路面、不同胎压下变化很大需要配合车辆标定数据反复修二是执行器的延迟和饱和前轮转角指令发出去到实际转角跟上中间有延迟延迟过大会让LQR反馈变成“迟到的修正”导致振荡三是传感器噪声横向偏差来自视觉或GPS融合噪声会造成方向盘抖动需要加滤波四是车速变化仿真里vx固定实车车速一直在变工程上会预先离线算好不同车速对应的K矩阵在线查表切换。5. 调Q和R的实战技巧这些坑我都替你踩过5.1 Q矩阵里的元素不是拍脑袋定的很多初学者拿到LQR就顺手在Q矩阵里填1和10跑出来效果不对就说LQR不行。其实Q矩阵每个元素的量纲和物理意义都需要斟酌。横向偏差的单位是米航向偏差的单位是弧度侧向速度单位是米每秒横摆角速度单位是弧度每秒。这些状态量的数值范围差别很大比如正常行驶中横向偏差可能只有几厘米到几十厘米而航向偏差可能只有零点几度到几度。如果不做尺度归一化Q矩阵的权重数值就会直接被量纲差异淹没。我的习惯是先用一组标称值做归一化比如把每个状态量除以典型最大值再乘以一个基础权重得到Q。如果某个状态在项目里是重点监控对象比如车道保持里横向偏差最重要就在归一化的基础上额外放大。这样可以保证Q矩阵的权重真正反映“重视程度”而不是被量纲带偏。5.2 R矩阵不是越小越好R是控制输入的权重R越小代表控制器对控制量的惩罚越小也就越舍得打方向盘。很多调参新手以为R设小一点跟踪性能就强一点实测下来会发现R过小时方向盘会出现高频抖动尤其在传感器噪声比较大的场景里这种抖动会被噪声放大直接拉低乘坐舒适性。我在实际项目里的规律是先给一个中等R值比如控制量归一化后的量级跑通闭环确认稳定性再逐步降低R观察性能变化。R每次缩小一半左右直到出现抖动信号就开始回退取前一组稳定值。这个方法简单但很管用。5.3 权重调整的直观方法看仿真曲线而不是看数字LQR的调参本质上是多目标权衡眼睛盯着Q和R的数字很难看出门道。我推荐的流程是先把状态量初始偏差设成一个典型极端值比如0.5米横向偏差3度航向偏差跑仿真画出横向偏差、航向偏差、控制量三条曲线。然后问自己三个问题偏差收敛时间能不能接受控制量峰值有没有超过执行器限幅曲线有没有振荡抖动。根据这三个答案去调Q和R每次只动一个参数对比前后两条曲线。这套流程看起来很笨但实际效率比“凭感觉改一堆权重”高得多。因为LQR是线性系统参数和响应之间有明显的单调性横向偏差权重加大横向偏差收敛变快控制量变大控制权重加大控制量变柔和偏差收敛变慢。利用这个单调性两三轮就能调出合理表现。5.4 常见问题速查表现象原因处理方式横向偏差收敛太慢Q中ey权重偏低增大Q[0,0]或降低R方向盘高频抖动R过小或传感器噪声大增大R加低通滤波大偏差时控制饱和执行器限幅导致增益不够降低偏差权重避免激进的响应需求高速工况不稳定线性化误差大、增益未切换按车速分段标定K增加低速阻尼稳态偏差不为零模型误差或未加入积分项补充前馈控制量或者引入误差积分扩展状态噪声造成转向抖动状态估计噪声大调估计算法参数不急着动Q/R6. 我看LQR在自动驾驶中的地位做了几年控制我个人的体会是LQR的“最优性”在实车上的价值其实不如它的“可预测性”来得大。因为车辆模型本身并不精确轮胎、路面、载重变化都会带来模型失配严格意义上的最优是谈不上的。但LQR把这个权衡过程结构化、参数化了让工程师能通过两个矩阵清晰地把控制行为表达出来这在实际工程里比“最优”两个字实在得多。很多做规划控制的朋友一开始就扑向MPC结果被约束优化、预测时域、求解器这些概念劝退。我建议先把LQR这个基础打牢——它的状态空间建模思想、代价函数设计思路、参数调优方法和MPC是相通的。搞清楚LQR再去看MPC会顺手很多。下一篇如果大家有兴趣我会拆一拆MPC和LQR在工程上的选用边界以及如何把LQR做成带前馈的LQR去处理稳态误差问题。