Apollo planning lane_change_decider解析

PathReuseDecider 是lanefollow 场景下，所调用的第 1 个 task，它的作用主要是对车辆换道状态进行管理，即对车辆换道状态（IN_CHANGE_LANE 换道中、CHANGE_LANE_FINISHED 换道结束、CHANGE_LANE_FAILED 换道失败）之间的转换进行管理

入口参数：frame、current_reference_line_info

该decider的主要操作是对记录车此时的换道状态，并不决定车是否换道。车的换道状态信息存于 injector_->planning_context()->mutable_planning_status()->mutable_change_lane()中，此dicider中出现最多的就是以下代码

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

可以看到，UpdateStatus()的操作就是通过lane_change_status指针来对车辆换道状态进行实时更新。更新逻辑如下：

//通过frame拿到车辆此时所在的区域参考线个数
std::list* reference_line_info =frame->mutable_reference_line_info();
//没有参考线就return，因为apollo的后续所有tasks全部都依赖参考线来做轨迹规划
if (reference_line_info->empty())
  {
    const std::string msg = "Reference lines empty.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }
...（省略的地方最后讲）
//判断参考线数量
bool has_change_lane = reference_line_info->size() > 1;
//如果只有一条参考线（比如往某个方向只有一条车道），那就通过updatestatus将车辆状态设置为CHANGE_LANE_FINISHED，这也符合我们认知，单向只有一条车道，还换什么道，所以车辆就该一直处于换到结束的状态
if (!has_change_lane) {
    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处理不止一条参考线的情况，正常道路都不止一条参考线，就连我们家小县城的马路都是单向3车道。主要逻辑为状态切换，实际操作还是通过updatestatus来实时更新车辆的换道状态。
else {  
    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.";
        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
      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);
    }
  }

 用图表示切换逻辑（在这里引用知乎的一张图）

 

PrioritizeChangeLane(const bool is_prioritize_change_lane,std::list* reference_line_info) 解析：

//当is_prioritize_change_lane为true,则遍历存储referenceLineInfo的链表，把
//当前车辆不位于的的那条（俗称换到参考线）放到链表的第一个位置
//当is_prioritize_change_lane为false,则遍历存储referenceLineInfo的链表，把
//当前车辆所位于的的那条放到链表的第一个位置
//我很纳闷，这里的形参reference_line_info是从frame类里面拿来的，你在这里任意调换
//referenceLineInfo的位置，就不怕public_road_planner那边对参考线进行遍历的功能吗？
//还有，将referenceLineInfo在链表中的位置来回换的目的我还不清楚，个人粗浅的认为
//此PrioritizeChangeLane（）功能意义不大
void LaneChangeDecider::PrioritizeChangeLane(
    const bool is_prioritize_change_lane,
    std::list* 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;
  }
  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;
  }
  reference_line_info->splice(reference_line_info->begin(),
                              *reference_line_info, iter);
  ADEBUG << "reference_line_info->IsChangeLanePath(): "
         << reference_line_info->begin()->IsChangeLanePath();
}

上文...省略的地方解析：

//当配置文件强制要求换道，即lane_change_decider_config.reckless_change_lane()为true，
//则调用PrioritizeChangeLane(true, reference_line_info)，将换道参考线放到链表第一位，
//直接return true。好奇连车的换道状态都不设置了吗？直接return了
if (lane_change_decider_config.reckless_change_lane()) {
    //若第一个参数为true,则将reference_line_info中的第一个变道车道参考线调到第一位
    //若第一个参数为false,则将reference_line_info中的第一个非变道车道参考线调到第一位
    PrioritizeChangeLane(true, reference_line_info);
    return Status::OK();
   }
//此处判断传进来的referenceLineinfo是否是变道参考线，如果是则通过
//IsClearToChangeLane()检查该参考线是否满足变道条件，
//IsClearToChangeLane只考虑传入的参考线上的动态障碍物，不考虑虚的和静态的障碍物。疑点：为什么只/考虑动态障碍物？
if (current_reference_line_info->IsChangeLanePath()) {
    //获取reference_line_info中存储的所有动态obstacle，一一的遍历这些obstacle
    //如果该障碍物不再reference_line_info所在的车道中，则不考虑
    //对于在此车道范围内的obstacle，通过分析该障碍物的位置与运动方向，结合车辆的位置来判断是否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));//获取非变道lane的id
    prev_status->set_last_succeed_timestamp(now);
    return Status::OK();
  }

IsClearToChangeLane（）解析：

//调选出位于该referenceline上的动态障碍物，结合障碍物的运动方向和车的运动方向，
//检查每个障碍物与车的前后距离，看是否都满足安全阈值。只要有一个动态障碍物不满足条件
//该referenceline就不满足换道条件。prev_status->set_is_clear_to_change_lane(false)
bool LaneChangeDecider::IsClearToChangeLane(
    ReferenceLineInfo* reference_line_info) {
  //车辆轮廓起点
  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;
    }

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

    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);
      //只考虑在reference_line_info所在的车道的障碍物
      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;
}

 总结与疑问：
lane_change_decider，我觉得应该叫车辆换道状态检测器，因为该decider通篇都在通过UpdateStatus来对injector_->planning_context()->mutable_planning_status()

->mutable_change_lane()信息进行实时更新。当然，顺便对proc传进来的current_reference_line_info通过IsClearToChangeLane()做了是否具备换道条件检测，是的话就额外的将injector_->planning_context()->mutable_planning_status()

->mutable_change_lane()->set_is_clear_to_change_lane设置为true,表面车辆此时周围有可以换道的参考线。这一信息很重要，后面的path_bound_decider会依据此标志位来确定regular_path_bound。

该决策器还有一个重点就是多参考线的换道逻辑状态图，这里有疑点:

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 {
//为什么CHANGE_LANE_FINISHED结束后，过个一定时间，又要切换到IN_CHANGE_LANE,
//就不能消停点吗？
        PrioritizeChangeLane(true, reference_line_info);
        UpdateStatus(now, ChangeLaneStatus::IN_CHANGE_LANE, current_path_id);
        ADEBUG << "change lane again after success";
      }

 图片链接：Apollo规划模块详解（五）：算法实现-lane change decider - 知乎