4. 车辆控制基础

端到端自动驾驶系统的最终执行层是车辆控制，其任务是将规划模块输出的参考轨迹（路径点序列、目标速度、航向角）转化为方向盘转角、油门开度和制动压力等底层执行指令。控制算法的优劣直接决定了乘坐舒适性、轨迹跟踪精度以及极端工况下的安全性。本章围绕四种经典且工程中广泛使用的控制方法展开：PID控制、纯跟踪控制（Pure Pursuit）、Stanley控制以及模型预测控制（MPC）。我们将从基本原理、数学表达、适用场景、参数整定与工程案例五个维度进行系统剖析，帮助读者建立从理论到实车的完整认知。

上图从工程选型角度给出了四种算法的定位差异。需要强调的是，实际自动驾驶系统中往往采用分层控制架构：上层MPC或Stanley负责横向路径跟踪，下层PID负责纵向速度与加速度闭环。下面逐一深入剖析每种算法的原理与实战要点。

4.1 PID控制：最经典的反馈闭环

4.1.1 核心原理

PID（比例-积分-微分）控制通过计算期望值与实际值之间的误差，将误差的比例、积分、微分三项线性组合作为控制量。在车辆控制中，典型应用是纵向速度控制：以目标车速与当前车速的误差为输入，输出油门/制动踏板开度。其数学表达式为：

u(t) = Kp * e(t) + Ki * ∫e(τ)dτ + Kd * de(t)/dt

其中，Kp决定响应速度，Ki消除稳态误差，Kd抑制超调。在横向控制中，PID也可用于转向角闭环，但需注意车辆动力学非线性较强，单一PID难以在全工况保持稳定。

4.1.2 参数整定与工程实践

工程中常用Ziegler-Nichols法或试凑法整定参数。首先令Ki=Kd=0，逐步增大Kp直至系统等幅振荡，记录临界增益Ku和振荡周期Tu，然后按经验公式计算各参数。实际调车时需注意：

• 积分饱和：长时间存在误差时积分项累积过大，导致超调。需加入积分限幅或抗积分饱和算法。
• 微分噪声放大：车辆速度传感器信号噪声较大时，微分项会引入剧烈抖动，通常需对微分项做低通滤波。
• 执行器死区：电子油门或制动踏板存在死区，需在控制量输出前加入死区补偿。

4.1.3 案例拆解：园区低速巡航纵向控制

某园区接驳车项目，目标车速为15km/h，车辆整备质量1.8吨。初始调试时采用纯比例控制，Kp=0.8，结果车速稳态误差达2.3km/h。加入积分项Ki=0.15后稳态误差消除，但起步阶段出现明显超调（车速冲到17.5km/h）。随后引入微分项Kd=0.05，并对速度信号做截止频率5Hz的一阶低通滤波，超调降至0.4km/h以内。最终参数为Kp=0.65，Ki=0.12，Kd=0.03，加减速冲击度控制在2.0m/s³以内，满足乘客舒适性要求。

4.1.4 常见误区

• 误区一：认为PID万能，忽略车辆模型非线性。在高速变道或紧急避障场景下，PID难以保证稳定性。
• 误区二：参数整定一次完成，不随车速、载重变化调整。建议采用增益调度（Gain Scheduling）策略，按车速分段设置不同参数。

4.2 纯跟踪控制（Pure Pursuit）：几何直觉的优雅实现

4.2.1 核心原理

纯跟踪算法基于车辆运动学模型，以车辆后轴中心为基准，在参考轨迹上寻找一个前方距离为Ld（前视距离）的目标点，然后计算车辆当前航向与该目标点方向之间的夹角α，通过阿克曼转向几何关系计算前轮转角δ：

δ = arctan(2 * L * sin(α) / Ld)

其中L为轴距。前视距离Ld是关键参数，通常与车速相关：Ld = k * v + Ld_min。车速越高，前视距离越大，路径跟踪越平滑但精度下降；车速越低，跟踪越精确但容易振荡。

4.2.2 适用场景与调参方法

纯跟踪算法在低速（<30km/h）、大曲率路径（如泊车、园区道路）下表现优异，因其不依赖复杂动力学模型，计算量极小，适合嵌入式MCU实现。调参时需注意：

1. 确定最小前视距离Ld_min（通常为2-5米），保证低速下不振荡。
2. 确定速度增益k（通常0.3-0.8），使高速时路径平滑。
3. 实车测试时，观察横向误差是否收敛，若出现蛇形摆动，增大Ld或降低k。

4.2.3 案例拆解：自动泊车路径跟踪

某AVP（自动代客泊车）系统采用纯跟踪算法，车辆轴距2.8m，泊车车速控制在5km/h以内。初始设置Ld_min=2.0m，k=0.5，在垂直车位泊入时横向误差最大达0.35m，超出车位边界。分析发现，由于泊车路径曲率半径小（最小R=3.5m），前视距离过大导致切弯内偏。将Ld_min调至1.2m，k调至0.2后，最大横向误差降至0.08m，泊车成功率从82%提升至97%。

4.2.4 常见误区

• 误区一：前视距离固定不变。必须随车速动态调整，否则低速振荡或高速发散。
• 误区二：忽略车辆最小转弯半径。当参考轨迹曲率超过车辆物理极限时，纯跟踪算法会输出超出执行器范围的转角，需增加转角限幅。

