ADAS 决策规划与横纵向控制

引言

如果把自动驾驶拆成「看、想、动」三段，感知与定位负责「看」（周围有什么、我在哪），决策规划负责「想」（我该怎么走），控制负责「动」（油门刹车方向盘打多少）。本文讲后两段。它们是感知的下游：感知的输出是目标列表与可行驶区域，定位的输出是自车位姿，而决策规划的输出是一条带时间戳的轨迹，控制的输出是执行器指令。

规划控制与感知的方法论差异很大。感知以数据驱动为主（神经网络、海量标注），规划控制至今仍以模型与规则为主（运动学模型、状态机、优化求解）——因为它必须可解释、可验证、可认证。一辆车为什么变道、为什么刹车，必须能在事故调查中复盘出逻辑链条，这对纯端到端模型是巨大挑战。

工程难点集中在三处：实时性（规划要在 100 ms 内出轨迹，控制在 10 ms 内出指令，且延迟必须可预测）、约束满足（不能撞、不能超动力学极限、要舒适）、以及安全兜底（算法失效时如何保证不产生危险）。这三条决定了实际量产的方案往往是「采样 + 规则 + 优化」的混合体，而非单一最优算法。

本文按「分层架构 → 行为决策 → 轨迹规划（采样 / 优化）→ 控制（基础 / MPC）→ 功能链路 → 安全降级 → 仿真验证」展开。

目录

1. 决策、规划、控制的分层与时间尺度
2. 行为决策：状态机与决策树
3. 轨迹规划：Frenet 采样法
4. 轨迹规划：二次规划与螺旋曲线
5. 横纵向控制基础
6. 模型预测控制 MPC
7. 典型功能链路：ACC、LKA、AEB
8. 安全边界、降级与最小风险状态
9. 仿真与场景验证

1. 决策、规划、控制的分层与时间尺度

三层结构不是学术上的洁癖，而是由时间尺度决定的工程必然：

分层与周期：
  行为决策（Decision）
    回答：做什么（保持车道 / 变道 / 超车 / 让行）
    周期：100 ms ~ 1 s
    输出：目标车道、目标速度区间、动作序列
    特点：离散、低频、可解释

  轨迹规划（Planning）
    回答：怎么走（一条带时间的平滑轨迹）
    周期：50 ~ 100 ms
    输出：未来 3~8 秒的 (x, y, θ, v, a) 序列
    特点：连续、中频、计算密集

  运动控制（Control）
    回答：执行器给多少（油门 / 制动 / 转角）
    周期：10 ~ 20 ms（对应 50~100 Hz）
    输出：加速度指令、曲率指令或转角指令
    特点：连续、高频、对延迟敏感

  执行器（Actuator）
    转向 EPS、制动 ESP、驱动 VCU
    周期：1 ~ 10 ms（内部闭环）

分层的关键价值是每层只需处理自己时间尺度上的问题。决策层可以在 100 ms 内做「要不要变道」的推理，不必关心曲率连续性；控制层只跟踪给定的轨迹，不必理解「为什么要去那条车道」。这样各层可以独立开发、独立测试、独立认证。

代价是接口定义的脆弱性。规划给控制的轨迹如果只有位置没有时间信息（或反之），控制的跟踪性能会大打折扣；决策给规划的目标如果没有明确的优先级与约束，规划会在冲突目标间产生抖动。工程上大量问题出在层间接口而不是层内算法——例如「变道决策每秒切换一次」，根因往往是决策层用了瞬时信息而没做迟滞（hysteresis）。

2. 行为决策：状态机与决策树

行为决策最常用、也最经得起认证考验的方案是有限状态机（FSM）：

车道巡航状态机（简化）：
  [LaneKeep] ── 前车慢且左侧空闲 ──> [WaitForGap]
  [WaitForGap] ── 空隙足够 ──> [LaneChange]
  [WaitForGap] ── 超时 / 条件消失 ──> [LaneKeep]
  [LaneChange] ── 完成 ──> [LaneKeep]
  [LaneChange] ── 目标车道被占用 ──> [Abort] ──> [LaneKeep]
  [LaneKeep] ── 前方障碍且无法变道 ──> [Follow]
  [Follow] ── 障碍消失 ──> [LaneKeep]

  每个状态有：
    进入条件（Entry）、退出条件（Exit）、驻留约束（最小停留时间）
  每条迁移有：
    触发条件（基于感知与定位输入）+ 优先级

