LQR在自动驾驶横向控制中的实践：车辆动力学建模与轨迹跟踪

做了一段时间的自动驾驶控制，尤其是横向运动控制这一块，我自己的体会是：LQR（线性二次型调节器）是性价比极高的一种方案，特别适合作为轨迹跟踪控制的入门和工程落地的首选算法。不是说它一定比MPC（模型预测控制）更强，而是在“理解成本低、计算量小、效果可控”这三件事上，LQR做到了一个非常难得的平衡。项目标题里提到的“质心侧偏角、横摆角速度、横向误差”，这三者结合起来，本质上是把车辆看作一个带动力学特性的系统，而不是单纯的地理点导航。这里面的讲究，值得拆开来细聊。

1. 项目整体拆解：为什么选LQR做轨迹跟踪

1.1 核心需求与状态变量的物理映射

要做轨迹跟踪，第一件事是搞清楚“跟踪”到底跟踪的是什么。很多新手入门时，习惯性地只看横向位置误差，也就是常说的lateral error——目标路径和车辆当前位置的垂直距离。但真正落到控制上，只压这一个误差是不够的。车辆是一个有质量的刚体，它有侧滑、有横摆，尤其是在中高速过弯的时候，车的姿态和朝向往往是超前的，横向位置没到，横摆角已经转过去了。这个时候再去只调横向误差，你会觉得“明明误差看着还好，但车就是在画龙，或者进入弯道的时候总感觉车身横着进”。

项目标题中明确出现的质心侧偏角（Sideslip Angle，记作β）和横摆角速度（Yaw Rate，记作r），就是用来描述车辆这个动力学状态的。我把这套系统的状态变量梳理成下面这几个维度：

• 质心侧偏角（β）：车辆实际速度方向与车身纵向轴线之间的夹角。这反映了车辆整体有没有在“漂”，是描述侧向滑动的核心物理量。
• 横摆角速度（r）：车身绕垂直轴转动的角速度。它直接反映了车头朝向的变化快慢，也就是通常说的“车是不是在扭头”。
• 横向误差（e_y）：车辆质心到参考路径的垂直距离。这是我们最关心的一个控制指标，日常驾驶里说的“压线了没有”，对应的大致就是它。
• 航向误差（e_psi，常常写作e_φ）：车辆实际航向角与参考路径切线方向之间的差值。这个量决定了车辆行进方向与路径期望方向的偏转程度，也是不能省的核心状态。

为什么要把β和r拉进来？很简单，因为它们的动力学特性决定了横向误差将来的走向。我常打一个比方：如果说横向误差是“你现在离终点线还有多远”，那横摆角速度和质心侧偏角就是“你当前跑步的节奏和重心向哪边偏”。只控制距离，不控制节奏和重心，跑姿肯定出问题。这其实也就是为什么LQR相比纯PID或者纯几何追踪（如纯追踪Pure Pursuit算法）在高速场景下更稳的原因——它本质上是建立了一个状态空间的预测视图，把车辆运动趋势和当前几何偏差联合起来做代价优化。

1.2 方案选型对比：LQR、PID、MPC三者如何取舍

很多人在选控制器的时候会犹豫，动不动就想着上MPC。我做了几次项目对比之后，心里其实形成了非常清晰的坐标轴来判断选型：

表中的结论已经很清晰了：在不考虑轮胎非线性进入极限工况的前提下，LQR是在性能和实现复杂度之间最优的折中。MPC当然更先进，但每次计算一个小型QP问题对嵌入式硬件有门槛，而且它的调参自由度更爆炸，新手一个不小心就把预测时域里的约束配得互相打架。我个人觉得，LQR就是一套“先建模型、再算反馈增益、然后把状态反馈回去”的严谨控制流程，它给后续切换MPC打了一个特别好的底子。

1.3 线性模型的适用边界与参数计算思路

