自动驾驶---Motion Planning之构建SLT Driving Corridor

1 背景

        在上篇博客《自动驾驶---Motion Planning之Speed Boundary》中，主要介绍了Apollo中Speed Boundary的一些内容，可以构造ST图得到边界信息，最后结合粗糙的速度曲线和路径曲线，即可使用优化的方法求解得到最终的轨迹信息（s，s'，s''，l，l'，l''）。

        本篇博客笔者主要介绍近几年开始逐渐投入到量产中的方法：SLT同步构造。本质上与Apollo的方案有相似之处。

        本篇博客的主要参考文献为《Safe Trajectory Generation for Complex UrbanEnvironments Using Spatio-temporalSemantic Corridor》，香港科技大学大疆联合实验室为第一作者。

2 STSC介绍

        什么是STSC（论文中简称为SSC）呢？可以理解为：时空联合走廊。

        Spatio-Temporal Semantic Corridor（STSC）是一个在复杂城市环境中用于轨迹规划和导航的概念。它主要利用时空语义信息来生成一个安全可行的轨迹走廊，使得车辆可以在这个走廊中安全、有效地行驶。以下是关于STSC的详细介绍：

（1）概念

        STSC是一个在三维时空中的轨迹走廊，其中包含了语义信息，如道路标记、交通信号、行人、车辆等。这些语义信息有助于车辆更好地理解其周围环境，并做出合适的驾驶决策。

（2）生成过程

• 种子生成：首先，根据车辆的前向预测模块生成一系列离散的点作为种子。
• Cube膨胀：然后，以这些种子点为中心，通过cube膨胀的方式逐渐向外扩展，直到碰到障碍物或语义边界为止（cube的ST剖面构建如下图中的a-b-c-d的顺序，LT剖面的构建同理可得，细节可参考论文）。
• 确定约束：在膨胀过程中，需要确定各种约束条件，如速度约束、停止线约束、变道时间约束等。这些约束条件确保了生成的轨迹既安全又可行。
• 保留约束：在生成走廊的过程中，只保留硬约束（如速度约束）和无碰撞约束，而放松其他允许的最大余量。

 构建Cube的代码如下（整体思路如上所述，也比较容易理解）：

// 根据输入的轨迹点来构建cube
ErrorType SscMap::ConstructCorridorsUsingInitialTrajectories(GridMap3D *p_grid, const vec_E<vec_E<common::FsVehicle>> &trajs) {GetSemanticVoxelSet(p_grid);driving_corridor_vec_.clear();int trajs_num = trajs.size();if (trajs_num < 1) return kWrongStatus;// ~ Stage I: Get seedsvec_E<vec_E<Vec3i>> seeds_vec;for (int i = 0; i < trajs_num; ++i) {common::DrivingCorridor3D driving_corridor;vec_E<Vec3i> traj_seeds;int num_states = static_cast<int>(trajs[i].size());if (num_states > 1) {bool first_seed_determined = false;for (int k = 0; k < num_states; ++k) {std::array&