理论上来说, 规划的算法应该具有时间一致性, 即如同数学当中的函数的概念一样, 只要输入一致, 输出是确定并且可重复的。然而由于现实中存在输入的噪声, 执行端出现误差或者延迟, 甚至是算法本身的选择, 会导致车辆执行的实际输出与规划结果差别很大,最终导致不同时刻规划的车辆轨迹有差异。
对于控制来说,连续稳定的轨迹更利于控制下发指令的平滑性,避免车辆产生抖动。
因此在每一个运行周期内,需要判断车辆实际执行效果与规划效果的差异,当差异不大时,直接使用上一个运行周期的规划结果;差异比较大时,才会发起RePlan(重新规划)。
实际测试表明,轨迹拼接(Trajectory Stitching)确实能够使实车的控制更加平滑。
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 -