既然LQR的设计前提是线性模型，那二自由度单车模型（Bicycle Model）就是目前工程上用得最多的建模范式。这个模型把车辆前后轮合并成一条轴上的两个等效轮胎，忽略悬架侧倾、俯仰和空气动力学效应，只保留侧向和横摆的自由度。它是一个线性时不变系统，前提是：轮胎工作在线性区（侧偏角在约正负4度以内，极限也尽量不超过8度），且纵向速度恒定。实际项目里车速不会是恒定的，但只要它在一个小区间内波动，就可以为每5km/h或者10km/h做一组LQR增益表，运行时按车速插值查表。这也就是通常说的增益调度（Gain Scheduling）。

我记得在第一次调这个模型时，用MATLAB里的lqr(A, B, Q, R)函数，一个矩阵输进去，三行代码就出解，但是矩阵为什么那样搭建，每个元素是怎么推导出来的，很多人其实还处于“会用但不会推”的状态。而这一点恰恰会在项目边界延展时卡住你。比如你要引入一个前馈补偿，就得对稳态输入做公式推导；你要控制起步阶段的大误差，又需要做积分增广（LQI），那些本质都是从运动学方程和误差动力学模型推下来的。所以下面这部分，我要把建模这条线完整走一遍，把矩阵每一项的来源讲清楚。

2. 车辆动力学建模：从牛顿方程到状态空间表达式

2.1 二自由度车辆模型核心推导

先约定车辆参数记号。质心到前轴的距离记作L_f，质心到后轴的距离记作L_r，车辆质量m，绕垂直轴的转动惯量I_z，前轮等效侧偏刚度C_f，后轮等效侧偏刚度C_r。这里的侧偏刚度本质上是轮胎侧向力和侧偏角的比值，量纲是 N/rad，反映轮胎抵抗侧向滑移的能力。

二自由度模型的输入是前轮转角δ，状态自然是质心侧偏角 β 和横摆角速度 r。把牛顿第二定律写出来：

• 横向力平衡：m * V * (dβ/dt + r) = F_yf + F_yr
• 横摆力矩平衡：I_z * dr/dt = L_f * F_yf - L_r * F_yr

其中，前轮侧向力 F_yf、后轮 F_yr 在线性模型里分别写成：

• F_yf = C_f * (δ - (β + L_f * r / V))
• F_yr = C_r * (-(β - L_r * r / V))

这个公式里最关键的一项就是前后轮的轮胎侧偏角。前轮侧偏角等于前轮转角减去前轮处侧向速度与纵向速度的比值，后轮同理但不含转向角。很多人在建模时容易把符号方向弄错，导致后面矩阵搭建出来系统零点不对。我当时调试时用了一个技巧：让车辆直线行驶但给一个纯横摆角速度扰动，然后观察模型响应是否与理论值一致。如果方向反了，矩阵里的元素符号立刻暴露出来。

把上面两个平衡方程展开并写成矩阵形式：

d/dt [β] = [A11 A12] [β] + [B1] δ [r] [A21 A22] [r] [B2]

其中：

• A11 = -(C_f + C_r) / (m * V)
• A12 = -1 - (L_f * C_f - L_r * C_r) / (m * V^2)
• A21 = -(L_f * C_f - L_r * C_r) / I_z
• A22 = -(L_f^2 * C_f + L_r^2 * C_r) / (I_z * V)
• B1 = C_f / (m * V)
• B2 = L_f * C_f / I_z

这个矩阵就是车辆本身的固有动力学矩阵A。它反映了车辆在当前车速下的稳定程度。比如你算一下特征值时会发现，随着V增大，特征值会越来越靠近虚轴甚至跑到右半平面，这对应的物理直觉就是：高速下车辆本身的横摆响应会更迟钝、更容易失稳，所以控制器必须介入。

2.2 误差动力学模型：从“车自身体态”到“路径偏差”

但仅仅有β和r还不够，LQR要跟踪路径，必须把横向误差和航向误差也纳入状态。这里需要推导误差动力学。

假设参考路径的曲率为κ（kappa，即路径弯曲程度），参考航向角为φ_des，参考横摆角速度为 r_des = V * κ。定义误差状态：

• e_1 = e_y，横向位置偏差
• e_2 = e_ψ = φ - φ_des，航向角偏差

