自动驾驶---Motion Planning之轨迹拼接

1 背景

        笔者在之前的专栏中已经详细讲解了自动驾驶Planning模块的内容：包括行车的Behavior Planning和Motion Planning，以及低速记忆泊车的Planning。

        本篇博客主要聊一聊Motion Planning中轨迹拼接的相关内容。从网络上各大品牌的车主拍摄的智驾视频来看，在开启自动驾驶过程中，车辆多多少少会出现 “画龙” 现象，原因主要包括：感知误差，path超调，轨迹平滑性等（感知误差和path超调本篇不做详细说明）。  

2 内容介绍

2.1 什么是“画龙”

        在讲解轨迹拼接（Trajectory Stitching）之前，先引入一个自动驾驶量产过程中会出现的一个普遍现象：“画龙”。那么什么是“画龙”呢？

        在自动驾驶中，“画龙” 现象是指车辆在辅助驾驶状态下无法稳定居中行驶，而是会向右或左偏移方向的现象。当车辆发生“画龙”现象时，驾驶的舒适性和安全性都会受到影响，这不仅可能导致乘客感到不适，还可能增加发生事故的风险。因此，解决自动驾驶量产中的“画龙”现象是一个重要的挑战。

2.2 “画龙”的原因

2.2.1 传感器及感知

        自动驾驶车辆出现“画龙”现象的原因主要是由于传感器的误判或者环境感知不准确所导致的。具体来说，可能的原因包括：

• 传感器误判：自动驾驶车辆通常通过激光雷达、摄像头、雷达等传感器来感知周围环境，如果传感器出现故障、污垢或者受到干扰，就有可能导致“画龙”现象，即车辆错误地识别周围环境或者障碍物。
• 地图数据不准确：自动驾驶车辆通常会使用高精度地图来辅助定位和导航，如果地图数据不准确或者更新不及时，就可能导致车辆在实际行驶中出现偏离预期路径的情况。
• 复杂路况：某些复杂的路况，如交通拥堵、施工路段、复杂的道路标志等，可能会对自动驾驶系统造成困扰，导致车辆无法准确地规划行驶路径。
• 环境变化：天气变化、光照条件变化等因素都可能影响自动驾驶系统的性能，导致系统无法准确感知周围环境，从而出现“画龙”现象。

        比如感知车道线或者动态车辆的决策或者预测轨迹出现偏差，导致Planning的约束构建出现一些误差，也会使得车辆横向的控制来回变化，间接导致 “画龙” 现象。

2.2.2 规划

        在本篇博客中，笔者主要探讨Motion Planning层面引起的 “画龙” 问题，其主要原因为：轨迹拼接不连续，导致轨迹不够平滑（如下图所示，图画的稍微夸张了一些）。为了实现比较好的轨迹平滑性，通常需要进行轨迹拼接的处理。

2.3 轨迹拼接

        轨迹拼接在自动驾驶中扮演着重要角色，能够保证车辆平稳、安全、高效地行驶。在自动驾驶量产任务中，轨迹拼接是Planning过程中的一个重要步骤，其主要有两个原因：

        平滑性： 轨迹拼接可以确保车辆在行驶过程中的轨迹是平滑的，避免了急刹或急加速等突变动作，提升了乘客的舒适度。

        连续性： 轨迹拼接可以保证车辆的轨迹在不同规划阶段之间是连续的，避免了不连续的轨迹对车辆行驶造成的不稳定性。

        综合来看，轨迹拼接体现在车上也包括两个方面：纵向相关（速度、加速度的连续性）以及横向相关（方向盘的稳定性）。

2.3.1 Replan机制

        对于Control模块来说，Planning模块能够发送连续稳定的轨迹，是有利于控制去处理轨迹信息的，避免车辆产生横向的抖动以及纵向的不舒适。

        然后就是replan机制，在高速状态下，replan越少越安全。因此在每一个周期内，planning模块需要计算车辆当前姿态与规划轨迹的偏差，当偏差不大时，直接使用上一个运行周期的规划结果；当偏差比较大时，才需要replan。

