【Apollo学习笔记】——规划模块TASK之LANE_CHANGE_DECIDER

TASK系列解析文章

1.【Apollo学习笔记】——规划模块TASK之LANE_CHANGE_DECIDER
2.【Apollo学习笔记】——规划模块TASK之PATH_REUSE_DECIDER
3.【Apollo学习笔记】——规划模块TASK之PATH_BORROW_DECIDER
4.【Apollo学习笔记】——规划模块TASK之PATH_BOUNDS_DECIDER
5.【Apollo学习笔记】——规划模块TASK之PIECEWISE_JERK_PATH_OPTIMIZER
6.【Apollo学习笔记】——规划模块TASK之PATH_ASSESSMENT_DECIDER
7.【Apollo学习笔记】——规划模块TASK之PATH_DECIDER
8.【Apollo学习笔记】——规划模块TASK之RULE_BASED_STOP_DECIDER
9.【Apollo学习笔记】——规划模块TASK之SPEED_BOUNDS_PRIORI_DECIDER&&SPEED_BOUNDS_FINAL_DECIDER
10.【Apollo学习笔记】——规划模块TASK之SPEED_HEURISTIC_OPTIMIZER
11.【Apollo学习笔记】——规划模块TASK之SPEED_DECIDER
12.【Apollo学习笔记】——规划模块TASK之PIECEWISE_JERK_SPEED_OPTIMIZER
13.【Apollo学习笔记】——规划模块TASK之PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER(一)
14.【Apollo学习笔记】——规划模块TASK之PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER(二)

前言

在Apollo星火计划学习笔记——Apollo路径规划算法原理与实践与【Apollo学习笔记】——Planning模块讲到……Stage::Process的PlanOnReferenceLine函数会依次调用task_list中的TASK，本文将会继续以LaneFollow为例依次介绍其中的TASK部分究竟做了哪些工作。由于个人能力所限，文章可能有纰漏的地方，还请批评斧正。

在modules/planning/conf/scenario/lane_follow_config.pb.txt配置文件中，我们可以看到LaneFollow所需要执行的所有task。

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: 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

本文将从第一个task——LANE_CHANGE_DECIDER开始介绍。

LANE_CHANGE_DECIDER功能简介

LANE_CHANGE_DECIDER主要功能是：产生是否换道的决策，更新换道状态。
其主要逻辑是：首先判断是否产生多条参考线，若只有一条参考线，则保持直行。若有多条参考线，则根据一些条件（主车的前方和后方一定距离内是否有障碍物，旁边车道在一定距离内是否有障碍物）进行判断是否换道，当所有条件都满足时，则进行换道决策。

LANE_CHANGE_DECIDER相关配置

LANE_CHANGE_DECIDER的相关配置集中在以下两个文件：modules/planning/conf/planning_config.pb.txt和modules/planning/conf/scenario/lane_follow_config.pb.txt

// modules/planning/conf/planning_config.pb.txt
default_task_config: {
  task_type: LANE_CHANGE_DECIDER
  lane_change_decider_config {
    enable_lane_change_urgency_check: false
    enable_prioritize_change_lane: false
    enable_remove_change_lane: false
    reckless_change_lane: false
    change_lane_success_freeze_time: 1.5
    change_lane_fail_freeze_time: 1.0
  }
}
// modules/planning/conf/scenario/lane_follow_config.pb.txt
  task_config: {
    task_type: LANE_CHANGE_DECIDER
    lane_change_decider_config {
      enable_lane_change_urgency_check: true
    }
  }

LANE_CHANGE_DECIDER总体流程

总体流程图如下所示：

接着来看一看LANE_CHANGE_DECIDER的整体代码，文件路径：modules/planning/tasks/deciders/lane_change_decider/lane_change_decider.cc
LANE_CHANGE_DECIDER实现逻辑在Process函数中:

// added a dummy parameter to enable this task in ExecuteTaskOnReferenceLine
Status LaneChangeDecider::Process(
    Frame* frame, ReferenceLineInfo* const current_reference_line_info) {
  // Sanity checks.
  CHECK_NOTNULL(frame);
  // 读取配置文件
  const auto& lane_change_decider_config = config_.lane_change_decider_config();
  // 读取ReferenceLineInfo,并检查是否为空
  std::list<ReferenceLineInfo>* reference_line_info =
      frame->mutable_reference_line_info();
  if (reference_line_info->empty()) {
    const std::string msg = "Reference lines empty.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }
  // 始终允许车辆变道。车辆可能持续变道 config_path:modules/planning/proto/task_config.proto
  if (lane_change_decider_config.reckless_change_lane()) {
    PrioritizeChangeLane(true, reference_line_info);
    return Status::OK();
  }
  // 获取上一时刻变道状态信息并记录时间戳
  auto* prev_status = injector_->planning_context()
                          ->mutable_planning_status()
                          ->mutable_change_lane();
  double now = Clock::NowInSeconds();
  // 判断当前参考线是否安全可用
  prev_status->set_is_clear_to_change_lane(false);
  if (current_reference_line_info->IsChangeLanePath()) {
    prev_status->set_is_clear_to_change_lane(
        IsClearToChangeLane(current_reference_line_info));
  }
  // 是否获取到状态信息
  if (!prev_status->has_status()) {
    UpdateStatus(now, ChangeLaneStatus::CHANGE_LANE_FINISHED,
                 GetCurrentPathId(*reference_line_info));
    prev_status->set_last_succeed_timestamp(now);
    return Status::OK();
  }
  // 参考线的数目是否大于1
  // 根据reference line的数量判断是否处于变道场景中，size() > 1则处于变道过程中，需要判断变道的状态
  bool has_change_lane = reference_line_info->size() > 1;
  ADEBUG << "has_change_lane: " << has_change_lane;
  // 只有一条reference line，没有进行变道
  if (!has_change_lane) {
    // 根据当前唯一的reference line，获得当前道路lane的ID
    const auto& path_id = reference_line_info->front().Lanes().Id();
    // 上一时刻是否变道完成
    if (prev_status->status() == ChangeLaneStatus::CHANGE_LANE_FINISHED) {
    // 上一时刻是否在变道中。若有，这一时刻只有一条reference line，说明变道成功
    } else if (prev_status->status() == ChangeLaneStatus::IN_CHANGE_LANE) {
    // 更新当前时刻，变道完成状态，以及当前道路的ID
      UpdateStatus(now, ChangeLaneStatus::CHANGE_LANE_FINISHED, path_id);
    // 上一时刻是否变道失败
    } else if (prev_status->status() == ChangeLaneStatus::CHANGE_LANE_FAILED) {
    } else {
      const std::string msg =
          absl::StrCat("Unknown state: ", prev_status->ShortDebugString());
      AERROR << msg;
      return Status(ErrorCode::PLANNING_ERROR, msg);
    }
    // 返回LaneChangeDecider::Process 的状态为OK
    return Status::OK();
  } else {  // has change lane in reference lines.
    // 获取自车当前所在车道的ID
    auto current_path_id = GetCurrentPathId(*reference_line_info);
    // 如果当前所在车道为空，则返回error状态
    if (current_path_id.empty()) {
      const std::string msg = "The vehicle is not on any reference line";
      AERROR << msg;
      return Status(ErrorCode::PLANNING_ERROR, msg);
    }
    // 如果上一时刻处在变道中，根据上一时刻自车所处道路ID与当前时刻所处道路ID对比，来确认变道状态
    if (prev_status->status() == ChangeLaneStatus::IN_CHANGE_LANE) {
      // ID相同则说明变道还在进行中，
      if (prev_status->path_id() == current_path_id) {
        // 同时调用PrioritizeChangeLane(),将目标车道的reference line放在首位
        PrioritizeChangeLane(true, reference_line_info);
      } else {
        // RemoveChangeLane(reference_line_info);
        // ID不同则说明变道已经完成，
        PrioritizeChangeLane(false, reference_line_info);
        ADEBUG << "removed change lane.";
        // 更新状态
        UpdateStatus(now, ChangeLaneStatus::CHANGE_LANE_FINISHED,
                     current_path_id);
      }
      return Status::OK();
    // 上一时刻变道失败
    } else if (prev_status->status() == ChangeLaneStatus::CHANGE_LANE_FAILED) {
      // TODO(SHU): add an optimization_failure counter to enter
      // change_lane_failed status
      // 判断当前时刻减上一时刻的时间差是否小于换道失败冻结时间
      // not allowed to change lane this amount of time if just failed
      if (now - prev_status->timestamp() <
          lane_change_decider_config.change_lane_fail_freeze_time()) {
        // RemoveChangeLane(reference_line_info);
        PrioritizeChangeLane(false, reference_line_info);
        ADEBUG << "freezed after failed";
      } else {
        UpdateStatus(now, ChangeLaneStatus::IN_CHANGE_LANE, current_path_id);
        ADEBUG << "change lane again after failed";
      }
      return Status::OK();
    // 若上一时刻换道完成
    } else if (prev_status->status() ==
               ChangeLaneStatus::CHANGE_LANE_FINISHED) {
      // 判断当前时刻减上一时刻的时间差是否小于换道完成冻结时间
      if (now - prev_status->timestamp() <
          lane_change_decider_config.change_lane_success_freeze_time()) {
        // RemoveChangeLane(reference_line_info);
        PrioritizeChangeLane(false, reference_line_info);
        ADEBUG << "freezed after completed lane change";
      } else {
        PrioritizeChangeLane(true, reference_line_info);
        UpdateStatus(now, ChangeLaneStatus::IN_CHANGE_LANE, current_path_id);
        ADEBUG << "change lane again after success";
      }
    } else {
      const std::string msg =
          absl::StrCat("Unknown state: ", prev_status->ShortDebugString());
      AERROR << msg;
      return Status(ErrorCode::PLANNING_ERROR, msg);
    }
  }
  return Status::OK();
}

LANE_CHANGE_DECIDER相关子函数

PrioritizeChangeLane

// 提升变道的优先级,找到变道的参考线，并将其置于首位（is_prioritize_change_lane == true）
void LaneChangeDecider::PrioritizeChangeLane(
    const bool is_prioritize_change_lane,
    std::list<ReferenceLineInfo>* reference_line_info) const {
  if (reference_line_info->empty()) {
    AERROR << "Reference line info empty";
    return;
  }

  const auto& lane_change_decider_config = config_.lane_change_decider_config();

  // TODO(SHU): disable the reference line order change for now
  if (!lane_change_decider_config.enable_prioritize_change_lane()) {
    return;
  }
  // 遍历reference_line_info列表中的元素，并检查当前元素是否为变道路径（IsChangeLanePath）
  // 找到第一个需要优先排序的元素后，循环会被中断
  // 0、is_prioritize_change_lane 根据参考线数量置位True 或 False
  // 1、如果is_prioritize_change_lane为True
  // 首先获取第一条参考线的迭代器，然后遍历所有的参考线，
  // 如果当前的参考线为允许变道参考线，则将第一条参考线更换为当前迭代器所指向的参考线,
  // 注意，可变车道为按迭代器的顺序求取，一旦发现可变车道，即推出循环。
  // 
  // 2、如果is_prioritize_change_lane 为False，
  // 找到第一条不可变道的参考线，将第一条参考线更新为当前不可变道的参考线
  auto iter = reference_line_info->begin();
  while (iter != reference_line_info->end()) {
    ADEBUG << "iter->IsChangeLanePath(): " << iter->IsChangeLanePath();
    /* is_prioritize_change_lane == true: prioritize change_lane_reference_line
       is_prioritize_change_lane == false: prioritize
       non_change_lane_reference_line */
    if ((is_prioritize_change_lane && iter->IsChangeLanePath()) ||
        (!is_prioritize_change_lane && !iter->IsChangeLanePath())) {
      ADEBUG << "is_prioritize_change_lane: " << is_prioritize_change_lane;
      ADEBUG << "iter->IsChangeLanePath(): " << iter->IsChangeLanePath();
      break;
    }
    ++iter;
  }
  // 将变道的参考线置于列表首位（is_prioritize_change_lane == true）
  reference_line_info->splice(reference_line_info->begin(),
                              *reference_line_info, iter);
  ADEBUG << "reference_line_info->IsChangeLanePath(): "
         << reference_line_info->begin()->IsChangeLanePath();
}

UpdateStatus

void LaneChangeDecider::UpdateStatus(double timestamp,
                                     ChangeLaneStatus::Status status_code,
                                     const std::string& path_id) {
  auto* lane_change_status = injector_->planning_context()
                                 ->mutable_planning_status()
                                 ->mutable_change_lane();
  lane_change_status->set_timestamp(timestamp);
  lane_change_status->set_path_id(path_id);
  lane_change_status->set_status(status_code);
}

IsClearToChangeLane

// 用于检查当前参考线是否安全，或者当前参考线是否可以偏离后返回
bool LaneChangeDecider::IsClearToChangeLane(
    ReferenceLineInfo* reference_line_info) {
  // 获得当前参考线自车的s坐标的起点与终点，以及自车线速度
  double ego_start_s = reference_line_info->AdcSlBoundary().start_s();
  double ego_end_s = reference_line_info->AdcSlBoundary().end_s();
  double ego_v =
      std::abs(reference_line_info->vehicle_state().linear_velocity());
  // 遍历障碍物，跳过虚拟的和静止的
  for (const auto* obstacle :
       reference_line_info->path_decision()->obstacles().Items()) {
    if (obstacle->IsVirtual() || obstacle->IsStatic()) {
      ADEBUG << "skip one virtual or static obstacle";
      continue;
    }
    // 初始化SL
    double start_s = std::numeric_limits<double>::max();
    double end_s = -std::numeric_limits<double>::max();
    double start_l = std::numeric_limits<double>::max();
    double end_l = -std::numeric_limits<double>::max();
    // 获取动态障碍物的边界点并转化为SL坐标
    for (const auto& p : obstacle->PerceptionPolygon().points()) {
      SLPoint sl_point;
      reference_line_info->reference_line().XYToSL(p, &sl_point);
      start_s = std::fmin(start_s, sl_point.s());
      end_s = std::fmax(end_s, sl_point.s());

      start_l = std::fmin(start_l, sl_point.l());
      end_l = std::fmax(end_l, sl_point.l());
    }
    // 以障碍物在S方向上的起始点与终点之和的二分之一作为障碍物中心点si，获取si点的道路宽度
    // 若障碍物在车道线之外，则不考虑
    if (reference_line_info->IsChangeLanePath()) {
      double left_width(0), right_width(0);
      reference_line_info->mutable_reference_line()->GetLaneWidth(
          (start_s + end_s) * 0.5, &left_width, &right_width);
      if (end_l < -right_width || start_l > left_width) {
        continue;
      }
    }

    // Raw estimation on whether same direction with ADC or not based on
    // prediction trajectory
    // 根据预测轨迹粗略判断障碍物的方向是否和自车相同
    bool same_direction = true;
    if (obstacle->HasTrajectory()) {
      double obstacle_moving_direction =
          obstacle->Trajectory().trajectory_point(0).path_point().theta();
      const auto& vehicle_state = reference_line_info->vehicle_state();
      // 获取车辆航向角
      double vehicle_moving_direction = vehicle_state.heading();
      if (vehicle_state.gear() == canbus::Chassis::GEAR_REVERSE) {
        vehicle_moving_direction =
            common::math::NormalizeAngle(vehicle_moving_direction + M_PI);
      }
      double heading_difference = std::abs(common::math::NormalizeAngle(
          obstacle_moving_direction - vehicle_moving_direction));
      same_direction = heading_difference < (M_PI / 2.0);
    }

    // TODO(All) move to confs
    static constexpr double kSafeTimeOnSameDirection = 3.0;
    static constexpr double kSafeTimeOnOppositeDirection = 5.0;
    static constexpr double kForwardMinSafeDistanceOnSameDirection = 10.0;
    static constexpr double kBackwardMinSafeDistanceOnSameDirection = 10.0;
    static constexpr double kForwardMinSafeDistanceOnOppositeDirection = 50.0;
    static constexpr double kBackwardMinSafeDistanceOnOppositeDirection = 1.0;
    static constexpr double kDistanceBuffer = 0.5;

    double kForwardSafeDistance = 0.0;
    double kBackwardSafeDistance = 0.0;
    // 根据方向、自车与运动障碍物之间速度关系设置安全距离
    if (same_direction) {
      kForwardSafeDistance =
          std::fmax(kForwardMinSafeDistanceOnSameDirection,
                    (ego_v - obstacle->speed()) * kSafeTimeOnSameDirection);
      kBackwardSafeDistance =
          std::fmax(kBackwardMinSafeDistanceOnSameDirection,
                    (obstacle->speed() - ego_v) * kSafeTimeOnSameDirection);
    } else {
      kForwardSafeDistance =
          std::fmax(kForwardMinSafeDistanceOnOppositeDirection,
                    (ego_v + obstacle->speed()) * kSafeTimeOnOppositeDirection);
      kBackwardSafeDistance = kBackwardMinSafeDistanceOnOppositeDirection;
    }
    // 通过滞后滤波器判断障碍物是否满足安全距离
    if (HysteresisFilter(ego_start_s - end_s, kBackwardSafeDistance,
                         kDistanceBuffer, obstacle->IsLaneChangeBlocking()) &&
        HysteresisFilter(start_s - ego_end_s, kForwardSafeDistance,
                         kDistanceBuffer, obstacle->IsLaneChangeBlocking())) {
      reference_line_info->path_decision()
          ->Find(obstacle->Id())
          ->SetLaneChangeBlocking(true);
      ADEBUG << "Lane Change is blocked by obstacle" << obstacle->Id();
      return false;
    } else {
      reference_line_info->path_decision()
          ->Find(obstacle->Id())
          ->SetLaneChangeBlocking(false);
    }
  }
  return true;
}

HysteresisFilter

// 滞后滤波器
// 在安全距离附近的情况下，通过引入距离缓冲区来调整安全距离的大小，从而避免频繁进行车道变换。
bool LaneChangeDecider::HysteresisFilter(const double obstacle_distance,
                                         const double safe_distance,
                                         const double distance_buffer,
                                         const bool is_obstacle_blocking) {
  if (is_obstacle_blocking) {
    // obstacle_distance是否小于safe_distance + distance_buffer，如果是则返回true，否则返回false。
    return obstacle_distance < safe_distance + distance_buffer;
  } else {
    // obstacle_distance是否小于safe_distance - distance_buffer，如果是则返回true，否则返回false。
    return obstacle_distance < safe_distance - distance_buffer;
  }
}

参考

[1] Apollo规划模块详解（五）：算法实现-lane change decider
[2] Apollo Planning决策规划代码详细解析 (6):LaneChangeDecider
[3] 百度Apollo5.0规划模块代码学习（四）换道决策分析
[4] Apollo planning lane_change_decider解析