那么：

• de_y/dt = V * (β + e_ψ)。值得停下来展开说说这个式子。横向误差的变化率由两部分决定：车辆本身速度方向偏离车身轴线的β，以及车身轴线偏离参考切线方向的e_ψ。两者加起来，就是车辆实际运动速度方向相对参考路径方向的夹角。换句话说，车是不是在靠近路径，完全由β和e_ψ这两个角度叠加后乘车速决定。这是整个误差模型里最直观也最容易记的一条关系式，建议在做任何控制器之前先画一画这个几何关系图。

• de_ψ/dt = r - V * κ。这个表示航向角偏差的变化率等于实际横摆角速度减去参考横摆角速度。在跟踪一段曲率不变的圆弧时，参考项是恒定值，所以本质上是在求横摆角速度的跟踪误差。

这两个误差量加进来之后，把β、r、e_y、e_ψ四维状态整合起来，可以得到完整的增广状态方程。系统的控制输入还是前轮转角δ，但扰动项则是参考曲率κ。写成状态空间就是：

d/dt [ e_ψ ] = A_lat * [ e_ψ ] + B_lat * δ + E_lat * κ [ e_y ] [ e_y ] [ β ] [ β ] [ r ] [ r ]

不过，需要审慎说明的是：状态排列顺序会直接影响Q矩阵的物理含义。我上手时习惯把状态排列为x = [e_y, e_ψ, β, r]^T，因为这样控制指标横向误差在最前面，调Q矩阵时读起来最直觉。

如果忽略曲率扰动项（即假设直道跟踪），状态矩阵可以直接从前面2.1和2.2的式子拼接出来。这里有一段经常在各类开源代码里出现的矩阵组合形式，用Python的numpy表达就像下面这样。实际项目中，我用这套代码在Simulink里搭过对应的S-Function，也用Python环境跑纯数字仿真对比过，效果一致。

import numpy as np # 车辆参数 m = 1412.0 # 质量 kg I_z = 1536.0 # 横摆转动惯量 kg.m^2 L_f = 1.2 # 质心到前轴 m L_r = 1.4 # 质心到后轴 m C_f = 55000.0 # 前轮侧偏刚度 N/rad C_r = 58000.0 # 后轮侧偏刚度 N/rad Vx = 20.0 # 纵向车速 m/s (72km/h) # 车辆固有动力学矩阵 A_11 = -(C_f + C_r) / (m * Vx) A_12 = -1.0 - (L_f * C_f - L_r * C_r) / (m * Vx**2) A_21 = -(L_f * C_f - L_r * C_r) / I_z A_22 = -(L_f**2 * C_f + L_r**2 * C_r) / (I_z * Vx) B_11 = C_f / (m * Vx) B_21 = L_f * C_f / I_z # 误差动力学增广状态: x = [e_y, e_psi, beta, r]^T A_lat = np.array([ [0.0, Vx, Vx, 0.0], [0.0, 0.0, 0.0, 1.0], [0.0, 0.0, A_11, A_12], [0.0, 0.0, A_21, A_22] ]) B_lat = np.array([[0.0], [0.0], [B_11], [B_21]]) # 验证矩阵维度和秩 print("A_lat shape:", A_lat.shape) print("B_lat shape:", B_lat.shape)

这段代码里有一个值得注意的细节：A_lat第一行第一列是0，第一行第二列是Vx，第一行第三列是Vx。为什么第三列也是Vx？因为推导de_y/dt时，横向误差导数里含β项（前一节说过的V * β）。所以β列会贡献Vx的系数到横向误差变化率。第二行则对应航向误差导数等于横摆角速度（如果忽略参考曲率），所以只有r所在列是1.0。矩阵前半部分看起来像个梯形结构，本质上是几何误差状态和车辆运动状态的耦合关系，物理意义非常清晰。很多学生在手动推导时喜欢把β和r放在前面，事实上那样会让矩阵上三角区域多出交叉项，后续调试时看状态轨迹转化不如这样直观。我在实际中更倾向于上面的排列方式。

