2. 自动驾驶基础
自动驾驶系统的感知、决策与控制链路，本质上是一个“数据获取→状态估计→行为决策→执行控制”的闭环。而支撑这一闭环的三大基础要素分别是：用于获取外部环境信息的传感器、用于描述车辆自身运动规律的车辆模型，以及用于统一空间语义的坐标系与坐标变换体系。三者共同构成了端到端自动驾驶算法设计的底层逻辑。本章将围绕这三部分展开，重点讲解各传感器的物理原理与适用场景、自行车模型与运动学模型的推导与使用边界，以及坐标系之间的变换流程与工程实现要点。
2.1 传感器：感知系统的“眼睛”与“耳朵”
2.1.1 核心原理与特性对比
自动驾驶感知层常用的三类传感器——摄像头、激光雷达、毫米波雷达——在物理原理、数据形态和适用场景上差异显著。摄像头通过CMOS/CCD感光元件捕捉环境反射的可见光，输出稠密的RGB图像，具有丰富的纹理和颜色信息，适合目标分类、车道线检测、交通标志识别。但其本质是“被动光学传感器”，对光照变化敏感，夜间或逆光环境下性能衰减明显，且缺乏直接测距能力，深度估计依赖三角测量或深度学习模型。
激光雷达（LiDAR）通过发射激光束并测量反射时间差（ToF）或调频连续波（FMCW）差频来获取目标距离，输出三维点云。其测距精度可达厘米级，不受环境光干扰，是构建高精度环境地图和障碍物检测的核心传感器。但激光雷达在雨雾天气下衰减严重，且点云稀疏性导致远距离小目标检测困难，硬件成本较高。毫米波雷达工作在76~81GHz频段，利用多普勒效应直接测量径向速度，穿透雨雾烟尘能力强，是AEB（自动紧急制动）和ACC（自适应巡航）功能的主力传感器。其缺点是角度分辨率低，点云密度远低于激光雷达，难以识别静止物体轮廓。
| 维度 | 摄像头 | 激光雷达 | 毫米波雷达 |
|---|---|---|---|
| 工作原理 | 被动光学成像 | 激光ToF/FMCW | 毫米波反射+多普勒 |
| 输出数据 | RGB图像（稠密） | 3D点云（稀疏） | 目标列表/点云（极稀疏） |
| 测距能力 | 间接（视觉深度估计） | 直接，厘米级精度 | 直接，米级精度 |
| 测速能力 | 弱（需帧间差分） | 弱（需帧间匹配） | 强（多普勒直接测速） |
| 环境鲁棒性 | 受光照影响大 | 受雨雾影响大 | 全天候，抗恶劣天气 |
| 典型应用 | 车道线/标志/目标分类 | 高精地图/障碍物轮廓 | AEB/ACC/盲区监测 |
2.1.2 传感器选型与融合策略
在实际工程中，单一传感器无法满足L3级以上自动驾驶的全场景需求。主流方案采用“摄像头+激光雷达+毫米波雷达”的多传感器融合架构。融合策略分为数据级融合、特征级融合和目标级融合。数据级融合将原始图像与点云进行像素级对齐，计算量巨大且对时间同步要求极高；特征级融合在特征提取后合并，兼顾信息丰富度与计算效率；目标级融合则对每个传感器独立检测的目标列表进行关联与匹配，工程实现简单，是目前量产方案的主流。
以典型城市NOA（Navigate on Autopilot）场景为例：摄像头负责识别红绿灯、车道线、行人姿态；激光雷达提供前方100米内障碍物的精确轮廓与距离；毫米波雷达则负责测量前方车辆相对速度，为ACC提供可靠的纵向控制输入。三者通过卡尔曼滤波或匈牙利算法进行目标关联，输出统一的障碍物列表供规划模块使用。
2.2 车辆模型：从运动学到动力学的抽象
2.2.1 自行车模型（Bicycle Model）
自行车模型将四轮车辆简化为前后两个车轮，假设车辆只在平面内运动，忽略垂向载荷转移和轮胎侧偏特性。其核心假设是：前轮转角δ为等效转角，后轮不参与转向；车辆质心侧偏角较小，轮胎工作在线性区。该模型在低速（通常<10m/s）和中等曲率路径下具有较高精度，是路径规划与跟踪控制中最常用的简化模型。
运动学方程如下（以前轮中心为参考点）：
x_dot = v * cos(θ + β)
y_dot = v * sin(θ + β)
θ_dot = v * cos(β) * tan(δ) / L
β = arctan( (lr * tan(δ)) / (lf + lr) )其中，L为轴距，lf和lr分别为质心到前后轴的距离，β为质心侧偏角，θ为航向角，v为纵向车速。该模型将车辆视为一个刚性杆，忽略了轮胎滑移和离心力影响，因此不适合高速紧急避障或湿滑路面工况。
2.2.2 运动学模型与动力学模型的边界
运动学模型（Kinematic Model）只考虑几何约束，不涉及力和力矩，适用于泊车、园区低速巡航等场景。动力学模型（Dynamic Model）则引入轮胎侧偏刚度、车辆质量、转动惯量等参数，能够描述高速工况下的不足转向/过度转向特性，是ESP（车身稳定系统）和高速LKA（车道保持辅助）的基础。工程中常采用“低速用运动学、高速用动力学”的分段策略，或通过自适应切换逻辑根据车速和横向加速度实时选择模型。
2.2.3 模型参数标定与验证
无论采用哪种模型，参数准确性直接影响控制效果。轴距L可从车辆公告参数获取，但质心位置（lf/lr）需通过实车称重或悬架高度传感器估算。轮胎侧偏刚度则需通过阶跃转向试验或正弦扫频试验辨识。验证模型精度时，可采集实车GPS/IMU数据，对比模型预测轨迹与真实轨迹的均方根误差（RMSE）。通常要求低速工况下横向位置误差小于0.3m，航向误差小于2°。
2.3 坐标系与坐标变换：统一空间语义
2.3.1 坐标系定义与约定
自动驾驶涉及多个坐标系，最核心的包括：世界坐标系（通常为UTM或局部ENU东北天坐标系，用于全局路径规划与地图匹配）、车体坐标系（原点位于车辆后轴中心或质心，X轴指向车头，Y轴指向左侧，Z轴向上，遵循ISO 8855或ROS REP-103标准）、传感器坐标系（每个传感器各自定义，如摄像头光心为原点，Z轴指向拍摄方向）。此外还有图像像素坐标系和相机归一化平面坐标系，用于视觉几何计算。
2.3.2 坐标变换流程与实现
坐标变换的核心是刚体变换，包含旋转和平移。旋转可用旋转矩阵R（3×3）、欧拉角（roll/pitch/yaw）或四元数表示。工程中推荐使用四元数，因其无万向节锁问题且插值平滑。变换公式为：
P_world = R_wc * P_cam + T_wc其中R_wc和T_wc为相机到世界的外参，通过标定获得。多传感器标定通常采用靶标法（如棋盘格）或自然特征匹配法（如ICP配准）。标定流程包括：采集多帧包含靶标的同步数据→提取特征点→求解初始外参→非线性优化（重投影误差最小化）→验证精度。
| 变换类型 | 公式 | 典型应用 | 注意事项 |
|---|---|---|---|
| 传感器→车体 | P_veh = R_sv * P_sen + T_sv | 点云投影到车辆周围 | 外参标定精度直接影响融合 |
| 车体→世界 | P_world = R_vw * P_veh + T_vw | 局部路径映射到全局地图 | 依赖定位模块（GPS/IMU/里程计） |
| 世界→车体 | P_veh = R_vw^T * (P_world - T_vw) | 全局路径转换到局部控制 | 注意旋转矩阵正交性 |
| 像素→相机 | P_cam = K^(-1) * [u,v,1]^T * d | 图像深度估计 | K为内参矩阵，d为深度 |
2.3.3 实战场景：多传感器融合中的坐标对齐
考虑一个典型场景：车辆行驶中，激光雷达检测到前方5米处有一个障碍物，同时摄像头在图像坐标(320, 240)处识别到该障碍物为行人。融合模块需要将激光雷达点云中的障碍物中心点从雷达坐标系变换到车体坐标系，再投影到图像平面，与摄像头检测框进行关联。具体步骤如下：
- 读取激光雷达外参（R_lr, T_lr），将点云坐标变换到车体坐标系：P_veh = R_lr * P_lidar + T_lr。
- 读取摄像头外参（R_cr, T_cr），将车体坐标变换到相机坐标系：P_cam = R_cr * P_veh + T_cr。
- 利用相机内参K进行投影：u = f_x * x/z + c_x，v = f_y * y/z + c_y。
- 计算投影点与摄像头检测框中心的欧氏距离，若小于阈值（如20像素），则判定为同一目标。
该流程中，外参标定误差1cm可能导致投影偏差5~10像素，因此标定精度需控制在毫米级。同时，时间同步误差10ms在车速20m/s时会造成0.2m的空间错位，必须通过插值或硬件同步解决。
2.4 三要素的协同与工程落地
传感器、车辆模型、坐标系并非孤立模块，而是通过数据流紧密耦合。传感器提供原始观测，经过坐标变换统一到车体坐标系后，输入给状态估计模块（如卡尔曼滤波），结合车辆运动模型进行预测与更新，输出车辆当前位姿和速度。规划模块基于该位姿和统一坐标系下的环境模型生成轨迹，控制模块再根据车辆模型将轨迹转化为方向盘转角与油门/制动指令。整个链路中，任何一环的精度不足都会向下游传递误差。
在实车调试中，建议按以下顺序排查问题：首先确认坐标系变换是否正确（可通过可视化点云与图像叠加验证）；其次检查车辆模型参数是否与实际车辆匹配（对比模型预测轨迹与GPS轨迹）；最后评估传感器数据质量（如点云是否畸变、图像是否过曝）。只有三要素协同工作，才能保证端到端自动驾驶系统的稳定可靠。