自动驾驶泊车轨迹优化：Apollo工程实践解析

1. 泊车轨迹优化背后的工程挑战

深夜调车是自动驾驶工程师的日常，而泊车场景恰恰是最考验轨迹优化算法的试金石。最近在调试Apollo的valet parking功能时，发现其轨迹优化模块藏着不少精妙设计。不同于教科书式的理想化方案，这些实现处处体现着工程团队对实际场景的深刻理解。

泊车轨迹优化的核心矛盾在于：如何在有限的计算时间内，平衡平滑性、安全性、舒适性三大指标。传统方案往往采用单一的优化目标，而Apollo的创新之处在于将问题拆解为三个相互制约的子系统：

• 参考线生成模块（Reference Line Provider）
• 分段优化器（Piecewise Jerk Optimizer）
• 非线性优化器（QP Solver with Constraints）

这三个模块通过特定的数据接口和状态机机制协同工作，形成了类似"三权分立"的架构。下面我们就深入代码层面，看看它们如何通过"打架"最终达成统一。

2. 核心模块解析与交互机制

2.1 参考线生成：全局视野的路径规划

在modules/planning/reference_line/中，DiscretizedReferenceLine类负责将高精地图的车道信息转化为可计算的离散点列。其关键创新在于：

1. 动态采样策略：根据曲率变化自动调整点距（0.1m-0.5m），在转弯处加密采样

cpp复制// 自适应采样算法核心逻辑
double step = std::max(0.1, 1.0 - curvature * kCurvatureFactor); 

2. 缓冲区机制：预生成多条候选参考线（主车道+左右偏移线），为后续优化留出裕度

实际调试中发现，当初始位姿误差较大时，需要适当增大缓冲区横向偏移量（建议0.3-0.5m）

2.2 分段加加速度优化：平衡舒适与效率

PiecewiseJerkPathOptimizer采用独特的"分而治之"策略：

1. 将路径按纵向距离划分为5-8个阶段（视场景复杂度而定）
2. 每个阶段独立优化加加速度（Jerk）指标
3. 通过边界条件约束保证段间连续性

优化目标函数设计值得玩味：

code复制min Σ(wi·xi² + wj·jerk_i²) + w_c·collision_cost

其中权重系数wi、wj会根据车辆状态动态调整：

• 低速时wj增大（提升舒适性）
• 狭窄空间wi增大（增强路径跟踪精度）

2.3 带约束的二次规划：安全边界的最后防线

QP Solver通过以下约束条件确保物理可行性：

1. 车辆运动学约束（最大转向角/转向速率）
2. 障碍物避碰约束（膨胀后的安全距离）
3. 曲率连续性约束（防止方向盘突变）

在代码中体现为：

cpp复制// 构建约束矩阵示例
constraints.emplace_back(variable_index, 
                        std::make_pair(-FLAGS_max_steer_angle, FLAGS_max_steer_angle));

3. 模块间的"打架"与协调

3.1 迭代优化中的博弈过程

三个模块通过多次迭代达成共识：

1. 参考线提供初始解
2. 分段优化器进行粗调
3. QP求解器微调并反馈约束违反情况
4. 根据QP结果调整参考线权重

这个过程中会出现典型的冲突场景：

• 分段优化追求舒适性导致路径偏离参考线
• QP求解因障碍物约束无法找到可行解
• 参考线采样不足导致优化陷入局部最优

3.2 工程实践中的调参技巧

经过大量实车测试，总结出几个关键参数调节经验：

特别注意：jerk_weight与车速的关系曲线建议用sigmoid函数平滑过渡，避免参数突变导致抖动

4. 典型问题排查实录

4.1 轨迹抖动问题排查流程

1. 检查分段优化器输出是否平滑
2. 验证QP求解器的约束裕度是否足够
3. 分析参考线曲率连续性
4. 确认控制模块的跟踪误差

常见根本原因：

• 参考线采样点距不均匀
• 优化阶段划分与曲率变化不匹配
• 约束条件过于严格导致求解器振荡

4.2 优化超时处理方案

当单次计算超过100ms时：

1. 降级策略：减少优化阶段数量
2. 应急方案：回退到上一周期可行解
3. 长期优化：预计算典型场景的参考解

5. 从代码细节看设计哲学

在modules/planning/constraint_checker.cc中，发现一个耐人寻味的处理逻辑：

cpp复制// 允许轻微违反约束的条件判断
if (constraint_violation < kSoftConstraintThreshold) {
    return Status::OK(); 
}

这体现了自动驾驶系统设计的核心原则：在严格的安全底线之上，允许适度的优化目标妥协。就像老司机停车时也不会完全遵循理论最优路径，而是在多个因素间寻找实操平衡点。

调试过程中最深刻的体会是：优秀的轨迹优化算法不是追求数学上的完美，而是构建容错性强、可解释性好的多模块协作体系。那些看似"打架"的模块交互，实则是通过约束和反馈不断逼近可行解的智能过程。