2.3 模型参数扰动的影响分析

建好了模型，另一个不能忽视的问题是参数不确定性。轮胎侧偏刚度C_f、C_r会因为路面附着系数、轮胎磨损、胎压变化发生明显变化，最高偏差可以达到20%-30%。质量m也会随着载重浮动。这些偏差会直接影响A矩阵中A11、A12、A21、A22的值。

我之前做过一次仿真扫描，把C_f、C_r同时乘以0.7（模拟低附着路面），来看LQR闭环系统的极点移动情况。结果显示，控制器虽然还能稳住，但阻尼比明显下降，超调量变大。如果车速再高一点，就可能进入振荡甚至失稳状态。这事给我的启发有两个：一是LQR不能盲目地只在“标称参数”下调一遍；二是在实际工程中，至少要把增益调度表的车速分辨率加密一些，同时给横向误差状态加上饱和限幅来提升鲁棒性。还有一点，就是在做一些前瞻性设计时，可以给侧偏刚度留出余量，比如把设计用的C_f取得略微小一点（相当于认为路面粘附变差），这样算出来的控制器会更保守一些，实际表现通常会更稳。这算是“设计鲁棒性”的一个小经验。

3. LQR控制器设计：从代价函数到代码实现

3.1 代价函数的物理语义与矩阵设定

LQR的思想是：找一个状态反馈控制律u = -K * x，使得下面这个无穷时域代价函数最小：

J = ∫( x^T * Q * x + u^T * R * u ) dt

把这个公式翻译成人话：系统在运行过程中，既希望状态偏差尽量小，又希望控制量不要太大。Q矩阵对角线上每个元素，就是对应的那个状态偏差平方之后乘以的权值。R矩阵则是控制输入u的权值。权重越大，代表该项的“成本”越高，最优控制器就会越努力去抑制它。

拿我们这套系统举例：

• Q[0,0] 对应横向误差e_y的权重。如果把它设得很大，控制器就会极其激进地把车拉回路径中心，倾向用比较大的前轮转角快速消差。
• Q[1,1] 对应航向误差e_ψ。权重偏大时，控制器会更主动地纠正车头朝向，但可能引起转向响应过急。
• Q[2,2] 对应质心侧偏角β。这个权重通常不宜太小，否则车会一直带着侧滑角“斜着走”。尤其在冰雪路面，β权重太小会累积出很大的车身摆动量。
• Q[3,3] 对应横摆角速度r。它抑制的是车辆自身横摆的剧烈变化，权重适度大一点可以让控制输出更平滑，减少转向抖振。
• R对应前轮转角δ的惩罚。R越大，意味着转向执行器动作越保守，响应会变慢。

根据带过项目的经验，我给出一个推荐的权重设置逻辑：

• 第一步，先把横向误差的权重设为1.0，其他状态权重设为0.1左右，R设为0.1。这是初始起点，保证系统先有基本的跟踪能力。
• 第二步，观察仿真中的横向误差绝对值。如果超过0.3米，则逐步把Q[0,0]往上调，调到能压到0.1米以内为止。
• 第三步，观察控制输入的抖振程度。如果前轮转角变化率过大，增大R，或者增大Q[3,3]来平滑横摆角速度。
• 第四步，优先保证误差性能，其次保证控制平滑度，不要一开始就把R搞得特别大，否则系统会“佛系”到根本不跟随路径。

一个当前验证有效的具体中间数值（标定基准、仅供参考）是：

Q = diag([2.0, 1.0, 0.5, 0.5]) R = [1.0] K = lqr(A_lat, B_lat, Q, R)

注意，实际使用中需要对Q和R做归一化处理，更好的方式是使用Bryson规则，即每个变量权重按它的容许最大偏差平方的倒数来取。比如最大容许横向误差0.2米，那权重就是1 / (0.2^2) = 25。这样做的好处是权重之间的相对比例更有物理意义，不会出现“凭感觉调了100组参数也不知道在调什么”的情况。

3.2 两种最常用的Riccati求解途径

LQR的核心在于求解Riccati方程，从而得到反馈增益矩阵K。工程上有两条路线很常见：

