自动驾驶算法：从感知到控制，一条链路上的四道关

一、整体架构：模块化还是端到端

主流有两条技术路线。

模块化（modular）：感知 → 预测 → 规划 → 控制，每一层职责清晰、接口明确。好处是可解释、可单测、出问题能定位到具体模块——对需要通过功能安全（ISO 26262）和预期功能安全（SOTIF）认证的量产项目，这是刚需。坏处是误差逐层累积，上游感知的微小偏差会放大成下游的错误决策。

端到端（end-to-end）：传感器输入直接映射到控制输出，中间用一个（或一组）大网络完成。近年随着 BEV、Occupancy Network 和 Transformer 的成熟，这条路线进展很快。它避免了人工接口的信息损失，但可解释性和验证难度是硬伤——出了事故，你很难说清是"哪一行逻辑"错了。

现实中的量产方案基本是模块化为主、局部用端到端替换，比如感知用 BEV 网络、规划仍保留规则兜底。理解这一点，比争论哪条路线更先进有用得多。

二、四道关：链路上的核心算法

2.1 感知：把三维世界变成结构化信息

感知要回答三个问题：有什么（检测）、在哪里（定位）、什么形状（分割/占用）。

早期方案是"图像检测 + 激光雷达点云检测"各自独立，再做后融合。现在的主流是前融合 / BEV 感知：把多摄像头图像、毫米波雷达、激光雷达统一投影到鸟瞰图（Bird's Eye View）空间，在这个共享表示上做检测和分割。BEV 的好处是视角一致、便于下游规划直接使用，也天然支持时序融合。

再往前一步是 Occupancy Network：不再局限于"识别出具体类别的物体"，而是预测三维空间每个体素是否被占据。这解决了一类经典难题——没见过的、无法分类的障碍物（掉落的货物、侧翻的三轮车），只要它占据空间，就能被规划层当障碍物规避。这比"分类正确"更接近安全的本质。

多传感器融合的关键不在网络结构，而在时空对齐：不同传感器的采样频率不同、时间戳有偏差、外参标定会随时间漂移。标定不准，融合就是错的。

2.2 预测：猜别人下一步要干什么

预测模块输出的是周围每个交通参与者未来几秒的轨迹分布。难点在于人类行为的多模态性：前车减速，可能是要转弯，也可能只是让行人——同一观测对应多种未来。

主流做法是输出多条带概率的轨迹（multi-modal trajectory），再叠加交互建模（车辆之间会互相影响）。工程上还要处理"预测错了怎么办"：规划层不能只信最高概率那条，而要考虑所有可能轨迹的最坏情况。

2.3 规划：在约束里找一条能走的路

规划通常分两层：

• 全局规划：从起点到终点，用 A*、Dijkstra 或 RRT 在路网图上搜一条路径；
• 局部规划：在几秒的时间窗内，结合障碍物预测，生成满足车辆运动学约束、舒适性约束和安全约束的轨迹。

局部规划有三类典型方法：采样法（Lattice、Frenet 坐标系下的横纵向解耦采样，实现简单、实时性好）、优化法（把轨迹生成写成一个带约束的最优化问题，用 QP 或 iLQR 求解，质量高但调参难）、学习法（模仿学习或强化学习直接输出轨迹）。

工程实践里常见的是"采样出候选 + 代价函数打分 + 规则兜底"。代价函数通常包含：安全性（离障碍物距离）、舒适性（加速度、jerk）、效率（偏离参考线的程度、速度损失）、合法性（是否压线、是否超速）。

2.4 控制：把轨迹变成方向盘转角

规划给出的是期望轨迹，控制要把它翻译成油门、刹车和转向。这是整条链路里最"经典控制"的一环，也是最适合用少量代码说清楚的部分。

三、少量代码：纯跟踪 + IDM

横向控制用 Pure Pursuit（纯跟踪）：在参考路径上找一个"前视点"，让车辆沿着一段圆弧驶向它，由几何关系直接解出前轮转角。它简单、鲁棒，至今仍是很多系统的基线。

import numpy as np