2.3.2 轨迹拼接

        轨迹拼接在Planning模块中是一个关键的技术环节，它主要涉及到在连续的时间帧内，将规划出的局部轨迹进行有效地连接，以形成一条平滑且符合行驶要求的整体轨迹。以下是对轨迹拼接的详细介绍：

（1）起始点的确定

        在每个轨迹规划的开始，都需要确定一个起始点。这个起始点通常包含了车辆在当前时刻的位置、速度、加速度、航向、曲率等信息。这个起始点的选择对于后续的轨迹生成具有至关重要的影响。在轨迹拼接中，起始点的确定可以基于上一帧的部分有序点集，这样可以确保连续帧之间轨迹的平滑性和连续性。

（2）轨迹生成与拼接        

        根据起始点的信息以及当前帧的环境感知数据（如道路信息、障碍物位置等），规划算法会生成一条局部轨迹。这条轨迹需要与上一帧的轨迹进行拼接，以确保整个行驶过程的连续性和平滑性。在拼接过程中，需要考虑到各种因素，如车辆的动力学特性、行驶的安全性、以及乘客的舒适度等。

（3）平滑性处理

        轨迹拼接的一个重要目标是确保整体轨迹的平滑性。如果轨迹拼接不当，可能会导致车辆在行驶过程中出现不必要的抖动或偏移，这不仅会影响乘客的舒适度，还可能增加事故的风险。因此，在轨迹拼接过程中，需要采用各种算法和技术来处理轨迹的平滑性问题，如使用滤波算法、优化算法等。

（4）适应性与鲁棒性

        自动驾驶车辆在行驶过程中会遇到各种复杂的环境和场景，如道路变化、障碍物突然出现等。因此，轨迹拼接算法需要具备一定的适应性和鲁棒性，能够根据不同的环境和场景动态调整轨迹的生成和拼接方式，以确保车辆的安全和稳定行驶。

        通过有效地进行轨迹拼接，可以确保自动驾驶车辆在行驶过程中的连续性、平滑性和安全性，为乘客提供更加舒适和安全的出行体验。

2.3.3 实现方式

        见下图，车辆上一时刻的轨迹为：last trajectory，车辆实时的位置为：ego，车辆实时位置投影到last trajectory的投影点为：matched point，为了保证控制模块收到的轨迹是稳定的，因此需要基于车辆当前位置和运动学关系（比如方向盘转角和车速）递推得到下一周期的位置，该位置定义为 replan的起始位置，那么repan得到的轨迹为：latest trajectory，车辆不能完全跟踪好的情况下（也确实难保证Control有非常好的跟踪效果），last trajectory和latest trajectory在replan point处是有偏差存在的。所以此时需要轨迹拼接来处理前后两个周期的轨迹的平滑性。

        Apollo源码中的做法：计算好拼接轨迹后，遍历这段拼接轨迹，对其时间戳和s进行赋值，使用stitching_trajectory的最后一个点作为当前帧轨迹规划的起始点（注意：上图中的stitching_trajectory最后一个点在平滑之前一般情况下是不会和replan_point重合的）。选择当前时间+规划周期的时间对应的上一帧轨迹的位置点作为本帧规划起点。

