Apollo规划模块整理

TrajectoryStitcher轨迹拼接算法解析

轨迹拼接的代码所在路径为：apollo/modules/planning/common/trajectory_stitcher 。在几种情况下，会重新以车辆当前位置生成拼接轨迹:

1. 自动驾驶模式关闭时
2. 没有上一帧的轨迹时
3. 轨迹拼接的flag标志量被关闭时
4. 车辆当前位置与目标点的位置相差过大时 （默认参数为横向差距0.5m，纵向差距2.5m）
5. 当当前时间小于上一轨迹的初始时间时（不太清楚，这种情况什么时候会发生？当规划频率出现问题的时候？）
6. 当当前时间大于上一轨迹的最终时间时
7. 当该时刻无法在上一帧轨迹中找到匹配点时

从该函数中我们可以看出，在起步时，直接使用当前车辆状态作为规划起始点；非起步时，根据车辆运动学模型，往前推planning_cycle_time时间，得到下一时刻的预测状态，再转为reinit_point作为规划起始点。最后该函数返回值为size为1的数组，即该条拼接轨迹仅有一个轨迹点。

若以上条件都不满足，则进行轨迹拼接。首先，根据 （当前时刻的绝对时间-上一时刻轨迹的初始时间+planning_cycle_time） 得到forward_rel_time，在上一帧的轨迹上截取出往前20个点开始，至forward_rel_time的一段轨迹，作为stitching_trajectory。最后，遍历这段轨迹，对其relative_time和s进行赋值。最终，使用stitching_trajectory的最后一个点，作为当前帧轨迹规划的起始点。