• MATLAB路线：直接调用lqr(A, B, Q, R)函数。这个函数会返回反馈增益K，同时还会返回Riccati方程的解P和闭环系统特征根e。网上的教程和论文里，这套代码能直接复现。要注意的是，MATLAB的lqr默认是连续时间设计，在做离散控制时需要用dlqr代替。

• Python/开源路线：用python-control库。安装命令是pip install control，然后这样：

import control as ct K, P, e = ct.lqr(A_lat, B_lat, Q, R) print("K =", K) print("closed-loop eigenvalues =", e)

个人在实际中更倾向用Python来做原型验证，原因是可视化方便，可以在同一个matplotlib图里叠加参考路径、车辆实际轨迹、误差曲线，调试效率高。但要注意，ct.lqr的求解稳定性在平时是没问题的；如果遇到系统维度更高（例如加上积分项变成五阶），建议用control.lqe或直接调用solve_continuous_are自己算Riccati解。

3.3 离散化与采样周期选择

实际嵌入式环境是离散的，所以设计完连续域的K，还需要把系统离散化。常用的离散化方法是零阶保持器（ZOH），也就是把一个采样周期内的控制输入视为恒定值。控制器反馈增益一般在离散化之后重新求解一个离散代数Riccati方程，即用dlqr来计算离散域的K。

这里要特别强调采样周期的影响。我自己遇到过的情况是，采样周期从10ms换到50ms后，原本调好的参数表现出了明显的相位滞后，横向误差的振荡频率明显变化。选择采样周期的依据是系统带宽：LQR闭环系统的穿越频率约在1~3Hz之间，采样频率至少是其10倍，即10~30Hz以上，实际工程中一般取50Hz（20ms）或100Hz（10ms）。如果你在仿真里跑得好好的，放到实车平台上一卡一卡的，首先检查采样频率和计算耗时，不要一上来就怀疑控制器设计有问题。

4. 前馈补偿与稳态误差消除

4.1 曲率干扰带来的固有稳态误差

如果直接用纯状态反馈u = -K * x跟踪带曲率的路径，会有一个令人困惑的现象：在跟踪固定半径圆弧时，横向误差并不收敛到0，而是稳定在一个常值偏置。这并不意外，原因是，参考曲率κ对应的稳态控制输入是一个非零的常值前轮转角，而纯反馈控制必须靠误差信号才能产生这个控制量，于是系统只能在“有误差”和“要控制量”之间达成平衡。在曲率很大的急弯里，这个稳态误差可能达到数十厘米，这是实车可感知的。

4.2 前馈项推导的一点记录

解决思路很简单也很经典：想办法算出一个前馈控制量，让这个量直接对应曲率所需的稳态转角度，剩下反馈控制量只需要负责修正瞬态误差。推导的方法是令状态误差的导数为零，解稳态方程。

我们的系统里其实有两种常见的做法。第一种是在模型里显式加入扰动项E * κ，通过解代数方程得到稳态状态x_ss和稳态输入u_ss，然后定义新的状态偏移量，去掉常值扰动。第二种是直接在反馈控制律后面加前馈项，即u = -K * x + δ_ff，其中δ_ff是曲率κ的函数。

对第二种做法，我实践的版本是：

对模型dx/dt = A_lat * x + B_lat * u + E_lat * κ，令x_dot=0，解出：

• 稳态状态x_ss = -A_lat^{-1} * (B_lat * δ_ss + E_lat * κ)

然后把控制律代进去，要求稳态时误差为零（即x_ss中e_y和e_ψ为0）。当然，这个方程在A_lat可逆且系统能控的条件下可解。更简洁也更容易工程实现的做法是直接利用稳态转向方程：在定曲率转弯中，稳态前轮转角约等于δ_ss = L * κ（L为轴距），但是因为轮胎侧偏柔度存在，还要加上一个速度相关的补偿量：

δ_ff = (L + K_us * Vx^2) * κ

