Apollo源码解析：决策规划PathReuseDecider

Apollo源码解析：决策规划PathReuseDecider

附赠自动驾驶最全的学习资料和量产经验：链接

一、概述

PathReuseDecider 是lanefollow 场景下，所调用的第 2 个 task，它的作用主要是换道时：

• 根据横纵向跟踪偏差，来决策是否需要重新规划轨迹；

• 如果横纵向跟踪偏差，则根据上一时刻的轨迹生成当前周期的轨迹，以尽量保持轨迹的一致性

二、PathReuseDecider的流程图与概述：

1、使用该功能前首先应该打开配置：

2、在之后进行的每个task，首先要进行检查配置是否打开，并读取PathReuseDecider的结果确定是否要进行之后的task完成新的决策规划任务：

接下来的lane_borrow_decider为例，在task进行的开始阶段，进行以下判断：

三、PathReuseDecider的具体逻辑如下：

1、PublicRoadPlanner 的 LaneFollowStage 配置了以下几个task 来实现具体的规划逻辑，PathReuseDecider 是第二个task：

scenario_type: LANE_FOLLOW
stage_type: LANE_FOLLOW_DEFAULT_STAGE
stage_config: {
  stage_type: LANE_FOLLOW_DEFAULT_STAGE
  enabled: true
  task_type: LANE_CHANGE_DECIDER
  task_type: PATH_REUSE_DECIDER
  task_type: PATH_LANE_BORROW_DECIDER
  task_type: PATH_BOUNDS_DECIDER
  task_type: PIECEWISE_JERK_PATH_OPTIMIZER
  task_type: PATH_ASSESSMENT_DECIDER
  task_type: PATH_DECIDER
  task_type: RULE_BASED_STOP_DECIDER
  task_type: ST_BOUNDS_DECIDER
  task_type: SPEED_BOUNDS_PRIORI_DECIDER
  task_type: SPEED_HEURISTIC_OPTIMIZER
  task_type: SPEED_DECIDER
  task_type: SPEED_BOUNDS_FINAL_DECIDER
  # task_type: PIECEWISE_JERK_SPEED_OPTIMIZER
  task_type: PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER
  task_type: RSS_DECIDER
}

2、 使用path_reuse的好处：

黑色轨迹为采用path_reuse，可以看到整个换道过程，轨迹完全一致; 红色、黄色轨迹为重新规划轨迹并采用trajectory_stitcher 拼接。

可以看到在轨迹拼接处以及整体轨迹可能出现不连贯不平滑的情况，可能会影响控制的平顺性：

3、PathReuseDecider 只对外暴露了Process() 一个接口，它的主要逻辑如下：

（1）当前处于非LaneFollow_Scenario场景，置位false

（2）当前未处于IN_CHANGE_LANE状态，置位false;

（3）如果存在可变车道，且已经完成换道轨迹生成，则置位false;

（4）前一时刻采用path_reuse, 若轨迹重规划、轨迹与静态障碍物发生碰撞、轨迹长度过短以及纵向求解失败，则置位false;

（5）只有前方静止障碍物走开(或大于阈值)、纵向求解成功、未与静态障碍物发生碰撞且轨迹长度大于阈值，才可置位true;

Status PathReuseDecider::Process(Frame* const frame,
                                 ReferenceLineInfo* const reference_line_info) {
  // Sanity checks.
  CHECK_NOTNULL(frame);
  CHECK_NOTNULL(reference_line_info);
 
  if (!Decider::config_.path_reuse_decider_config().reuse_path()) {
    ADEBUG << "skipping reusing path: conf";
    reference_line_info->set_path_reusable(false);
    return Status::OK();
  }
 
  // skip path reuse if not in LA