状态机的优势是可解释、可形式化验证（可以用状态覆盖、迁移覆盖做测试），这也是它能通过功能安全评审的原因。缺点是状态爆炸：当功能从「高速巡航」扩展到「城区 + 泊车 + 施工区绕行」，状态数与迁移数会呈组合式增长，维护成本急剧上升。

应对手段有三类。**层次状态机（HFSM）**把状态分组，子状态机只在父状态激活时运行，减少无效迁移；决策树/规则表把决策表达为「条件 → 动作」的规则集合，便于按场景增删规则；**行为树（Behavior Tree）**用组合节点（Sequence / Selector / Decorator）组织行为，天然支持优先级与回退，在机器人领域广泛使用。

决策层的两个工程要点：

• 迟滞与最小驻留时间：任何决策切换都要有迟滞带（进入条件与退出条件不同）和最小驻留时间（如变道决策至少保持 2 秒），否则会在阈值附近高频抖动。抖动会传导到规划与控制，表现为方向盘抖动或频繁加减速。
• 决策与规划的职责边界：决策只给「离散目标」（去左侧车道、跟车到 30 km/h），不给具体轨迹。若决策层直接给出轨迹点，规划层就退化成插值器，失去了处理动态障碍的能力。

3. 轨迹规划：Frenet 采样法

采样法的核心是把二维规划问题解耦到Frenet 坐标系：以参考线（车道中心线）为纵轴 s，横向偏移为 d。这样纵向（速度规划）与横向（路径规划）可以分开处理：

Frenet 坐标下的解耦：
  纵向 s(t)：跟车、限速、停止线 → 速度 profile
  横向 d(s)：车道居中、避障、变道 → 路径曲线

横向采样（多项式）：
  用五次多项式连接起点与候选终点
  d(s) = a0 + a1·s + a2·s² + a3·s³ + a4·s⁴ + a5·s⁵
  边界条件：起点 (d, d', d'') 与终点 (d, d', d'') 共 6 个
  → 恰好确定 6 个系数
  终点横向偏移采样：{车道中心, 左偏 0.5m, 右偏 0.5m, ...}
  终点纵向距离采样：{10m, 20m, 40m, 60m}

纵向采样：
  以目标速度与目标位置为终点，用四次/五次多项式生成 v(t)
  采样多个目标速度（如 ±20% 与限速的多个档位）

横纵候选组合后得到一批候选轨迹，用代价函数选优：

代价项（加权求和）：
  安全性：与障碍物的最小距离、碰撞时间 TTC
  舒适性：横向加速度、纵向加速度、加加速度 jerk
  效率  ：与目标速度的偏差、到达目标的时间
  一致性：与上一次轨迹的偏差（避免抖动）
  居中  ：偏离车道中心的距离

  权重需要按场景调：高速侧重舒适，低速泊车侧重精确

采样法的优点很实际：并行性好（候选轨迹可并行评估）、无需求解器（不存在优化不收敛的问题）、易于调试（可以可视化所有候选轨迹与代价）。缺点是不保证最优（依赖采样密度），且候选数量随维度指数增长（横向 × 纵向 × 时间），需要剪枝。

4. 轨迹规划：二次规划与螺旋曲线

优化法把规划表达为一个带约束的最优化问题，在可行域内求最优解：

二次规划（QP）形式：
  min  ½·xᵀ·Q·x + cᵀ·x        （目标：平滑 + 贴近参考）
  s.t. A_eq·x = b_eq          （等式约束：起终点边界）
       A_ineq·x ≤ b_ineq      （不等式约束：避障、动力学、车道边界）
       x_min ≤ x ≤ x_max      （变量上下界）

  变量常取路径点或控制点（如二次/三次样条的系数）
  目标项：最小化曲率、jerk、与参考线的偏差
  约束：曲率 ≤ κ_max（防侧滑）、加速度 ≤ a_max、避障距离 ≥ d_safe

避障约束是 QP 的难点——「离障碍物距离 ≥ d」本质是非凸的（障碍物两侧都可以绕，形成非凸可行域）。工程上的处理方式是把非凸问题凸化：先由决策层决定从障碍物左侧还是右侧绕行（离散决策），把可行域限定在一侧，约束就变成线性的。

**螺旋曲线（spiral / clothoid）**是路径平滑的经典工具：它的曲率随弧长线性变化，因此曲率连续（C² 连续），不会出现方向盘突变的「折点」。高速公路的缓和曲线（回旋线）用的就是螺旋曲线。规划里常用它做路径平滑的最终一步，或在采样法中作为候选曲线的参数化形式。