其中K_us是不足转向梯度（Understeer Gradient）。这个公式在教科书上叫作“稳态转向公式”，它与我们模型里的线性轮胎参数有明确对应关系。因为我搭建的二自由度模型把前轮转角直接等效为轮胎转角，所以只需要通过车辆模型参数比例算出这部分，再乘以转向系统的传动比即可折算成方向盘转角。在实际代码里，前馈补偿所带来的横摆角速度跟踪精度提升，是纯反馈所不能及的地方。

值得提醒的是，前馈项的正确性高度依赖模型参数，特别是侧偏刚度的准确性。因为一旦轮胎进入非线性区，稳态转向梯度会随侧向加速度浮动，前馈就会失准。此时不能继续仰仗常值前馈，后续要靠反馈把剩下的误差吃掉，或者切换到MPC这类带约束预测的方案。

4.3 执行器链路里的方向盘转角映射

控制器算出来的前轮转角，并不是车里实际打的方向盘角度。需要通过转向比（舵角传动比）进行换算。比如前轮转角0.04 rad，如果转向传动比为15，那么对应的方向盘转角就是0.6 rad，约合34.4度。同时还要注意ES（电动助力转向）或线控转向系统可能存在小角度磨损死区，幅度在0.5-2度之间。在实际工程中，我会在控制输出的前向链路上加一个死区补偿模块，再叠加一个转向速率限制。这样做下来，控制的平滑度会有明显提升。

5. 仿真搭建与经验：从纯数字到硬件在环

5.1 快速搭建LQR轨迹跟踪仿真环境

常用的搭建方式有三种：

• 纯编程环境（Python + Matplotlib）：适合快速算法验证。把车辆模型作为被控对象的预测模型，控制器把我的LQR状态反馈加前馈，然后把控制量作用进模型，画出轨迹对比。这种环境的好处是任何人都能在没有CarSim等重型工具的背景下复现。
• Simulink + 车辆动力学模型：适合做高保真验证。我在Simulink里搭了一个包含轮胎Fiala模型、悬架简化的14自由度整车模型，与LQR控制器串联。你会发现，LQR用二自由度模型设计的增益，在14自由度整车里因为悬架侧倾动力学额外引入的扰动，会产生一些轻微的高频横摆响应，这时候便需要对Q矩阵做适度鲁棒性调整。
• CarSim / TruckSim联合仿真：适合做与实车接近的动力学测试。它能生成高度可信的侧偏力和横摆力矩，控制器代码只要编译成可与CarSim交换数据的动态链接库，就能验证从传感器输入到指令输出的全链路。

测试场景上，我从实际经验出发，格外推荐这么三个：

1. 双移线工况（ISO 3888-1规避测试标准）：验证高速变道时的瞬态超调和收敛性能。
2. 定曲率圆弧：验证稳态跟踪误差和前馈补偿的准确性。调试时把R值设为较大，看横向误差能不能随速度正确收敛。
3. S形连续变曲率路径：验证前馈和反馈对曲率变化的动态跟随能力，这个场景最容易暴露前馈参数不准的问题。

5.2 仿真向实车迁移中的典型工程坑

在从纯仿真向实车迁移过程中，以下几个坑基本每个人都躲不开：

第一，时间延迟。纯仿真里控制器输出的控制量下一时刻立刻作用在模型上，而实车路径上存在着采集延迟、通信延迟、执行机构响应滞后。LQR对延迟的敏感度较高，因为它依靠状态反馈的即时性。处理办法是在状态反馈前加一个Smith预估器，或者直接在模型中串入一个一阶惯性环节来模拟执行器延迟，然后对延时增广状态重新设计LQR。我试过后者，效果非常稳健。

第二，状态不可直接测量。质心侧偏角β在量产车上很少直接通过传感器测量，因为它需要对车速和横摆角速度做积分融合，而且噪声很大。一种常用的替代方案是用陀螺仪和GPS信号做一个简单的动力学观测器，或者直接采用带遗忘因子的最小二乘估计。若实在不方便做估计，可以把β状态从反馈中暂时去掉，只反馈其余的误差和横摆角速度，然后对剩余系统重新算K——但必须在仿真中验证这个降维后的系统仍然稳定。