def pure_pursuit(pose, path, wheelbase=2.7, ld=6.0):
    """pose=(x, y, yaw)，path 为 Nx2 参考路径，返回前轮转角(rad)"""
    x, y, yaw = pose
    dist = np.hypot(path[:, 0] - x, path[:, 1] - y)
    i = int(np.argmin(dist))                     # 找最近点

    # 沿路径向前搜索，取第一个距离 >= 前视距离的点
    target = path[-1]
    for j in range(i, len(path)):
        if np.hypot(path[j, 0] - x, path[j, 1] - y) >= ld:
            target = path[j]
            break

    # 转到车体坐标系，求航向偏差
    dx, dy = target[0] - x, target[1] - y
    alpha = np.arctan2(dy, dx) - yaw
    alpha = np.arctan2(np.sin(alpha), np.cos(alpha))   # 归一化到 [-pi, pi]

    # 圆弧曲率 -> 前轮转角
    kappa = 2.0 * np.sin(alpha) / ld
    return np.arctan(wheelbase * kappa)

两个常被忽略的细节：alpha 必须归一化，否则车辆在跨越 ±π 时会突然打满方向；前视距离 ld 通常随车速增大（ld = k * v），低速灵活、高速平稳。

纵向控制用 IDM（智能驾驶员模型）：根据期望车速和前车间距，直接算出一个加速度。它把"想开快"和"别追尾"两个诉求合在一个公式里，天然平滑。

def idm(v, v0, gap, dv, a_max=1.5, b=3.0, s0=2.0, T=1.5):
    """v: 当前车速, v0: 期望车速, gap: 前车间距, dv: 本车-前车速度差"""
    s_star = s0 + max(0.0, v * T + v * dv / (2 * np.sqrt(a_max * b)))
    acc = a_max * (1 - (v / v0) ** 4 - (s_star / max(gap, 1e-3)) ** 2)
    return np.clip(acc, -8.0, a_max)             # 限制在物理可行的减速度内

这两段加起来不到三十行，却构成了一个能沿路径跟车行驶的最小闭环。真实系统的复杂度主要不在这里，而在于上游输入不可靠时如何仍然安全。

四、几个几乎必踩的坑

1. 长尾场景没有终点。 每解决一类 corner case，就会暴露新的一类。白色卡车在逆光下与天空同色、井盖被误检为障碍物、施工路段的临时标线……靠堆规则永远追不上。这也是 BEV + Occupancy 这类"不依赖分类正确"的方案越来越受重视的原因。

2. 时序不同步比精度不够更致命。 摄像头 30Hz、激光雷达 10Hz、定位 50Hz，如果时间戳没对齐，高速行驶时融合结果会"看到过去的位置"。所有传感器必须统一到同一时钟域，这一条做不好，再好的网络也白搭。

3. 标定漂移是慢性病。 车辆震动、温度变化会让外参缓慢偏移，短期内看不出问题，跑几个月后融合精度就悄悄下降。量产方案通常需要在线标定或定期校准机制。

4. 仿真和实车的鸿沟（sim2real gap）。 在仿真里跑通不等于实车可用。光照、雨雾、传感器噪声的建模偏差，会让模型在真实世界里表现断崖式下跌。所以实车数据闭环（触发式回传 + 自动标注）才是真正的护城河。

5. 指标选错，优化方向就错。 只看"碰撞率"会掩盖舒适性问题，只看"接管次数"会忽略潜在风险。合理的一套指标至少包含：安全性、舒适性（jerk 峰值）、通行效率、以及规划轨迹与人类驾驶的偏离度。

五、什么时候不该迷信"更强的算法"

有两个务实的判断值得记住。

第一，能用规则兜底的场景，别全靠模型。 AEB（自动紧急制动）这类安全功能，业界普遍保留独立的、基于规则的触发链路，不依赖主感知模型的输出。原因很简单：安全件需要可验证、可复现。

第二，不是所有问题都该用学习解决。 低速泊车、结构化道路的车道保持，经典几何方法足够可靠且可解释。引入大模型只会增加验证成本。真正该上学习的，是那些规则难以穷举的场景——交互博弈、无保护左转、密集人流中的通行。

结语

自动驾驶算法看起来包罗万象，拆开看就是一条清晰的链路：感知回答"世界是什么样"，预测回答"别人要干什么"，规划回答"我该怎么走"，控制回答"方向盘打多少"。每一层都有成熟的经典方法，也都有正在被学习类方法改造的部分。

真正的工程难度不在单个算法有多先进，而在这条链路在异常输入下的鲁棒性：传感器脏了、标定漂了、预测错了、模型没见过，系统还能不能安全停下。

如果你准备动手，建议从一个闭环仿真场景开始：给定一条参考路径和几个移动障碍物，用上面的纯跟踪 + IDM 把车跑起来，观察它在跟车、避让、减速时的表现。比起一上来就搭 BEV 网络，这种"先让车动起来"的路径，能让你最快建立起对整条链路的直觉。