4.3 Stanley控制：前轮位置反馈的精准之选

4.3.1 核心原理

Stanley算法由斯坦福大学提出，以车辆前轴中心为参考点，同时考虑横向位置误差e和航向误差θ_e，计算前轮转角：

δ = θ_e + arctan(k * e / v)

其中θ_e为车辆航向与最近路径点切线方向的夹角，e为前轴中心到最近路径点的横向距离，v为车速，k为增益系数。该算法在数学上可证明其指数收敛性，且对路径曲率变化不敏感，在中高速（30-80km/h）场景下跟踪精度优于纯跟踪。

4.3.2 工程实现要点

• 最近路径点搜索：需高效查找车辆当前位置在参考轨迹上的最近点，工程中常用KD-Tree或分段线性插值加速。
• 航向误差计算：需对路径切线方向做平滑处理，避免路径点稀疏导致的角度跳变。
• 增益k整定：k值越大，横向误差收敛越快，但过大易引起振荡。通常k取0.5-2.0，需结合车速和路径曲率调整。

4.3.3 案例拆解：高速公路车道保持

某L2级辅助驾驶系统在高速公路上以100km/h巡航，采用Stanley算法进行车道保持。初始k=1.0，在直线路段表现良好，但进入弯道（半径R=500m）时横向误差达0.45m，且车辆出现轻微左右摆动。分析发现，由于路径点间隔较大（5m），航向误差计算存在量化噪声。解决方案：将路径点间隔加密至1m，并对航向误差做滑动平均滤波（窗口5个点），同时将k降至0.7。优化后横向误差稳定在0.12m以内，方向盘转角平滑度提升40%。

4.3.4 常见误区

• 误区一：忽略车辆动力学延迟。高速时轮胎侧偏特性导致实际横摆响应滞后，需在控制量中加入前馈补偿或采用动力学模型预测。
• 误区二：在冰雪等低附着路面直接使用。Stanley算法基于运动学模型，未考虑轮胎附着力极限，低附着路面需降低车速或切换至MPC。

4.4 模型预测控制（MPC）：面向约束的先进控制

4.4.1 核心原理

MPC的核心思想是在每个控制周期内，基于车辆动力学模型，以当前状态为初始条件，在有限预测时域（N步）内求解一个带约束的最优控制问题。目标函数通常包含轨迹跟踪误差、控制量变化率、舒适性指标等，约束条件包括执行器极限（最大转角、最大加速度）、状态约束（侧向加速度、质心侧偏角）以及安全约束（避障边界）。其数学形式为：

min J = Σ (y_ref - y_pred)ᵀQ(y_ref - y_pred) + ΔuᵀRΔu
s.t. x(k+1) = f(x(k), u(k))
     u_min ≤ u(k) ≤ u_max
     x_min ≤ x(k) ≤ x_max

在每个周期求解得到最优控制序列，但仅执行第一个控制量，下一周期滚动优化。这种“滚动时域”策略使MPC具备前馈-反馈结合的特性，能显式处理约束，是当前自动驾驶高速场景的主流方案。

4.4.2 工程实现与调优

MPC的工程落地难点在于实时性与模型精度。常用简化方法包括：

• 线性时变MPC（LTV-MPC）：在每个周期将非线性模型在参考点线性化，降低求解复杂度。
• 显式MPC：离线求解多参数二次规划，在线查表，适用于嵌入式平台。
• 求解器选择：常用OSQP、qpOASES、ACADO等，需根据预测时域（通常10-30步）和控制周期（20-50ms）评估计算耗时。

4.4.3 案例拆解：高速紧急避障控制

某自动驾驶测试车在80km/h行驶时，前方50m处出现静止障碍物，需紧急变道避障。采用MPC控制器，预测时域N=20步（步长0.1s），车辆模型采用自行车模型+轮胎线性区假设。约束设置：最大横向加速度3m/s²，最大转角变化率0.5rad/s。初始权重Q=diag(10,10,1)，R=0.1，求解时间约35ms（满足50ms控制周期）。实车测试中，车辆成功避开障碍物，最大横向误差0.3m，侧向加速度峰值2.8m/s²，乘员体感良好。若改用Stanley算法，在同样工况下横向误差达0.8m且出现明显侧滑趋势。

4.4.4 常见误区

• 误区一：模型越复杂越好。高精度模型（如魔术公式轮胎模型）虽提升预测精度，但大幅增加计算量，可能导致求解超时。工程中需在精度与实时性间权衡。
• 误区二：忽略约束的可行性。当预测时域内约束过紧（如避障边界过窄），优化问题可能无解。需引入松弛变量，在目标函数中惩罚约束违反。
• 误区三：权重参数固定不变。在高速、雨雪等不同工况下，应动态调整Q和R矩阵，例如低附着路面增大控制量变化率权重以抑制激进操作。

4.5 四种算法综合对比与选型决策

选型决策建议：若项目以纵向控制为主，PID是首选；若为低速封闭场景（泊车、园区），纯跟踪性价比最高；若为高速公路结构化道路，Stanley可满足基本需求且成本低；若涉及复杂约束（避障、执行器极限）或对舒适性有高要求，MPC是唯一选择。实际量产车中，常采用“MPC+Stanley”或“MPC+PID”的混合架构，兼顾性能与安全冗余。