第三，噪声驱动的抖振。实测中一旦车速到100km/h以上，横向加速度传感器上会出现明显的高频噪声，这些噪声经过K矩阵放大后会转化成方向盘的抖动甚至蜂鸣噪声。解决办法是Q矩阵里适当提高横摆角速度状态权重、降低对噪声敏感的状态权重，同时在控制输出上加低通滤波。我从实测得到的经验是，100Hz的控制器输出先过一个截止频率约10Hz的一阶低通，转向平滑度会显著改善。

5.3 一套有效的开发调试顺序

为了提高排错的可控性，我自己形成了一套从“模型”到“控制器”再到“实车”的调试顺序：

1. 开环推模型：给定一组前轮转角阶跃输入，观察β、r响应曲线是否符合预期物理趋势；
2. 闭环推模型：用LQR控制，在直道上给一个初始横向误差大偏置，看车能不能平滑归中；
3. 在双移线场景里调整Q和R，记录每条误差曲线的超调和收敛速度；
4. 加入前馈，做圆弧工况验证稳态误差是否降到接近零；
5. 加入噪声模型与执行器限幅，观察控制量波动；
6. 再到CarSim等平台做高保真验证；
7. 最后实车，先在封闭场地的低车速（20km/h）双移线试跑，再逐渐提速。

这套顺序里的雷区在于：不要一上来就在仿真里追求0误差。把Q矩阵压得过狠，会在真实的执行器限幅和延迟下变成剧烈振荡，一旦搬到实车，你看着方向盘左右来回猛打，会比调试误差本身更让人头疼。

6. 常见问题与防范技巧

6.1 系统发散与震荡：最先查的地方

如果仿真里一闭环就发散，优先检查事情按以下顺序来排：

1. 状态矩阵符号错误：尤其是我前面反复强调的A12和B矩阵里的符号。可以把矩阵特征值打出来，车辆开环固有动力学特征值在正常车速下应有负实部，如果出现明显正实部，多半是方程里的正负号搞错了。
2. 能控性检查：用ct.ctrb(A_lat, B_lat)矩阵，算一下能控性矩阵的秩是否等于状态维度。如果不满秩，说明系统有不可控模态，LQR没法全状态反馈。
3. 采样周期过大：离散化后，如果采样周期大于系统主导时间常数的1/5，离散后的控制效果会和连续设计相差很远，表现为闭环振荡，此时调小采样周期或改用dlqr重新求解。
4. Q矩阵未正定：常有人在调试时把某个权重设为0，理论上非负定也是可以的，但实际操作中某权重为0很容易导致对那个状态完全无约束，从而出现不可预期的漂移。建议至少保留一个非常小的正数，比如1e-6。

6.2 总是偏向道路外侧或内侧：优先检查前馈与标定

如果在固定曲率弯道中，横向误差稳定后总是偏向道路外侧（即车辆切弯不足），最常见的原因是前馈项太小。这时候加大K_us或者按实际侧偏刚度重新算一遍前馈增益。相反地，如果总是偏向内侧，说明前馈过大。在实车上，还有一层传感器安装角度和零偏的影响：横向位置传感器有零点漂移时，直道上都会出现恒定侧偏，需要在标定程序里把传感器零点校准掉。

6.3 高速下效果下降：增益调度怎么落地

单纯用一组K在40km/h和120km/h下工作，效果一定是吊打的。车速越高，动力学模型中的速度相关性越强，因此标准做法是按速度分组调度。最简单形式如下：

这张表只是结构化示意，实际K的值需要按计算公式求解。调度方式上，可以采用简单的查表加线性插值。实测中这个做法让控制器在不同速度下基本都能保持一致的阻尼比和超调特性。要注意的是，增益突变时需在K值之间做平滑过渡，不然高速切换瞬间会诱发一次横摆激励。

6.4 实车测试时的安全与渐进策略

做横向控制的实车测试时，建议遵循“一低二慢三隔离”的原则：

