Apollo轨迹拼接模块(Trajectory Stitching)研读

[TOC]
分析 Apollo 中的 Trajectory Stitching 模块, 参考 Replan 的逻辑.

思路介绍

• 背景：需要在每个运行周期都进行完整的规划吗? 答案是否定的.
  理论上来说, 规划的算法应该具有时间一致性, 即如同数学当中的函数的概念一样, 只要输入一致, 输出是确定并且可重复的, 尤其是在涉及到时间维度上的输出, 在任意时间点上的输入都是连续的, 如同动态规划算法里的最优性原理. 然而由于现实中存在输入的噪声, 执行端出现误差或者延迟, 甚至是算法本身的选择上, 会导致之前某个时刻的规划结果与实际的输出差别很大, 这会导致继续执行下去会导致一些可能的坏结果. 例如误差越来越大无法收敛. 或者重新执行算法导致突变, 对控制器产生极大的挑战, 也会造成乘客的不舒适性. 因此我们需要在每个执行周期内判断之前算法的结果与实际效果差异度如何, 不大的话可以直接使用之前的结果, 不用每时每刻都运行算法, 节省计算资源. 大的话, 根据误差再调整相应的策略进行重新规划(类似于控制工程中的反馈与轨迹追踪).

换句话说只有满足了一定条件才需要进行重新规划, 也就是 Replan.
如果不需要 Replan 直接查点截取上次轨迹点即可(可能要自车坐标系转换一下). 如果需要 Replan, 需要在自车动力学模型航迹推算本次时间戳 + 运行周期后的点的位置, 进行规划. 一图胜千言:

Apollo 里实际的思路见下图

• 参考：Apollo 官方问答

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

代码分析

TrajectoryStitcher 类头文件如下.

class TrajectoryStitcher {
 public:
  TrajectoryStitcher() = delete;

  static void TransformLastPublishedTrajectory(
      const double x_diff, const double y_diff, const double theta_diff,
      PublishableTrajectory* prev_trajectory);

  static std::vector<common::TrajectoryPoint> ComputeStitchingTrajectory(
      const common::VehicleState& vehicle_state, const double current_timestamp,
      const double planning_cycle_time, const size_t preserved_points_num,
      const bool replan_by_offset, const PublishableTrajectory* prev_trajectory,
      std::string* replan_reason);

  static std::vector<common::TrajectoryPoint> ComputeReinitStitchingTrajectory(
      const double planning_cycle_time,
      const common::VehicleState& vehicle_state);

 private:
  static std::pair<double, double> ComputePositionProjection(
      const double x, const double y,
      const common::TrajectoryPoint& matched_trajectory_point);

  static common::TrajectoryPoint ComputeTrajectoryPointFromVehicleState(
      const double planning_cycle_time,
      const common::VehicleState& vehicle_state);
};

• TransformLastPublishedTrajectory：把上一时刻的规划轨迹 PublishableTrajectory 中的每个点进行坐标转换(自车位置变化, 转换坐标在两帧自车坐标系中). 官方注释: only used in navigation mode, 如此设计的理由, 我猜测是因为高速等场景除了换道/side pass 一般情况下前后帧的自车位置变化不需要进行轨迹点的坐标系转换.
• ComputeStitchingTrajectory：核心成员函数, 负责决定哪些情况下需要 Replan,随后进入 ComputeReinitStitchingTrajectory 成员函数.不 Replan 的话, 截取上次轨迹的一段作为规划轨迹发出去.
  需要进行 Replan 的情况如下:
  1. 配置文件：stitch is disabled by gflag.
  2. 不存在上次规划轨迹：replan for no previous trajectory.
  3. 完成自动驾驶模式, 退出到手动模式: canbus::Chassis::COMPLETE_AUTO_DRIVE. 我的理解是下降沿 Replan, 为什么不是上升沿的原因是刚进入规划应该用正常的 Plan 算法, 而不是上来就 Replan.
  4. 上次规划轨迹中的点数目为0：replan for empty previous trajectory.
  5. 本次规划开始时间小于上次规划轨迹初始时间：current time smaller than the previous trajectory’s first time.
  6. 本次规划开始时间大于上次规划轨迹结束时间：replan for current time beyond the previous trajectory’s last time.
  7. 本次规划时间戳对应的上次规划轨迹的时间戳位置处的点为空: replan for previous trajectory missed path point.
  8. 先找到本次定位点距离上次轨迹最近的点–匹配点QueryNearestPointWithBuffer(), 通过ComputePositionProjection 成员函数计算横纵向偏差, 偏差太大的情况下：the distance between matched point and actual position is too large.
  9. 进入缝合阶段, 但是得到的轨迹 stitching_trajectory 中不含轨迹点(代码健壮性?).

不 Replan, 截取轨迹 stitching_trajectory index 范围为: 本次匹配点 index-preserved_points_num向前倒的点数 (20)~`veh_rel_time + planning_cycle_time` 处的点 index, 即前文 T + dt 处点. 最后再把 stitching_trajectory 每个点的 relative_time 与累积距离 s$(0-\Delta s)$ 计算后发布出去.

• ComputeReinitStitchingTrajectory: 此函数返回值 std::vector<common::TrajectoryPoint> 中只有一个点, 即 Replan 规划起始点. 自车速度与加速度较小时, 不需要航迹推算, 直接用 ComputeTrajectoryPointFromVehicleState 自车当前位置点作为 Replan 规划起始点即可(此处有疑问: 对于泊车尤其是狭小空间内的泊车, 这个思路误差有可能会不会太大). 否则利用 VehicleModel::Predict(planning_cycle_time, vehicle_state) 函数进行航迹推算得到 Replan 规划起始点.

代码：https://github.com/ApolloAuto/apollo/blob/r6.0.0/modules/planning/common/trajectory_stitcher.cc
参考文章: https://zhuanlan.zhihu.com/p/390229961