Apollo Planning——换道：LANE_CHANGE_DECIDER

LaneChangeDecider 是lanefollow 场景下，所调用的第一个task，它的作用主要有两点：判断当前是否进行变道，以及变道的状态，并将结果存在变量lane_change_status中；变道过程中将目标车道的reference line放置到首位，变道结束后将当前新车道的reference line放置到首位

LaneChangeDecider的具体逻辑如下：

1、PublicRoadPlanner 的 LaneFollowStage 配置了以下几个task 来实现具体的规划逻辑，LaneChangeDecider是第一个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、在stage阶段会依次调用每个 task 的 Execute() 函数，LaneChangeDecider继承自 Decider 类，Decider继承自基类 task 类，并且override了Execute() 方法；

modules/planning/tasks/task.h

class Task {
 public:
  explicit Task(const TaskConfig& config);

  Task(const TaskConfig& config,
       const std::shared_ptr<DependencyInjector>& injector);

  virtual ~Task() = default;

  const std::string& Name() const;

  const TaskConfig& Config() const { return config_; }

  virtual common::Status Execute(Frame* frame,ReferenceLineInfo* reference_line_info);

  virtual common::Status Execute(Frame* frame);

 protected:
  Frame* frame_ = nullptr;
  ReferenceLineInfo* reference_line_info_ = nullptr;

  TaskConfig config_;
  std::string name_;

  std::shared_ptr<DependencyInjector> injector_;
};

modules/planning/tasks/deciders/decider.h

class Decider : public Task {
 public:
  explicit Decider(const TaskConfig& config);
  Decider(const TaskConfig& config,const std::shared_ptr<DependencyInjector>& injector);
  virtual ~Decider() = default;

  apollo::common::Status Execute(
      Frame* frame, ReferenceLineInfo* reference_line_info) override;

  apollo::common::Status Execute(Frame* frame) override;

 protected:
  virtual apollo::common::Status Process(
      Frame* frame, ReferenceLineInfo* reference_line_info) {
    return apollo::common::Status::OK();
  }

  virtual apollo::common::Status Process(Frame* frame) {
    return apollo::common::Status::OK();
  }
};

重写Execute() 的代码在 modules/planning/tasks/deciders/decider.cc

apollo::common::Status Decider::Execute(
    Frame* frame, ReferenceLineInfo* reference_line_info) {
  Task::Execute(frame, reference_line_info);
  // 调用 子类 modules/planning/tasks/deciders/lane_change_decider/lane_change_decider.cc  类LaneChangeDecider中的 Process 方法
  return Process(frame, reference_line_info);
}