求解器选型影响实时性：OSQP（算子分裂，适合稀疏 QP）、qpOASES（适合小规模稠密 QP）、IPOPT（非线性，慢但通用）。车端通常用前两者，且要在 10 到 50 ms 内求解完成，否则轨迹来不及更新。

5. 横纵向控制基础

控制层的输入是规划轨迹，输出是执行器指令。横纵向通常分开设计，因为它们的执行器与动力学特性不同：

横向控制（转向）：
  纯跟踪（Pure Pursuit）
    在轨迹上取前方距离 Ld 处的一点作为目标点
    转角 = atan(2·L·sin(α) / Ld)，L 为轴距，α 为与车辆航向的夹角
    优点：简单、鲁棒；缺点：高速下 Ld 需调大否则震荡

  Stanley
    前轴中心到参考路径的横向误差 e 与航向误差 θ
    转角 = θ + atan(k·e / v)
    优点：低速跟踪好；缺点：高速需限幅

  LQR
    基于自行车模型的线性二次调节
    最小化「横向误差 + 航向误差 + 转向量」的加权平方和
    优点：理论最优、参数有物理意义；缺点：依赖模型精度

纵向控制（油门/制动）：
  前馈 + 反馈
    前馈：由期望加速度反查标定表得到油门/制动量（补偿阻力、坡度）
    反馈：PI 控制补偿模型误差与扰动
  执行器切换：油门与制动不能同时给，切换时要做死区与平滑

前馈 + 反馈是纵向控制的标准结构。纯反馈（只用 PID）会有稳态误差与响应滞后；加入基于车辆纵向动力学的前馈项后，控制器只需补偿模型残差，响应更快且超调更小。标定工作量的很大一部分就是做这张「期望加速度 → 执行器开度」的标定表，它随车速、坡度、载荷变化。

延迟补偿是控制层的隐形杀手。从传感器采样到执行器动作的端到端延迟包括：感知延迟（30 到 100 ms）、规划计算（50 ms）、通信（CAN 报文周期 10 ms × 数跳）、执行器响应（EPS/ESP 内部 20 到 50 ms）。累计可达 150 到 300 ms，在 120 km/h 下相当于 5 到 10 米的距离。补偿方式是在控制器里用模型预测「延迟期间车辆会走到哪」，或者用 Smith 预估器，或者简单地把控制目标点前移。

6. 模型预测控制 MPC

MPC 是近年规划控制融合的主流方案，它的核心思想是滚动时域优化：在每个控制周期，基于当前状态与预测模型，求解未来一段时域内的最优控制序列，但只执行第一个控制量，下一周期重新求解。

MPC 要素：
  1. 预测模型（状态空间）
     x(k+1) = A·x(k) + B·u(k)
     x = [横向误差, 横向误差率, 航向误差, 航向误差率, ...]
     u = [前轮转角, 加速度]
  2. 预测时域 Np 与控制时域 Nc
     Np 通常 20~40（如 100 ms 周期 → 预测 2~4 秒）
     Nc ≤ Np（控制量在 Nc 后保持不变）
  3. 目标函数
     J = Σ ‖x(k) - x_ref(k)‖²_Q + Σ ‖u(k)‖²_R + Σ ‖Δu(k)‖²_S
     分别惩罚：跟踪误差、控制量、控制量变化率（平滑）
  4. 约束
     转角 ≤ 转角上限、转角变化率 ≤ 上限
     加速度 ∈ [制动上限, 驱动上限]
     轮胎侧偏力在线性区（防侧滑）

  求解：每周期解一个 QP（线性 MPC）或非线性优化（NMPC）
  输出：u(0)，其余丢弃，下一周期重算

MPC 的最大优势是显式处理约束。传统 PID 无法直接表达「转角不超过 30 度」「加速度不超过 3 m/s²」，只能靠饱和与限幅补救；MPC 把这些作为优化约束，求解出的控制序列天然满足限制，且在多约束冲突时能找到折中解。这正好契合自动驾驶对「安全边界」的需求。

代价是算力与模型依赖。一个含 20 步预测、5 维状态、2 维控制的线性 MPC，每周期要解一个规模不大的 QP，在车规 SoC 上约 1 到 10 ms，可接受；但若做横纵向一体化 MPC 并加入非线性轮胎模型，计算量会上升一个数量级。另一个风险是模型失配：MPC 的预测精度依赖车辆模型参数（质量、轴距、轮胎侧偏刚度），这些参数随载荷与路况变化，模型不准会导致预测偏差，进而影响控制效果。工程上通过在线参数辨识或增加反馈项缓解。