• 一低：初始车速要低，从20km/h起步，验证控制方向和极性是否正确。
• 二慢：转向指令变化速率要有限幅。在前向链路加上每周期最大转角增量（例如每100ms不超过0.02rad），避免车辆突然变向造成失控。
• 三隔离：把LQR输出和整车原有限速/制动系统隔离开，保证横向控制出问题时纵向安全不受牵连。

还有一个特别容易忽视的点：安全降级逻辑。当检测到横向误差持续大于某个阈值（比如0.6m）或车辆横摆角速度超过物理极限时，要让控制器停止输出并清空积分量，接管权归还给驾驶员或安全模块。这个机制在仿真里看不出价值，实车上是真的能救命的。

7. 从LQR出发的延展方向

7.1 加积分：LQI如何消除准静态偏置

纯LQR的稳态误差靠前馈校正，但如果前馈参数不精确，系统仍会留下小偏置。更系统的做法是增广积分项，构造LQI（线性二次型积分控制器）。做法是在状态向量后加一个横向误差的积分状态：

x_integral_new = [e_y, e_ψ, β, r, ∫e_y dt]^T

代价函数相应把积分状态的权重也给上。我做过对比，加入积分项的LQI系统对前馈失配、侧偏刚度偏差的鲁棒性显著增强，在低速和中速工况甚至能实现长期零静差。唯一的代价是积分累积会导致高速工况下“过冲”，因此积分权重必须设得很小，并加上抗饱和（Anti-windup）机制。

7.2 从LQR到LQG再到MPC的升级路径

如果噪声很强，可以在LQR基础上加Kalman滤波观测器，形成LQG控制。这个方案的工程价值在于你不必再依赖昂贵的高精度组合惯导来测量β，Kalman可以从噪声缠杂的横摆角速度陀螺和加速度计信号中把β估计出来。不过，LQG在建模误差大时鲁棒性会明显下降，如果模型参数偏差超过一定范围，反而比纯LQR更容易失稳。因此，LQG适合在线性度较好的室内/低速场景。

到了真正需要处理执行器饱和、轮胎极限等约束的工况，就应向MPC靠拢。MPC可以在每次求解时直接加入前轮转角限幅、转角变化率限幅，这是LQR无法显式做到的。从项目迭代节奏上看，先用LQR跑通整套逻辑，再升级到MPC，是比较务实的推进路线。

7.3 时速与场景适配的进一步建议

举一个具体的适用场景例子，在园区低速无人配送车上，车速通常不超过10km/h，此时四轮转向、低附着路面这些都很常见，LQR加侧偏角反馈在低速大转角情况下依然可以稳定，但要注意在近距离掉头场景中，转弯半径小到使轮胎侧偏角超过线性区，就该切换为几何算法（如纯追踪）兜底。反过来在高速主路巡航场景，LQR配合前馈和增益调度已能完成大部分直道、缓弯、弯道切换功能。如果进入雨雪路面，附着系数掉到0.3以下，LQR即使在低车速下也可能因为模型假设失效而性能恶化，这时更适合切换到专门的低附着工况控制策略。

我的建议是：LQR作为核心中距控制主体，在周边配上以下几种补充逻辑：

• 低速精确入位时，使用正弦航向规划或者纯追踪类几何法；
• 高速变道时，LQR的权重适时调高横向误差权重，并配合转向速率限制；
• 车道居中保持时，LQR负责日常运行，MPC/约束控制作为冲突场景的降级切换。

想到哪里写到哪里。最后说一个“硬刀”经验：我踩过最大的坑，是把权重矩阵Q调得太漂亮了——仿真里误差曲线完美，但一上实车就暴露延迟和噪声问题。后来学会了一招，就是在设计阶段把控制输入R的权重降低到原本的1/3到1/2，这看起来让仿真中的误差稍微“恶化”了一些，但实车上方向盘不再抽搐，最终的综合横向误差反而更小。控制这东西就是这样，往往不是把一个指标推到极限，而是在实际系统的各种脾气之间，找到那个真正能用得住的工作点。希望这篇梳理，能帮你缩短从“写出代码”到“跑得稳”之间的距离。