由以上分析可知，LaneChangeDecider 的主要决策逻辑在Process() 方法中，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);

  /**
   * 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
  }
  }
   * **/
  const auto& lane_change_decider_config = config_.lane_change_decider_config();
  // 通过frame拿到车辆此时所在的区域参考线个数
  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);
  }

  //判断是否是强制换道功能，如果是，调用优先换道功能
  if (lane_change_decider_config.reckless_change_lane()) {
    // 将换道参考线放到参考线的首位
    PrioritizeChangeLane(true, reference_line_info);
    return Status::OK();
  }

  /**
   * modules/planning/proto/planning_status.proto
   * 
   * message ChangeLaneStatus {
   *  enum Status {
   *    IN_CHANGE_LANE = 1;        // during change lane state
   *    CHANGE_LANE_FAILED = 2;    // change lane failed
   *    CHANGE_LANE_FINISHED = 3;  // change lane finished
   *  }
   *  optional Status status = 1;
   *  // the id of the route segment that the vehicle is driving on
   *  optional string path_id = 2;
   *  // the time stamp when the state started.
   *  optional double timestamp = 3;
   *  // the starting position only after which lane-change can happen.
   *  optional bool exist_lane_change_start_position = 4 [default = false];
   *  optional apollo.common.Point3D lane_change_start_position = 5;
   *  // the last time stamp when the lane-change planning succeed.
   *  optional double last_succeed_timestamp = 6;
   *  // if the current path and speed planning on the lane-change
   *  // reference-line succeed.
   *  optional bool is_current_opt_succeed = 7 [default = false];
   *  // denotes if the surrounding area is clear for ego vehicle to
   *  // change lane at this moment.
   *  optional bool is_clear_to_change_lane = 8 [default = false];
   * }
   *
   * **/
  // 获取换道信息，记录当前时间戳
  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);
  // /判断传进来的referenceLineinfo是否是变道参考线，如果是则通过
  if (current_reference_line_info->IsChangeLanePath()) {
    // IsClearToChangeLane()检查该参考线是否满足变道条件
    // IsClearToChangeLane 只考虑传入的参考线上的动态障碍物，不考虑虚的和静态的障碍物
    prev_status->set_is_clear_to_change_lane(IsClearToChangeLane(current_reference_line_info));
  }
  // 头次进入task，车道换道状态应该为空，默认设置为换道结束状态
  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();
  }
  // 判断参考线数量
  bool has_change_lane = reference_line_info->size() > 1;
  ADEBUG << "has_change_lane: " << has_change_lane;
  // 如果只有一条参考线（比如往某个方向只有一条车道），那就通过updatestatus将车辆状态设置为CHANGE_LANE_FINISHED，
  // 这也符合我们认知，单向只有一条车道，还换什么道，所以车辆就该一直处于换到结束的状态
  if (!has_change_lane) {
    // 没有换道参考线（参考线数量小于1条）：如果上个周期状态是已经换道完成或者换道失败，则返回进入下个task或者下个周期
    const auto& path_id = reference_line_info->front().Lanes().Id();
    if (prev_status->status() == ChangeLaneStatus::CHANGE_LANE_FINISHED) {
    } 
    // 如果上个周期状态是正在换道，更新换道状态
    else if (prev_status->status() == ChangeLaneStatus::IN_CHANGE_LANE) {
      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);
    }
    return Status::OK();
  //  下面的else处理不止一条参考线的情况，正常道路都不止一条参考线，
  //  主要逻辑为状态切换，实际操作还是通过 updatestatus 来实时更新车辆的换道状态。
  } 
  else {  // has change lane in reference lines.
    // 得到当前参考线的id
    auto current_path_id = GetCurrentPathId(*reference_line_info);
    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);
    }
    // 上一次换道中
    if (prev_status->status() == ChangeLaneStatus::IN_CHANGE_LANE) {
      // 换道开始的参考线是否和当前参考线未同一条线
      if (prev_status->path_id() == current_path_id) {
        // 如果是，表示没有换道完成
        PrioritizeChangeLane(true, reference_line_info);
      } else {
        // RemoveChangeLane(reference_line_info);
        PrioritizeChangeLane(false, reference_line_info);
        ADEBUG << "removed change lane.";
        // 更新换道状态为CHANGE_LANE_FINISHED
        UpdateStatus(now, ChangeLaneStatus::CHANGE_LANE_FINISHED, current_path_id);
      }
      return Status::OK();
    } 
    // 上一次换道失败
    else if (prev_status->status() == ChangeLaneStatus::CHANGE_LANE_FAILED) {
      
      if (now - prev_status->timestamp() < lane_change_decider_config.change_lane_fail_freeze_time()) {
        // 当前时间减去上次换道的时间间隔小于1s 
        // RemoveChangeLane(reference_line_info);
        PrioritizeChangeLane(false, reference_line_info);
        ADEBUG << "freezed after failed";
      } else {
        // 当前时间减去上次换道的时间间隔大于1s 
        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) {
      // 当前时间减去上次换道的时间间隔小于1.5s 
      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 {
        // 当前时间减去上次换道的时间间隔大于等于1.5s 
        PrioritizeChangeLane(true, reference_line_info);
        // 更改换道状态为 IN_CHANGE_LANE
        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();
}

3、其中lane_change_decider_config 配置文件很关键，决定了整个函数的流程走向，它定义在以下两个文件中：
modules/planning/conf/planning_config.pb.txt

  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

    lane_change_decider_config {
      enable_lane_change_urgency_check: true
    }

4、判断是否为可变车道时调用了 IsChangeLanePath()，它的逻辑也很简单， 如果自车在当前ReferenceLine 的车道segment上，则为FALSE；如果自车不在当前ReferenceLine 的车道segment上，则为TRUE。

bool ReferenceLineInfo::IsChangeLanePath() const {
  // 如果自车在当前ReferenceLine 的车道segment上，则为FALSE
  // 如果自车不在当前ReferenceLine 的车道segment上，则为TRUE。
  return !Lanes().IsOnSegment();
}

5、更新变道状态时用到了 UpdateStatus() 函数，它的定义如下：

void LaneChangeDecider::UpdateStatus(ChangeLaneStatus::Status status_code,
                                     const std::string& path_id) {
  UpdateStatus(Clock::NowInSeconds(), status_code, path_id);
}

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);
}

