车辆轨迹跟踪里把横向控制和纵向控制拆开做横向交给 LQR、纵向交给模糊 PID这个组合我最早看到时心里是打问号的明明都是跟踪一条参考轨迹为什么不能一个控制器全搞定后来在模拟项目X上分别把纯 LQR、纯模糊 PID、LQR模糊 PID 三种方案都跑了一遍才明白这个横纵分离不是设计者偷懒而是车辆模型本身就不允许你用一套固定参数去同时驾驭横向和纵向的动态特性。今天这篇东西就是想把我在这个组合里踩过的坑、推过的公式、调过的参数一次性写清楚给准备在车辆轨迹跟踪上做控制算法选型的人一个参考。1. 横纵向分开控不是炫技是车辆模型逼出来的1.1 运动学模型与动力学模型先分清在控什么做轨迹跟踪第一步不是急着写控制器而是先想清楚被控对象是什么。横向控制关心的是车辆相对于参考轨迹的横向偏差、航向偏差以及横摆角速度这些量。它要求控制器能够根据前轮转角的变化把车辆从偏离轨迹的状态拉回到贴在轨迹上的状态。这个过程主要用自行车模型来描述也就是把左右前轮合并成一个虚拟前轮、左右后轮合并成一个虚拟后轮忽略车身侧倾和载荷转移只保留侧向速度和横摆角速度两个核心自由度。纵向控制关心的则是另一个维度的东西车速、加速度、跟车距离。它要响应的是油门和刹车指令被控对象本质上是一个带着发动机、变速器、制动系统等一大堆延迟环节的质量块。这两个维度看起来互不干扰但它们在车辆模型里是强耦合的横向动力学里的轮胎侧偏刚度会随着纵向加速度变化纵向控制的目标车速又会直接影响横向控制器里的状态转移矩阵。换句话说你在纵向上踩一脚急刹车横向控制器的模型参数其实已经变了。1.2 横向和纵向的性格差异横向控制的难点在于它是一个典型的欠阻尼系统。车辆在高速行驶时横摆角速度对前轮转角的响应存在明显的延迟和超调如果控制器增益给得太高车辆会开始画龙给得太低弯道里又会冲出轨迹。它需要的是一个能根据状态变量之间的内在关系来做最优折中的控制器而不是简单地对误差信号做比例、积分、微分运算。纵向控制的难点反而在于不确定性太多。同样的油门开度满载和空载的加速度完全不一样同一条路上坡和下坡的阻力也不一样。模糊 PID 的用武之地就在这里它不需要你精确建模发动机的扭矩 map、不需要你算清楚滚动阻力和空气阻力系数只需要你用一组如果误差大就加大比例增益这样的规则就能把一个在经典 PID 手里容易震荡或响应迟钝的速度环调稳。这套分工逻辑成立之后剩下的问题就是横向的 LQR 怎么设计纵向的模糊 PID 怎么设计以及两者在同一个轨迹跟踪任务里怎么不打架。后面几节逐个说。2. LQR 横向控制器从误差状态方程到前馈补偿2.1 状态量和误差量的选择LQR 全称是线性二次型调节器它假设被控对象是线性的然后在一个无穷时域里寻找一组反馈增益使状态变量和控制输入的二次型性能指标最小。车辆横向动力学虽然本质非线性但在参考点附近、小角度假设下可以线性化所以可以用 LQR。建模时我习惯用的是车辆横向跟踪误差模型而不是绝对坐标下的动力学模型。简单说就是把状态量定义成轨迹跟踪误差e_y车辆质心到参考轨迹的横向距离偏差e_y_dot / e_theta横向偏差变化率、航向偏差变化率如果写出完整的连续时间误差状态方程大致是这样的e_dot A * e B * delta_f C * ρ其中 ρ 是参考轨迹的曲率delta_f 是前轮转角控制量。A 矩阵里含有纵向速度 vx、横摆角速度的当前值、轮胎侧偏刚度等参数。C 矩阵是曲率前馈项它代表了即使 LQR 反馈增益算得很好如果没有这一项车辆进弯道时依然会产生稳态横向误差。这一步的关键在于不要把绝对位置放进去当状态量。我之前第一次做的时候直接把车辆在大地坐标系下的 x、y、heading 放进状态空间结果 LQR 在直道上一路正常一进弯道就发散。原因是绝对坐标下的位置误差是双积分特性线性状态反馈收敛得很慢必须引入航向偏差和横向偏差的耦合关系才能把相位裕度补回来。2.2 权重矩阵 Q 和 R 的整定逻辑LQR 的性能指标写成J ∫ (e^T Q e delta_f * R * delta_f) dtQ 是状态误差权重矩阵R 是控制量的惩罚权重。Q 长得越大说明你越在乎轨迹跟踪精度R 越大说明你越不希望前轮转角动作剧烈。整定的时候很多人直接套公式Q 和 R 各给一组对角线阵跑完发现横向误差曲线是收敛了但前轮转角像锯齿一样抖。这不是算法的问题是权重矩阵没按物理意义来。我自己常用的做法是先只保留 e_y 对应的权重项其他状态量权重置零试出一个基础 Q 值。逐渐增加航向偏差 e_theta 的权重观察车辆在弯道入口的转向切入是否平滑。最后再增加横向偏差变化率的权重用来抑制高速时的横向振荡。R 值从 1 开始如果发现前轮转角饱和就把 R 调大一个数量级再试。举个例子在模拟项目X里vx 30 km/h 时我用的是一组偏保守的权重直道跟踪精度在 0.05 m 以内弯道瞬态误差最大 0.15 m。但如果把 Q 的 e_y 项从 10 提到 100同样的弯道误差会从 0.15 m 降到 0.08 m代价是方向盘指令的方差涨了大概三倍。横向控制指标不能只看均方根误差还要看转向执行器的寿命所以我的习惯是误差小于十分之一道宽就不要再加增益。2.3 前馈补偿消除稳态误差LQR 反馈控制本质上是从最优控制里解出来的比例反馈它没有积分项因此在有常数干扰或参考输入变化时稳态误差是躲不掉的。轨迹跟踪里的参考输入变化就是弯道曲率 ρ所以必须在前馈通道里补偿曲率的影响。前馈控制量可以这样估算delta_ff K_ff * ρK_ff 在稳态转向模型里是一个跟车速、轴距、轮胎侧偏刚度有关的系数。你可以直接从稳态转向公式推也可以简单一点在恒定半径的圆弧轨迹上做一组开环前轮转角扫描记录稳态横向误差为零时的转角与曲率比值用这个比值做前馈系数。加了前馈之后你会发现原本 LQR 在弯道里的横向误差会从稳态偏差变成动态过冲。这是因为前馈把大部分曲率补偿掉了LQR 只需要处理剩余误差。此时 LQR 的反馈增益可以适当降低车辆过弯时的手感或者说控制连续性会明显好很多。2.4 离散化与代码落地的细节实际写代码时连续时间 LQR 一般有三种落地方式直接用连续时间 Riccati 方程解出 K然后把控制律离散化。把 A、B 矩阵用零阶保持器离散化然后解离散代数 Riccati 方程。直接用 Python 的 python-control 或 MATLAB 的 dlqr 函数算离散增益。我自己偏向于用离散 LQR因为控制周期通常是 10 ms 或 20 ms连续增益直接乘以控制周期在高速场景下会和离散模型差出几个百分点的跟踪误差虽然不至于发散但在做硬件在环对比时会很扎眼。还有一个很容易踩的点状态反馈增益在转向执行器上是有幅值限制的一般前轮转角限幅在 ±0.4 rad 附近。在做 LQR 增益矩阵迭代时我一般会把控制量饱和直接仿真进去否则算出来的 Q、R 在低附着路面上会让你看到一个近乎开环的失控表现。原因很简单极限工况下你要求的控制力矩很大但执行器给不了这么多反馈增益再大也只是在给饱和控制器充电。3. 模糊 PID 纵向控制器给速度环装上经验规则3.1 普通 PID 在纵向控制上不够用的地方先说一说为什么纵向速度环我不直接用普通 PID。普通 PID 的问题不是不能用而是它对被控对象模型的变化非常敏感。同一个模拟项目X里我在空载工况下把一组 PID 参数调到临界最优误差均方根能压到 0.3 km/h 以内随后往行李舱加了配重模拟满载同样的参数跑坡道路段速度误差翻了一倍还出现了明显的低频振荡。原因是纵向动力学里有明显的非线性发动机扭矩 map 的变化、液力变矩器的特性、制动力矩在不同车速下的非线性增益都是普通 PID 没有办法用一组固定参数覆盖的。模糊 PID 的目标就是让 Kp、Ki、Kd 三个参数跟随当前工况自动调整从而在不重新建模的情况下适应各种负载和坡度。3.2 模糊 PID 的整体结构模糊 PID 的结构不复杂核心是在经典 PID 外面套了一层模糊推理引擎。输入是两个速度误差 e v_target - v_actual误差变化率 ec de/dt输出有三个ΔKp、ΔKi、ΔKd。最终实际 PID 参数由基准参数加增量得到Kp Kp_base ΔKp Ki Ki_base ΔKi Kd Kd_base ΔKd每次控制周期做一次模糊推理查询输入隶属度函数、应用规则表、反模糊化然后更新 PID 参数。这个逻辑在纯软件仿真里大概只占几行代码但在嵌入式里要特别注意查表效率避免浮点运算过多拉长控制周期。3.3 隶属度函数与规则表怎么定模糊 PID 的性能好坏很大程度上取决于规则表怎么定而不是模糊推理过程本身。我的经验是先把误差和误差变化率各划分为七个语言变量负大NB、负中NM、负小NS、零ZO、正小PS、正中PM、正大PB。规则表的设计核心可以浓缩成几条经验误差绝对值大时大幅提高 Kp让系统快速逼近目标速度同时调低 Kd 防止超调。误差和误差变化率符号相同时说明系统在加速偏离目标应该继续加大 Kp 和 Kd。误差和误差变化率符号相反时说明系统正在回摆要减小 Kp防止过冲。稳态附近误差和误差变化率都接近零时适当增加 Ki 消除静差但 Kp 要保守。举例说明我常用的一条规则是这样写的如果 e 是 NB速度偏低很多且 ec 是 NB还在继续减速则 ΔKp 取 PB最大、ΔKi 取 ZO、ΔKd 取 PS。如果已经靠近目标速度但还在快速变化即 e 是 ZO、ec 是 PB那么 ΔKp 取 NM、ΔKi 取 PM让积分慢慢把余差吃掉。这里最容易犯的错是把规则表写得太对称。速度环不像位置环加速响应可以猛一点但减速响应如果同样猛车内乘员会很难受。所以我在加速方向的 Kp 增量做得比减速方向更大在减速方向的 Kd 增量做得更大一些这样既能跟上目标速度又不会产生急刹车式的顿挫感。3.4 量化因子对响应速度的影响量化因子通俗地说就是把 e 和 ec 的实际数值映射到 [-3, 3] 这个论域里的比例系数。量化因子选多少直接决定了模糊规则触发的灵敏度。量化因子太大会出现规则跳变误差明明还很大但映射以后已经落在 PB 区域输出立刻顶满看起来响应快实际上控制量突变严重。量化因子太小误差落在 ZO 区域很久都触发不了增大幅度系统响应就会特别迟钝。我的调试套路是先设定一个基准速度阶跃比如从 20 km/h 加速到 40 km/h。e 的量化因子初始取 1/误差最大范围ec 的量化因子初始取 1/最大误差变化率。跑完看速度曲线有没有超调过 10%有就减小 Kp 基准值而不是动量化因子。观察速度误差的稳态残差如果残差一直存在加大 Ki 基准值和 e 的量化因子。这个套路在模拟项目X上几次迭代后速度跟踪的均方根误差就稳定在了 0.2 km/h 左右坡道工况下也能压在 0.5 km/h 以内。对比经典 PID调参时间少了大概一个下午。4. 横纵向耦合怎么处理速度会改变横向模型的根轨迹4.1 纵向速度作为 LQR 模型参数如何处理前面说过横向模型的状态转移矩阵里含有纵向速度 vx。这里展开说清楚自行车模型里的横摆角速度阻尼项、侧偏刚度项都和 vx 有直接关系。也就是说当你在纵向上用模糊 PID 把车速从 50 km/h 拉到 80 km/h 时横向 LQR 的 A 矩阵、B 矩阵其实已经变了需要在新的工作点重新计算反馈增益。如果忽略这件事一个在 60 km/h 下调好的 LQR 增益直接放到 90 km/h 工况跑会看到横向误差方差显著增大严重时会出现蛇形振荡。原因是模型的跟随带宽变了但反馈增益没有跟上。解决办法通常有三条路离线增益表在一组离散车速点分别设计 LQR 增益然后通过查表或线性插值实时更新反馈增益。这是最简单、最稳妥的方法。在线求解 Riccati 方程每个控制周期用当前 vx 更新 A、B 矩阵再在线求解状态反馈增益。优点是模型最准确缺点是对嵌入式算力要求高。忽略变化依靠前馈和鲁棒性硬扛适合低速、小曲率场景但在自动驾驶常用的变车速场景下不建议。我的选择是离线增益表加线性插值因为横向控制周期通常在 10 ms 到 20 ms在线求解 4 阶 Riccati 方程虽说不慢但控制器里同时还要跑模糊 PID、轨迹插值和状态估计没必要把算力浪费在这个重复计算上。4.2 协调策略分层调度与增益调度横纵向控制器在架构上应该是松耦合的。我的具体做法是上层轨迹规划器输出参考速度、参考曲率和参考横摆角速度。纵向控制模块接收参考速度自己产生油门和刹车指令同时把当前实际车速发布到总线上。横向控制模块接收参考轨迹、实际车速再从离线增益表里查出当前车速对应的 LQR 增益最后结合前馈计算出前轮转角指令。这里比较隐蔽的问题是当纵向控制器正在减速时车辆实际车速处于剧烈变化过程中横向控制器如果直接读取这个瞬时的 vx 去查表增益表可能会在很短时间里被来回切换好几轮导致转向指令出现抖动。解决办法是给查表用的 vx 加上一阶低通滤波时间常数取 0.2 秒左右忽略高频速度波动。另外一个协调细节是当纵向车速接近零的时候LQR 横向控制器会退化。车辆原地转向的时候轮胎侧偏力几乎为零只能靠低速运动学模型约束来描述。这种情况下我会把控制策略切换到纯运动学前馈前轮转角直接按预瞄距离上的曲率计算不再依赖模型反馈增益。4.3 仿真对比只看横向误差会吃大亏我做横纵向联合仿真时的评价指标通常不止一个。横向最大误差、横向均方根误差、纵向速度误差、前轮转角变化率、加速度突变次数这五个指标缺一不可。有一组对比数据比较有代表性在同一个蛇形绕桩工况下如果只把 LQR 的误差权重调高横向最大误差可以从 0.09 m 降到 0.06 m但前轮转角变化率从 0.8 rad/s 涨到了 1.5 rad/s纵向速度的波动范围也从 0.3 km/h 扩大到 1.1 km/h。这说明转向动作过于激进车辆会产生额外的横摆纵向控制器为了维持车速必须频繁修正油门和刹车。横纵向出现了一个典型的耦合共振横向用力过猛纵向就跟着抖。这个现象提醒我在评价横纵向控制器时必须把转向指令变化率和纵向加速度突变率放在同一张图里看而不是只看横向误差曲线。5. 从仿真到试验平台参数整定和踩坑记录5.1 第一次跑通后的三个意外我第一次把横向 LQR 和纵向模糊 PID 合在一起跑通时本以为万事大吉结果一上试验平台就发现了三个问题。第一个是转向执行器响应延迟。仿真里 LQR 算出的前轮转角是瞬间施加到模型上的但实际平台的转向电机有约 80 ms 的响应延迟这个延迟在高车速工况下直接让横向误差多出一个明显的慢回正滞后。解决办法不是硬调 LQR 增益而是在控制链路里加一个 Smith 预估器或者在状态估计里把转向延迟建模进去。第二个是模糊 PID 的初始输出跳变。Kp_base 在纵向控制器启动瞬间是非零的而油门执行器在控制器切换的瞬间可能带着一个旧指令如果不做输出限幅和软启动车辆会猛地蹿一下。我在纵向控制器代码里加了一个两秒的斜坡软启动才把启动冲击压掉。第三个是信号时序问题。横向 LQR 和纵向模糊 PID 跑在同一个实时内核里但传感器数据的到达时间不同导致速度信号比轨迹信号早了 10 ms 到达查表增益和实际横向状态不匹配。后来给所有输入信号都统一加了 10 ms 的对齐缓冲问题就消失了。这些小问题单个看都不算大但放到一起会让整个系统表现得很不专业。所以调试时不要只盯着控制算法本身还要把执行器延迟、信号噪声、调度时序都考虑进去。5.2 模糊 PID 规则表的反直觉修正我最初做模糊 PID 规则表时下意识地遵循了误差越大 Kp 越大这条常规思路结果在减速工况出现了连续震荡。具体现象是目标速度从 60 km/h 降到 40 km/h 时速度曲线没有平滑下降而是在 42 km/h 附近来回震荡幅度约 0.8 km/h。排查后发现问题出在减速工况下 Kp 增量也被规则表放大了。因为速度低下来之后车辆本身就有较大的自然减速阻力再叠加过大的 Kp控制器把油门给得很小或者直接切刹车导致速度回升和回落交替出现。修正方式有点反直觉在误差为负实际车速高于目标车速且误差变化率也为负还在加速偏离时我把 ΔKp 从 PM 降成 NS反而让速度回落曲线更加平滑。因为此时阻力本身就在帮忙减速控制器只需要轻踩制动辅助不需要大比例增益去猛拉速度。这个案例让我明白模糊 PID 的优势不在于规则表写得越丰富越好而在于规则表是否贴合被控对象的物理特征。纵向控制器里速度高时自然阻力大这件事本身就是一条隐藏的前馈效应规则表没必要去对抗它。5.3 执行器饱和与采样周期的坑仿真里最容易忽略的问题是执行器饱和。我在模拟项目X里给定前轮转角限幅 ±0.4 rad但横向误差偏大时 LQR 算出来的转角指令会持续顶在饱和值上饱和期间车辆实际上处于开环控制状态。一旦误差减小、指令解除饱和车辆会有一次明显的反向修正动作视觉效果就是画龙。解决饱和问题我用了两个手段在 Q、R 整定时把控制量权重 R 提高到使 99% 以上的控制指令都落在饱和以内。在控制器输出端加积分限幅和变化率限幅避免指令从 ±0.4 rad 瞬间跳回另一个方向的绝对值。采样周期同样坑人。LQR 连续设计时控制周期我选了 10 ms但实际平台的主循环走 20 ms。起初我认为差 10 ms 无所谓后来发现当横向误差收敛速度足够快时20 ms 的控制周期会让车辆在弯道里走一个阶梯式转向的路径横向误差波动明显增加。重新用 20 ms 离散化后的 A、B 矩阵重新解了一次 LQR效果立刻正常了。这里也建议读者在选控制周期时先量一下实际软硬件的调度抖动别拿理想周期写代码。5.4 我现在的初始参数推荐虽然每个场景的参数不同但可以给出一组我自己目前常用的初始值方便上手表格横纵向控制器初始参数参考参数项数值范围说明横向控制周期10-20 ms越快越好受执行器限制Q 对角元e_y 权重10-100越大越贴线但转向变猛Q 对角元e_theta 权重5-50建议不超过 e_y 权重的两倍R转向权重1-100越大转向越平滑误差略大前馈系数 K_ff5-15按稳态转向特性标定模糊 PID 基准 Kp1-3加速和减速可分开模糊 PID 基准 Ki0.05-0.2过大会引起低频振荡模糊 PID 基准 Kd0.1-0.8主要抑制急加速波动vx 增益表插值滤波0.1-0.3 s滤掉高频速度波动这里要强调初始参数只是起跑线真正跑起来后发现的现象往往和预期不同还是要回到误差曲线 控制量曲线 执行器状态曲线三张图里去判断。6. 最后分享两个小技巧调试横纵向车辆轨迹跟踪到后面我最大的体会是不要指望一组参数通吃所有工况也不要把横向和纵向的调参过程完全隔离。第一个技巧是建一个「误差-控制量散点图」。把每一时刻的横向偏差当作横轴、前轮转角当作纵轴你很直觉地就能看到控制器是在贴线运行还是在反复修正。这个散点图比单纯看误差曲线更容易暴露出 LQR 阻尼不足、模糊 PID 规则触发太敏感之类的问题。第二个技巧是给模糊 PID 的规则数做减法。我见过很多人把规则表写到 7x7 全满总共 49 条规则结果规则之间互相矛盾反模糊化输出反而在各种局促工况里来回跳。后来我把规则精简到大约 20 条只保留物理意义上最明确的组合效果不降反升。规则表不是越密越好关键是覆盖住所有可能出现的误差状态。这套横纵向组合方案有它天然的优势也有它绕不开的调参成本。横向 LQR 给你一条清晰的最优反馈路径纵向模糊 PID 给你一块能适应未知负载的弹性缓冲。两者配合好好的时候车辆就像在轨道上平稳滑行横向不抖、纵向不冲。希望这篇文章能帮你在自己的项目里少走一点弯路。