规划算法基础：坐标系转换、车辆运动学模型、路径与轨迹的区别、规划问题数学建模
自动驾驶规划算法处理的是“车在哪里、要去哪里、怎么去”的核心问题。在工程落地时，我们面对的第一个挑战并非算法本身的复杂度，而是如何将物理世界中的车辆状态、传感器观测和道路信息，统一到一个可计算、可推理的数学框架中。本章将围绕四个基础但极易出错的关键模块展开：坐标系转换、车辆运动学模型、路径与轨迹的本质区别，以及规划问题的数学建模方法。这些内容构成了后续所有规划算法（包括采样、优化、图搜索）的公共底座，任何一处理解偏差都可能在实车测试中放大为严重的安全事故。
2.1 坐标系转换：统一空间基准的工程关键
核心原理
自动驾驶车辆同时涉及多个坐标系：全局世界坐标系（通常为UTM或自定义东北天坐标系）、车辆车身坐标系（原点在后轴中心，X轴朝前，Y轴朝左）、传感器坐标系（相机、激光雷达、毫米波雷达各自独立）。规划算法接收的障碍物信息来自传感器坐标系，而全局路径规划需要世界坐标系，控制模块则需要车身坐标系下的目标点。坐标系转换的数学本质是刚体变换，包括三维平移和旋转。工程中常用齐次变换矩阵 T 将点从一个坐标系映射到另一个坐标系：
P_world = T_world_vehicle * T_vehicle_sensor * P_sensor其中 T 为 4x4 矩阵，包含旋转矩阵 R（3x3）和平移向量 t（3x1）。旋转矩阵可用欧拉角（roll, pitch, yaw）、旋转向量或四元数表示。在规划算法中，我们最关心的是车辆横摆角 yaw（绕Z轴旋转），因为它在水平面内决定车辆朝向。
判断方法与操作流程
工程中判断坐标系转换是否正确，最直接的方法是“闭环验证”：将世界坐标系下的一个已知点（如车道线端点）转换到车身坐标系，再转换回世界坐标系，检查误差是否在毫米级。具体操作流程如下：
- 确认源坐标系与目标坐标系定义：明确各坐标系原点位置、轴向方向、单位（米/毫米）。
- 获取外参标定结果：传感器到车身的旋转平移矩阵，通常由标定间完成。
- 编写转换函数：使用齐次矩阵乘法，注意旋转矩阵的转置等于其逆矩阵（正交性）。
- 时间同步：确保参与转换的传感器数据时间戳一致，否则车辆运动会导致转换误差。
- 单元测试与可视化验证：在仿真环境中将激光雷达点云投影到图像上，或在地图上绘制车身坐标系原点轨迹。
| 坐标系 | 原点位置 | X轴 | Y轴 | Z轴 | 典型用途 |
|---|---|---|---|---|---|
| 世界坐标系 | 地图原点 | 东 | 北 | 天 | 全局路径规划 |
| 车身坐标系 | 后轴中心 | 车辆前进方向 | 车辆左侧 | 车辆上方 | 运动学计算、控制 |
| 传感器坐标系 | 传感器光心/中心 | 传感器朝向 | 传感器左侧 | 传感器上方 | 障碍物感知 |
案例拆解：传感器外参标定后的坐标验证
某项目使用前视摄像头检测车道线，需要将车道线从像素坐标转换到车身坐标系。初始现象：在实车测试中，车道线投影到车身坐标系后，与高精地图车道线存在约0.5米的横向偏差。分析过程：首先检查外参标定文件，发现旋转矩阵的Yaw角标定值为0.02弧度（约1.15度），但实际安装时摄像头支架存在微小倾斜。通过将车辆停在标定间，使用已知距离的靶标进行验证，发现转换后的靶标位置与实测位置偏差随距离增大而增大。操作步骤：重新进行外参标定，使用多位置靶标采集数据，优化后Yaw角修正为0.018弧度。结果对比：修正后横向偏差降至0.05米以内。复盘经验：坐标系转换的误差往往不是单一来源，需要结合静态标定和动态验证共同排查。
2.2 车辆运动学模型：从几何关系到状态方程
核心原理
运动学模型描述车辆位置、速度、朝向角随时间变化的几何关系，不考虑产生运动的力和力矩。最常用的是自行车模型（Bicycle Model），它将车辆简化为前后两个轮子，假设车辆低速行驶时轮胎无侧偏，后轮方向与车身朝向一致。状态量通常为 [x, y, θ, v, δ]，其中 x, y 为后轴中心坐标，θ 为航向角，v 为纵向速度，δ 为前轮转角。运动学微分方程为：
dx/dt = v * cos(θ)
dy/dt = v * sin(θ)
dθ/dt = v * tan(δ) / L
dv/dt = a其中 L 为轴距，a 为纵向加速度。该模型在低速泊车、园区接驳等场景下精度足够，且计算简单，适合实时规划。
判断方法与操作流程
选择运动学模型还是动力学模型，取决于车速和工况。判断标准如下：
- 车速 < 10 m/s（36 km/h）：运动学模型足够，忽略轮胎侧偏。
- 车速 10-20 m/s：需考虑轮胎侧偏角，使用线性动力学模型。
- 车速 > 20 m/s：必须使用非线性动力学模型，考虑轮胎力饱和。
操作流程：首先确定车辆参数（轴距、轮距、最大转角），然后根据场景选择模型，最后在仿真中对比模型输出与实车数据（如GPS/IMU融合轨迹）的误差。
案例拆解：自动泊车轨迹规划中的模型选择
某自动泊车项目，车辆需要以不超过5 km/h的速度在狭窄车位内完成入库。初始目标：生成一条从起始点到目标位姿的平滑轨迹。关键输入：车位尺寸（长5.3m，宽2.4m），车辆轴距2.8m，最大前轮转角35度。分析过程：由于车速极低，轮胎侧偏角可忽略，选择自行车运动学模型。但发现直接使用连续曲率路径规划（如RS曲线）生成的轨迹包含尖点，车辆无法原地转向。操作步骤：改用混合A*算法，在状态空间 (x, y, θ) 中搜索，使用运动学模型进行节点扩展，并添加Reeds-Shepp曲线作为启发式。结果对比：生成轨迹的最大曲率为0.18 1/m，满足车辆最小转弯半径5.6m的要求，泊车成功率从78%提升至95%。复盘经验：低速场景下运动学模型是规划的核心，但必须结合车辆最小转弯半径约束，否则生成的轨迹不可执行。
2.3 路径与轨迹的区别：时间维度的缺失与补全
核心原理
路径（Path）是空间中的一条几何曲线，只包含位置信息（x, y, 可能包含朝向角），不包含时间信息。轨迹（Trajectory）则是在路径的基础上附加了时间戳、速度、加速度等时间维度信息。规划算法的最终输出必须是轨迹，因为控制模块需要知道每个时刻的目标位置和速度。路径规划只解决“走哪条路”的问题，而轨迹规划解决“以什么速度、什么时候到达”的问题。
| 维度 | 路径（Path） | 轨迹（Trajectory） |
|---|---|---|
| 数学表达 | P(s) = [x(s), y(s), θ(s)] | T(t) = [x(t), y(t), θ(t), v(t), a(t)] |
| 参数 | 弧长 s | 时间 t |
| 包含信息 | 几何形状 | 几何 + 运动状态 |
| 控制输入 | 无法直接使用 | 可直接用于跟踪控制 |
| 典型算法 | A*, RRT, 样条曲线 | 时间弹性带（TEB）、模型预测控制（MPC） |
判断方法与操作流程
在工程中，判断一个规划模块输出的是路径还是轨迹，只需检查数据结构中是否包含时间戳或速度字段。操作流程：
- 路径生成：使用图搜索或采样算法生成候选路径。
- 速度规划：在路径上分配速度曲线，考虑障碍物距离、限速、舒适性约束。
- 时间分配：将速度曲线积分得到时间戳，形成轨迹。
- 可行性检查：验证轨迹的加速度是否超过车辆极限。
案例拆解：城市道路跟车场景的轨迹生成
某城市NOA系统，车辆以60 km/h巡航，前方100米处检测到静止障碍物。初始目标：生成一条安全且舒适的减速轨迹。关键输入：当前速度16.7 m/s，障碍物距离100m，最大减速度3 m/s²，舒适减速度1.5 m/s²。分析过程：如果只生成路径（保持车道中心线），不规划速度，控制模块会以恒定速度行驶直到接近障碍物才紧急制动，导致乘客严重不适。操作步骤：在路径上应用梯形速度规划，先以1.5 m/s²减速至0，计算所需距离为 v²/(2a) = 16.7²/(2*1.5) ≈ 93m，剩余7m作为安全余量。结果对比：生成轨迹的最大减速度为1.5 m/s²，乘客体感良好，且停车位置距障碍物5m。复盘经验：速度规划必须考虑舒适性约束，不能只追求最短时间。
2.4 规划问题数学建模：目标函数与约束的统一框架
核心原理
规划问题本质上是一个带约束的优化问题。数学建模需要定义状态变量、控制变量、目标函数和约束条件。状态变量通常为车辆位姿和速度，控制变量为加速度和前轮转角。目标函数需要权衡多个指标：安全性（与障碍物的距离）、效率（到达时间或平均速度）、舒适性（加速度变化率，即加加速度jerk）、能耗（加速度的平方积分）。约束条件包括：车辆运动学/动力学约束、道路边界约束、障碍物碰撞约束、速度限制、加速度限制。
min J = w₁ * Σ(1/d_obs) + w₂ * T_total + w₃ * Σ(jerk²) + w₄ * Σ(a²)
s.t. x(k+1) = f(x(k), u(k)) // 运动学模型
x(k) ∈ X_free // 状态可行域
u(k) ∈ U // 控制可行域
d(x(k), obs) > d_min // 安全距离判断方法与操作流程
建模时需根据场景选择合适的优化框架：
- 离散优化：将时间离散化，使用数值优化方法（如SQP、IPOPT）求解。
- 图搜索：将状态空间离散化为网格，使用A*或Dijkstra搜索。
- 采样方法：随机生成候选轨迹，通过代价函数评估选择最优。
操作流程：首先明确优化变量和约束，然后选择求解器，最后在仿真中调整权重系数。权重调整通常采用层次法：先保证安全权重最大，再优化效率，最后微调舒适性。
案例拆解：交叉路口左转场景的轨迹优化
某无保护左转场景，车辆需要在对向车流中寻找间隙完成左转。初始目标：在保证安全的前提下，最小化通过时间。关键输入：对向车辆速度12 m/s，距离交叉口30m，车辆最大加速度2 m/s²，最大减速度4 m/s²。分析过程：将问题建模为最优控制问题，状态量为 (x, y, θ, v)，控制量为 (a, δ)。约束包括：车辆必须保持在车道边界内，与对向车辆距离大于1.5m。操作步骤：使用MPC框架，预测时域2秒，控制周期100ms。目标函数中安全权重设为10，效率权重设为1，舒适权重设为0.5。求解后得到轨迹：先加速至8 m/s，在距对向车辆5m时开始左转，同时减速至5 m/s完成转弯。结果对比：与保守策略（等待对向车完全通过）相比，通过时间从8秒缩短至4.5秒，且未发生碰撞风险。复盘经验：数学建模的关键在于约束的准确表达，特别是动态障碍物的时空占用，需要将障碍物预测位置作为时变约束加入优化问题。
总结复盘
本章四个模块环环相扣：坐标系转换提供统一的空间基准，运动学模型描述车辆运动规律，路径与轨迹的区分明确了规划输出的完整性，数学建模则将规划问题转化为可求解的优化形式。工程落地中，建议按以下顺序进行自检：
- 确认所有输入数据（障碍物、地图、定位）处于同一坐标系且时间同步。
- 验证运动学模型在目标车速范围内的精度。
- 检查规划输出是否包含完整的时间-速度信息。
- 在仿真中调整优化权重，确保安全约束始终满足。
只有将这四个基础打牢，后续的采样规划、优化规划、学习规划等方法才能在实际车辆上稳定运行。