6、在调整参考线的顺序时，使用了PrioritizeChangeLane() 函数，它的调整参考线顺序的功能，需要配置enable_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();

  // 如果没有配置变道优先，则退出该函数
  if (!lane_change_decider_config.enable_prioritize_change_lane()) {
    return;
  }

  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 */

    // 0、is_prioritize_change_lane 根据参考线数量置位True 或 False
    // 1、如果 is_prioritize_change_lane 为True
    // 首先获取第一条参考线的迭代器，然后遍历所有的参考线，如果当前的参考线为允许变道参考线，则将第一条参考线更换为当前迭代器所指向的参考线.
    // 注意，可变车道为按迭代器的顺序求取，一旦发现可变车道，即推出循环。
    // 2、如果 is_prioritize_change_lane 为False，
    // 找到第一条不可变道的参考线，将第一条参考线更新为当前不可变道的参考线
    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;
  }
  
  reference_line_info->splice(reference_line_info->begin(),*reference_line_info, iter);
  ADEBUG << "reference_line_info->IsChangeLanePath(): " << reference_line_info->begin()->IsChangeLanePath();
}

7、 IsClearToChangeLane() 判断当前的参考线是否变道安全，并将结果写入lane_change_status 这个变量中

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()) {
    // a) 只对动态障碍物进行处理，忽略虚拟障碍物和静态障碍物；     
    if (obstacle->IsVirtual() || obstacle->IsStatic()) {
      ADEBUG << "skip one virtual or static obstacle";
      continue;
    }

    double start_s = std::numeric_limits::max();
    double end_s = -std::numeric_limits::max();
    double start_l = std::numeric_limits::max();
    double end_l = -std::numeric_limits::max();

    // 遍历当前目标的预测轨迹点集，或得预测轨迹的边界点
    for (const auto& p : obstacle->PerceptionPolygon().points()) {
      // 对于动态障碍物，先进行投影，获取S和L值
      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());
    }

    // c) 忽略换道目标参考线上2.5米之外的障碍物;
    if (reference_line_info->IsChangeLanePath()) {
      static constexpr double kLateralShift = 2.5;
      if (end_l < -kLateralShift || start_l > kLateralShift) {
        continue;
      }
    }

    // Raw estimation on whether same direction with ADC or not based on
    // prediction trajectory

    // 根据航向角判断是否为相同方向
    bool same_direction = true;
    // d) 对于需要考虑的障碍物进行方向粗略计算，评估是否和自车同向；
    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);
    }

    // 设置安全距离
    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;
    // e) 根据方向，计算纵向上的安全距离，考虑了速度差，比较直观。分为前方和后方两个维度。
    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;
    }
    /**
     * f) 根据前面计算的阈值，判断障碍物是否安全，采用的是滞回区间的方法，
     * 如果障碍物小于安全距离，laneChangeBlocking 为true。
     * 如果障碍物大于安全距离，laneChangeBlocking 为false。
     * 通过滞回区间进行滤波。一旦发现有block的障碍物，函数就返回，
     * 就认为该Reference 非clear(安全)。
     *   static bool HysteresisFilter(const double obstacle_distance,
                               const double safe_distance,
                               const double distance_buffer,
                               const bool is_obstacle_blocking);
     * 
     * **/
    // 判断障碍物是否满足安全距离
    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;
}

bool LaneChangeDecider::HysteresisFilter(const double obstacle_distance,
                                         const double safe_distance,
                                         const double distance_buffer,
                                         const bool is_obstacle_blocking) {
  if (is_obstacle_blocking) {
    return obstacle_distance < safe_distance + distance_buffer;
  } else {
    return obstacle_distance < safe_distance - distance_buffer;
  }
}