std::vector<TrajectoryPoint> TrajectoryStitcher::ComputeStitchingTrajectory(
    const 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) {
  const double veh_rel_time =
      current_timestamp - prev_trajectory->header_time();

  size_t time_matched_index =
      prev_trajectory->QueryLowerBoundPoint(veh_rel_time);

  size_t position_matched_index = prev_trajectory->QueryNearestPointWithBuffer(
      {vehicle_state.x(), vehicle_state.y()}, 1.0e-6);

  double forward_rel_time = veh_rel_time + planning_cycle_time;

  size_t forward_time_index =
      prev_trajectory->QueryLowerBoundPoint(forward_rel_time);

  ADEBUG << "Position matched index:\t" << position_matched_index;
  ADEBUG << "Time matched index:\t" << time_matched_index;

  auto matched_index = std::min(time_matched_index, position_matched_index);

  std::vector<TrajectoryPoint> stitching_trajectory(
      prev_trajectory->begin() +
          std::max(0, static_cast<int>(matched_index - preserved_points_num)),
      prev_trajectory->begin() + forward_time_index + 1);
  ADEBUG << "stitching_trajectory size: " << stitching_trajectory.size();

  const double zero_s = stitching_trajectory.back().path_point().s();
  for (auto& tp : stitching_trajectory) {
    if (!tp.has_path_point()) {
      *replan_reason = "replan for previous trajectory missed path point";
      return ComputeReinitStitchingTrajectory(planning_cycle_time,
                                              vehicle_state);
    }
    tp.set_relative_time(tp.relative_time() + prev_trajectory->header_time() -
                         current_timestamp);
    tp.mutable_path_point()->set_s(tp.path_point().s() - zero_s);
  }
  return stitching_trajectory;
}

        为了保证平滑的效果更好，通常需要保留一段长度的历史轨迹作为输入。在进行轨迹拼接时，常用的算法包括基于曲线的拼接方法，如贝塞尔曲线、样条曲线等（确定起点和终点的约束求解得到参数方程，再由参数方程可得到坐标，速度，加速度等信息）。

        此外，还有基于优化算法的拼接方法（量产中使用较多，为了保证整个架构的一致性）（参考《自动驾驶---Motion Planning之轨迹Path优化》和《自动驾驶---Motion Planning之轨迹Speed优化》），二次规划等，相当于重新运行了一次Optimizer，这些算法可以优化轨迹，使输出的轨迹更加平滑和舒适。

2.3.2 轨迹拼接的优势

        自动驾驶planning需要进行轨迹拼接（Trajectory Stitching），通过轨迹拼接能提升哪些问题呢？

• 时间一致性与连续性：理论上，规划算法应该具有时间一致性，即输入一致时，输出也是确定且可重复的。然而，由于现实世界的复杂性，包括输入噪声、执行端误差或延迟，甚至是算法本身的选择，都可能导致车辆实际执行的轨迹与规划结果存在差异。轨迹拼接能够确保在不同时刻规划的车辆轨迹之间具有连续性和一致性，使控制指令下发更加平滑，避免车辆产生不必要的抖动。
• 平滑性：在自动驾驶系统中，planning模块输出trajectory信息作为control模块的输入。如果前后拍的轨迹不够平滑，会引起控制的抖动，实际表现就是方向盘抖动或者整车画龙。轨迹拼接有助于确保轨迹的平滑性，从而提高驾驶的舒适性和安全性。
• 适应复杂场景：自动驾驶车辆需要处理的场景非常繁杂，包括空旷的道路、与行人或障碍物共用的道路、十字路口等。在这些场景中，自动驾驶车辆可能需要执行一系列简单的行为来完成复杂的驾驶任务。轨迹拼接能够帮助将这些简单的行为组合起来，形成连贯且符合道路交通规则的行驶轨迹。
• 处理换道等复杂操作：在自动驾驶过程中，车辆可能需要进行换道等复杂操作。这些操作需要精确计算自车轨迹与相邻车道线的交点，并进行轨迹拼接处理。通过轨迹拼接，可以确保车辆在换道过程中能够平滑、安全地过渡到目标车道。

3 总结

        在自动驾驶过程中，为了解决轨迹不连续导致的“画龙”问题，引入了轨迹拼接策略。轨迹拼接在自动驾驶Planning模块中起到了至关重要的作用，它有助于确保车辆行驶的连续性、平滑性和安全性，并适应各种复杂的驾驶场景。