大家好，我已经把CSDN上的博客迁移到了知乎上，欢迎大家在知乎关注我的专栏慢慢悠悠小马车（https://zhuanlan.zhihu.com/duangduangduang）。希望大家可以多多交流，互相学习。

---

在百度Apollo Lattice轨迹规划算法中，针对跟车（follow）和超车（overtake）2种不同的场景，使用了ST图在纵向上来采样障碍物车辆周边的目标点，作为拟合轨迹的终点。跟车时，在ST图中障碍物下方采样，即自车速度小于并趋近于障碍物车速度；超车时，在ST图中障碍物上方采样，即自车速度大于障碍物车速度。至于横向采样，因为参考线会切换到目标车道中心线，故横向一般围绕0（参考线即s轴）左右对称采样，如 [-0.5，0，0.5]。

因为Lattice Planner并没有对变道场景单独处理，常会发生变道轨迹规划失败的情况。提升变道成功率，便成了非常重要的任务。

在做变道时的轨迹规划时，我查到有些论文采用了建立变道评估模型以选择最佳变道插入间隙（gap）的方法，以指导轨迹规划的目标点所在区间。对比这个思路和Lattice的思路，这种模型在规划前提前筛选出了最优区间，减少了采样点数目，降低了规划的计算量，同时，采样点的减少，却也降低了最终规划出的轨迹的最优性。因此，我觉得Lattice是对这个思路的泛化，也启发了我使用ST图规划变道轨迹的方法设计。

Lattice中，ST图用来描述自车所在车道中的障碍物纵向运动情况，也就是纵向速度。在变道场景中，我们需要用ST图反映变道目标车道的障碍物速度。假设自车前方没有障碍物（在自车车道上）（图1），目标车道的ST图如图2，则可以在  区间内、且无障碍物占据的空白处采样，作为自车可到达的目标状态。然后由cost functions评价哪一条轨迹是最优轨迹。因为高速场景中自车不能倒车，故s<0的情况可以忽略。采样前，也可以对欲采样区间的长度进行检验，过短的区间可直接忽略。

若自车E前方有障碍物D，如图3所示，且与该障碍物横向相邻区间内无障碍物，则E依然可以在纵向采样时到达A-B、B-C之间的间隙（A-D、B-D之间的纵向距离要大于某个距离阈值，否则自车E无法安全的穿过间隙）。即障碍物D对于上述采样过程没有影响。

若自车E前方有障碍物D，如图4所示，且与该障碍物横向相邻区间内有障碍物B（或B-D虽然不紧密相邻、但相距较近，小于某距离阈值），则B-D“卡”住了E向前纵向采样的区间上限，该上限为障碍物B的车尾纵向坐标。自车只能在目标车道的B车尾后方范围内纵向采样。

因为ST图只反映一条车道的纵向运动信息，对于B-D的考察，则把相邻2条车道的横向限制纳入进来，使得横纵向信息结合起来。

按照上述思路进行轨迹生成，有2种可能会导致规划失败，一是采样区间都不符合长度限制，没有有效的采样点；二是生成的轨迹无法通过有效性检测（如加速度和jerk的限制）和碰撞检测。当变道轨迹规划失败时，自车应该继续沿当前车道行进，继续寻找变道的机会。为实现这一点，在按上述策略在目标车道采样后，增加对自车车道的横纵向采样，并区别对待2车道采样的优先级。

算法实现后的仿真效果：