近年来的趋势是横纵向解耦的 MPC：横向 MPC 控制转向跟踪路径，纵向 MPC 控制加减速跟踪速度 profile，两者通过共享的预测轨迹协调。这样计算量可控，且各自可以独立调参。

7. 典型功能链路：ACC、LKA、AEB

三个 L1/L2 功能覆盖了规划控制的典型链路：

ACC（自适应巡航）：
  输入：本车速度、前车距离与相对速度、设定车速、时距设置
  决策：跟车 or 定速
  纵向控制：期望加速度 = f(距离误差, 速度误差)
    跟车距离 = 最小距离 + 时距 THW × 本车速度
    THW 典型 1.0~2.5 秒（对应 4 档车距设置）
  输出：加速度指令 → 驱动/制动
  停走（Stop & Go）：低速跟停后自动起步，需 3 秒内前车起步才自动

LKA（车道保持）：
  输入：车道线（摄像头）、自车横向位置与航向
  决策：是否需要干预（偏离趋势）
  横向控制：LQR 或 MPC 输出转角
  约束：驾驶员脱手检测（方向盘力矩传感器），脱手超时则降级
  变体：LDW（只预警）、LCC（车道居中，持续控制）

AEB（自动紧急制动）：
  输入：目标（车/人/骑行者）、相对距离与速度
  决策：分级触发
    TTC = 相对距离 / 相对速度
    TTC < 2.4 s  → 预填充（制动系统建压，减小响应延迟）
    TTC < 1.6 s  → 预警（声光提示）
    TTC < 0.8 s  → 部分制动
    TTC < 0.4 s  → 全力制动（最大减速度）
  输出：制动请求 → ESP
  关键：误触发抑制（对静止目标、对向车、弯道内的护栏）

AEB 是安全等级最高的 L1 功能，通常要求 ASIL B 甚至 ASIL D（涉及制动这一安全关键执行器）。它的开发难点不在算法而在误触发抑制——一次高速上的误刹车可能引发追尾。抑制手段包括：目标分类置信度门限、目标运动状态合理性检查（对向车、横穿车、静止车分别处理）、驾驶员操作意图融合（踩油门时抑制）、以及弯道场景的曲率补偿（避免把相邻车道的车误判为本车道）。

这些功能的控制指令下发到 EPS/ESP/VCU 时，通常走 CAN 总线并配合 E2E 保护（防篡改与防丢失）；若涉及跨域（感知在 SoC、执行在 MCU），指令会经 CAN FD 与车载以太网 的网关转发，转发延迟必须计入端到端预算，否则控制层的延迟补偿会算错。

8. 安全边界、降级与最小风险状态

规划控制的安全设计围绕「当算法或硬件失效时，如何不产生危险」展开：

设计运行域（ODD）：
  明确定义功能可用的条件：道路类型、车速范围、天气、光照、地图覆盖
  超出 ODD → 不激活或主动退出

降级阶梯（从轻到重）：
  1. 性能降级：降低车速上限、扩大跟车距离、限制变道
  2. 功能降级：关闭部分功能（如禁用变道，仅保持车道）
  3. 请求接管：HMI 分级告警（视觉 + 声音 + 触觉），给驾驶员接管时间
  4. 最小风险状态（MRM）：驾驶员未接管时，本车道减速停车或靠边停车

失效检测：
  传感器失效：数据超时、范围异常、多源不一致
  执行器失效：指令与反馈不符（如请求转角与实际转角偏差过大）
  通信失效：报文超时、E2E 校验失败、心跳丢失
  算力失效：任务超时、看门狗未喂

关键设计原则有三条。故障可检测：每个失效模式都必须有对应的检测机制与检测时间预算（对应 26262 的 FTTI），检测不出来就等于没有降级。降级路径独立：降级逻辑不能依赖已经失效的模块，例如「感知失效」的降级不能靠感知来触发，要靠独立的监控通道（如心跳超时）。降级本身要安全：从「自动变道」切到「人工接管」的瞬间，如果方向盘突然失去助力或转向角突变，反而制造危险，所以切换过程要平滑（力矩渐退而非瞬断）。

