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的代码如下(整体思路如上所述,也比较容易理解):
// 根据输入的轨迹点来构建curb
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 seeds
vec_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::arra