Lattice算法在自动驾驶决策规划中的动态障碍物处理实践-育师

1. 项目背景与核心价值

自动驾驶决策规划算法是车辆智能化的核心大脑，而Apollo开源平台中的Lattice算法因其结构化道路场景下的出色表现，成为工业界广泛采用的解决方案。最近在调试某量产车型的变道逻辑时，我发现官方文档对动态障碍物处理部分的实现细节着墨有限，这促使我决定深入源码层面进行系统性分析。

Lattice算法的精髓在于将复杂的路径规划问题转化为Frenet坐标系下的轨迹采样与优化过程。与传统笛卡尔坐标系相比，Frenet系通过投影将车辆运动分解为沿参考线方向和垂直方向，这种解耦特性特别适合处理结构化道路场景。但在实际项目中，动态障碍物的交互预测常常成为算法落地的瓶颈点。

2. 环境搭建与调试技巧

2.1 本地调试环境配置

推荐使用Ubuntu 20.04 + Docker组合搭建Apollo 6.0环境，这个版本对Lattice算法进行了重要优化。通过以下命令可快速拉起开发环境：

git clone https://github.com/ApolloAuto/apollo cd apollo bash docker/scripts/dev_start.sh

关键依赖项需要特别注意：

Protobuf 3.14.0（版本不匹配会导致编译失败）
Eigen 3.3.7（线性代数运算核心库）
GFlags 2.2.2（命令行参数解析）

经验：在docker容器内安装gdb和cgdb调试工具，配合VSCode的远程开发插件，可以实现源码级单步调试，这对理解算法执行流程至关重要。

2.2 数据回放与可视化

使用Apollo的record数据回放功能时，建议先通过cyber_monitor工具检查channel状态：

cyber_monitor -c /apollo/planning

对于动态障碍物分析，需要同步查看以下关键topic：

/apollo/perception/obstacles（感知原始数据）
/apollo/prediction（预测模块输出）
/apollo/planning（规划决策结果）

通过Dreamview的Layer菜单开启"Planning Debug"视图，可以直观看到：

绿色轨迹线：采样生成的候选轨迹
红色区域：动态障碍物的ST投影
蓝色框：最终选择的最优轨迹

3. 核心算法实现解析

3.1 轨迹采样策略

在modules/planning/lattice/trajectory_generation中，Trajectory1dGenerator类实现了四种采样策略：

enum class SampleType { LON_STOP = 0, // 纵向停止采样 LON_CRUISE = 1, // 纵向巡航采样 LON_FOLLOW = 2, // 纵向跟车采样 LAT_DIRECTION = 3 // 横向偏移采样 };

动态障碍物处理的核心在于LON_FOLLOW模式下的QP（二次规划）问题构建：

# 代价函数示例 cost = w1 * jerk + w2 * time + w3 * obs_dist s.t. s(t) > s_obs(t) + safe_margin # 安全约束

权重系数调参经验：

城市道路：w1=0.8, w2=0.1, w3=0.1（强调舒适性）
高速公路：w1=0.5, w2=0.3, w3=0.2（平衡效率与安全）

3.2 动态障碍物投影

DynamicObstacleProcessor类将三维障碍物映射到ST图的关键步骤：

预测模块提供的轨迹进行五次多项式拟合
计算障碍物轮廓的凸包在Frenet系的投影
生成时间膨胀的安全边界（含不确定性补偿）

实测中发现对两轮车的处理需要特别注意：

// 增加两轮车的横向不确定性补偿 if (obstacle.type() == PerceptionObstacle::BICYCLE) { boundary.lateral() += 0.3; // 额外增加30cm }

4. 典型问题排查实录

4.1 轨迹抖动问题

现象：车辆在跟车时出现横向摆动排查步骤：

检查prediction_obstacles中的confidence值
验证QP问题的约束条件是否过松
分析ST图中障碍物投影是否跳变

解决方案：

# 修改planning.conf参数 lattice_planner_config { trajectory_resolution: 0.2 # 从0.5调整为0.2秒 lon_collision_buffer: 2.5 # 增大安全距离 }

4.2 变道犹豫问题

根本原因：动态障碍物的ST投影重叠导致代价函数突变优化方法：

引入障碍物交互意图预测（cut-in概率）
使用滑动窗口平滑代价函数
增加变道决策的hysteresis阈值

代码修改点：

// 在PathTimeGraph中添加意图判断 if (obs.relative_v() < -2.0 && distance < 20.0) { obs.set_cut_in_probability(0.7); }

5. 进阶调试技巧

5.1 自定义场景测试

通过修改modules/common/data/map目录下的demo地图，可以构造特定测试场景：

<scenario> <ego car="true" start="100,50" end="200,50"/> <obstacle type="car" trajectory="(120,55)->(130,55)" speed="8"/> </scenario>

5.2 实时参数调优

利用Cyber RT的parameter服务动态调整参数：

cyber_param set /apollo/planning/lattice_weight 0.8

关键可调参数监控表：

参数名	影响范围	推荐值区间
planning_distance	规划视野长度	120-200m
lon_collision_buffer	纵向安全距离	2.0-3.0m
max_acceleration	舒适性限制	2.5-3.5m/s²
trajectory_resolution	轨迹点密度	0.1-0.3s

6. 性能优化实践

在量产项目中发现的几个关键性能瓶颈点：

轨迹采样数量优化：

// 原版均匀采样改进为动态密度采样 if (has_dynamic_obs) { sample_num = 7; // 密集采样 } else { sample_num = 3; // 稀疏采样 }

并行化改造：

# 在planning.dag中增加线程池配置 process_config { pipe_conf { name: "LatticePlanner" workers: 4 # 利用多核加速 } }

预测结果缓存：

// 对稳定跟踪的障碍物启用轨迹预测缓存 if (obs.tracking_time() > 2.0) { use_cached_prediction = true; }

经过上述优化后，在Intel i7-1185G7处理器上实测：

平均计算耗时从58ms降至32ms
99分位耗时从126ms降至71ms
CPU占用率降低40%

7. 实车调试注意事项

时钟同步问题：

# 在所有ECU上执行时间同步 sudo ptpd -i eth0 -G -u /var/run/ptpd.pid

延迟补偿方法：

# 在PostProcessor中添加运动补偿 compensated_trajectory = [ point for point in trajectory if point.relative_time > current_latency * 1.2 ]

紧急避障策略：

// 在ReferenceLineProvider中添加安全校验 if (emergency_flag) { reference_line.Smooth(FLAGS_emergency_smooth_factor); }

实际路测中总结的黄金法则：

首次测试选择晴天、干燥路面
初始速度控制在30km/h以下
提前准备紧急停止开关
记录完整的CAN总线数据备查

8. 扩展应用方向

基于Lattice算法框架可以进一步开发：

特殊场景增强：

// 施工区域处理 if (is_construction_zone) { config.set_lon_stop_distance(5.0); }

V2X协同规划：

message V2XTrajectory { optional apollo.common.Trajectory trajectory = 1; optional double confidence = 2 [default = 0.9]; }

学习型参数调优：

# 使用贝叶斯优化自动调参 optimizer = BayesianOptimization( f=objective_function, pbounds={'w1': (0.1, 1.0), 'w2': (0.0, 0.5)} )

Lattice算法在自动驾驶决策规划中的动态障碍物处理实践