企业官网建设流程全解析

车辆轨迹跟踪里把横向控制和纵向控制拆开做，横向交给 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) dt

Q 是状态误差权重矩阵，R 是控制量的惩罚权重。Q 长得越大，说明你越在乎轨迹跟踪精度；R 越大，说明你越不希望前轮转角动作剧烈。整定的时候很多人直接套公式，Q 和 R 各给一组对角线阵，跑完发现横向误差曲线是收敛了，但前轮转角像锯齿一样抖。这不是算法的问题，是权重矩阵没按物理意义来。

我自己常用的做法是：

1. 先只保留 e_y 对应的权重项，其他状态量权重置零，试出一个基础 Q 值。
2. 逐渐增加航向偏差 e_theta 的权重，观察车辆在弯道入口的转向切入是否平滑。
3. 最后再增加横向偏差变化率的权重，用来抑制高速时的横向振荡。
4. 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 一般有三种落地方式：

1. 直接用连续时间 Riccati 方程解出 K，然后把控制律离散化。
2. 把 A、B 矩阵用零阶保持器离散化，然后解离散代数 Riccati 方程。
3. 直接用 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 区域很久都触发不了增大幅度，系统响应就会特别迟钝。

我的调试套路是：

1. 先设定一个基准速度阶跃，比如从 20 km/h 加速到 40 km/h。
2. e 的量化因子初始取 1/误差最大范围，ec 的量化因子初始取 1/最大误差变化率。
3. 跑完看速度曲线有没有超调过 10%，有就减小 Kp 基准值，而不是动量化因子。
4. 观察速度误差的稳态残差，如果残差一直存在，加大 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 工况跑，会看到横向误差方差显著增大，严重时会出现蛇形振荡。原因是模型的跟随带宽变了，但反馈增益没有跟上。

解决办法通常有三条路：

1. 离线增益表：在一组离散车速点分别设计 LQR 增益，然后通过查表或线性插值实时更新反馈增益。这是最简单、最稳妥的方法。
2. 在线求解 Riccati 方程：每个控制周期用当前 vx 更新 A、B 矩阵，再在线求解状态反馈增益。优点是模型最准确，缺点是对嵌入式算力要求高。
3. 忽略变化，依靠前馈和鲁棒性硬扛：适合低速、小曲率场景，但在自动驾驶常用的变车速场景下不建议。

我的选择是离线增益表加线性插值，因为横向控制周期通常在 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 算出来的转角指令会持续顶在饱和值上，饱和期间车辆实际上处于开环控制状态。一旦误差减小、指令解除饱和，车辆会有一次明显的反向修正动作，视觉效果就是"画龙"。

解决饱和问题我用了两个手段：

1. 在 Q、R 整定时，把控制量权重 R 提高到使 99% 以上的控制指令都落在饱和以内。
2. 在控制器输出端加积分限幅和变化率限幅，避免指令从 ±0.4 rad 瞬间跳回另一个方向的绝对值。

采样周期同样坑人。LQR 连续设计时控制周期我选了 10 ms，但实际平台的主循环走 20 ms。起初我认为差 10 ms 无所谓，后来发现当横向误差收敛速度足够快时，20 ms 的控制周期会让车辆在弯道里走一个"阶梯式转向"的路径，横向误差波动明显增加。重新用 20 ms 离散化后的 A、B 矩阵重新解了一次 LQR，效果立刻正常了。这里也建议读者在选控制周期时，先量一下实际软硬件的调度抖动，别拿理想周期写代码。

5.4 我现在的初始参数推荐

虽然每个场景的参数不同，但可以给出一组我自己目前常用的初始值，方便上手：

表格：横纵向控制器初始参数参考

这里要强调，初始参数只是起跑线，真正跑起来后发现的现象往往和预期不同，还是要回到"误差曲线 + 控制量曲线 + 执行器状态曲线"三张图里去判断。

6. 最后分享两个小技巧

调试横纵向车辆轨迹跟踪到后面，我最大的体会是：不要指望一组参数通吃所有工况，也不要把横向和纵向的调参过程完全隔离。

第一个技巧是建一个「误差-控制量散点图」。把每一时刻的横向偏差当作横轴、前轮转角当作纵轴，你很直觉地就能看到控制器是在贴线运行还是在反复修正。这个散点图比单纯看误差曲线更容易暴露出 LQR 阻尼不足、模糊 PID 规则触发太敏感之类的问题。

第二个技巧是给模糊 PID 的规则数做减法。我见过很多人把规则表写到 7x7 全满，总共 49 条规则，结果规则之间互相矛盾，反模糊化输出反而在各种局促工况里来回跳。后来我把规则精简到大约 20 条，只保留物理意义上最明确的组合，效果不降反升。规则表不是越密越好，关键是覆盖住所有可能出现的误差状态。

这套横纵向组合方案有它天然的优势，也有它绕不开的调参成本。横向 LQR 给你一条清晰的最优反馈路径，纵向模糊 PID 给你一块能适应未知负载的弹性缓冲。两者配合好好的时候，车辆就像在轨道上平稳滑行，横向不抖、纵向不冲。希望这篇文章能帮你在自己的项目里少走一点弯路。