轨迹拼接(Trajectory Stitching)

轨迹拼接(Trajectory Stitching)

理论上来说, 规划的算法应该具有时间一致性, 即如同数学当中的函数的概念一样, 只要输入一致, 输出是确定并且可重复的。然而由于现实中存在输入的噪声, 执行端出现误差或者延迟, 甚至是算法本身的选择, 会导致车辆执行的实际输出与规划结果差别很大，最终导致不同时刻规划的车辆轨迹有差异。

对于控制来说，连续稳定的轨迹更利于控制下发指令的平滑性，避免车辆产生抖动。

因此在每一个运行周期内，需要判断车辆实际执行效果与规划效果的差异，当差异不大时，直接使用上一个运行周期的规划结果；差异比较大时，才会发起RePlan(重新规划)。

实际测试表明，轨迹拼接(Trajectory Stitching)确实能够使实车的控制更加平滑。

Apollo中的轨迹拼接

Apollo官方的一些分享:

Q：每个实时规划的初始状态量,比如 s、速度、加速度等是以车体底盘实时反馈为主还是从组合导航获得, 还是说通过一定方式从上帧规划结果获得参考量? 交给控制去执行的连续两帧轨迹如何联系起来, 才能保证控制模块在连接处速度、加速度、曲率等不发生突变？ A：这个问题非常好, 在今天的分享中没有专门的介绍. 我这里简要描述一下, 车辆的状态是由上游的定位模块获得的, 融合了多种传感器的数据, 包括当前地图坐标系下的坐标, 朝向, 转向角度, 速度, 加速度等等。 轨迹规划模块以固定的频率进行, 我们使用了轨迹拼接的算法(Trajectory Stitching)保证相邻帧的轨迹在控制器看来是平滑的。 假设我们的周期时间是 dt 秒, 如果我们没有上一周期的轨迹, 那我们使用运动学模型, 对当前从定位模块获得的车辆状态进行外推, 获得 dt 时间之后的状态作为规划起始点, 我们称之为重新规划(Replan)； 如果上一周期的轨迹存在, 我们会根据当前系统时间 T, 在上一周期的轨迹中找到相对应的轨迹点, 然后我们进行一个比较, 比较这个轨迹点与定位模块获得的当前车辆状态的差异, 如果这个差异在一定范围内, 我们找到 T + dt 时间的上一周期轨迹点作为规划起始点；如果这个差异超过设定范围, 说明控制器有了较大的误差, 我们会做第一种情况的 replan. 这种机制保证了在控制误差允许的情况下, 做到相邻帧轨迹的平滑拼接. 在控制器看起来, 规划模块发出的轨迹是一小段一小段 dt 长度的轨迹光滑拼接起来的. Q：为什么每次规划时, 不以车辆当前的状态为规划起始点呢, 而是"找到 T + dt 时间的上一周期轨迹点作为规划起始点"？ A: 因为规划结果真正送到控制器是 T + dt 时刻。

Apollo中Trajectory Stitching的具体实现：

图片来源【1】

参考资料

1.https://www.chuxin911.com/apollo_trajectory_stitcher_intro_20210926/

- END -