控制指令的完整性保护不可省：从规划到执行器的指令要加 E2E 保护（CRC + 计数器 + 数据 ID），防止通信干扰导致的错误指令被执行。这是 功能安全 ISO 26262 工程实践 里 E2E 机制的直接应用场景。

9. 仿真与场景验证

规划控制的验证无法只靠实车——危险场景（AEB 触发、失效降级）不能也不该在开放道路上反复复现。仿真是主力：

仿真层级（由虚到实）：
  MIL（模型在环）：算法模型与车辆模型在同一仿真环境
  SIL（软件在环）：编译后的实际代码跑在 PC 上，接仿真车辆模型
  PIL（处理器在环）：代码跑在目标处理器上，接仿真车辆模型
  HIL（硬件在环）：真实 ECU 接仿真车辆与被控对象
  实车：封闭场地 → 开放道路

  规划控制的验证重心在 SIL 与 HIL：
    SIL 验证算法逻辑与场景覆盖
    HIL 验证实时性、时序、执行器接口

场景来源：
  1. 实车数据回放：把路采数据喂给算法，可重复
  2. 参数化场景：定义场景模板 + 参数空间（车速、距离、角度）
  3. 对抗场景：用搜索或强化学习主动寻找失败用例
  4. 标准场景库：法规与行业标准定义的必测场景（如 Euro NCAP AEB 场景）

评价指标：
  安全性：碰撞率、最小距离、TTC 分布
  舒适性：纵向/横向 jerk、加速度幅值
  效率  ：通行时间、与人类驾驶的偏差
  稳定性：接管率、决策切换频率（抖动）

参数化场景 + 大规模仿真是当前的主流做法：把「前车急刹」「cut-in」「弯道静止车」等场景模板化，随机采样参数生成百万级用例，统计碰撞率与失败分布。这与 V2X 车路协同与车联网通信 里提到的场景库思路一致，也是 SOTIF 要求的「把未知不安全场景转化为已知」的工程手段。

要警惕的是仿真与实车的鸿沟（sim-to-real gap）：仿真里的传感器模型、轮胎模型、执行器延迟都是近似的，仿真通过不代表实车通过。工程上要求「仿真覆盖广度 + 实车验证关键场景」，且实车数据要反哺仿真模型（用实车标定轮胎与执行器模型），形成闭环。

权衡取舍

常见坑清单

1. 决策无迟滞：阈值附近高频切换导致方向盘抖动，必须加迟滞带与最小驻留时间。
2. 忽略端到端延迟：控制目标未补偿 150 到 300 ms 延迟，高速下跟踪偏差大。
3. 轨迹无时间信息：只给几何路径不给速度 profile，控制层无法做时间对齐。
4. 避障约束非凸：直接在 QP 里写「离障碍距离 ≥ d」导致不收敛，需先离散决策再凸化。
5. MPC 预测时域过长：Np 太大导致计算超时，且远期预测不准，一般 2 到 4 秒足够。
6. 执行器切换不平滑：油门与制动切换无死区与渐变，乘客感到顿挫。
7. AEB 误触发：未做目标分类与弯道曲率补偿，把相邻车道目标误判为本车道。
8. 降级依赖失效模块：感知失效却要靠感知触发降级，降级逻辑必须独立。
9. 脱手检测缺失：LKA 未检测驾驶员脱手，法规与安全均不达标。
10. 只测仿真不测实车：sim-to-real 鸿沟导致仿真通过实车失败，必须闭环验证。

小结

决策规划与控制是自动驾驶里最「工程化」的一段：感知可以靠数据与算力堆出效果，规划控制必须在实时性、约束满足与可解释性之间做精确权衡。三条主线是分层与时间尺度（决策慢、规划中、控制快）、模型与优化（Frenet 采样 + QP + MPC 的组合）、以及安全兜底（ODD、分级降级、最小风险状态）。

入门路径建议：先理解 Frenet 坐标系与横纵解耦，这是所有规划方法的共同语言；再实现一个纯跟踪控制器跑通闭环，建立对控制延迟与跟踪误差的直觉；然后学 MPC 的建模与约束表达；最后补安全降级与场景验证。不要一上来就追求端到端——量产落地靠的是可解释、可测试、可认证的模块化方案。

继续深入：上游的感知与定位见 ADAS 感知与传感器融合 ；安全机制与 ASIL 论证见 功能安全 ISO 26262 工程实践 ；控制指令跨域传输的时延与网关见 CAN FD 与车载以太网 ；协同驾驶相关的决策见 V2X 车路协同与车联网通信 。