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_STAGEenabled: truetask_type: LANE_CHANGE_DECIDERtask_type: PATH_REUSE_DECIDERtask_type: PATH_LANE_BORROW_DECIDERtask_type: PATH_BOUNDS_DECIDERtask_type: PIECEWISE_JERK_PATH_OPTIMIZERtask_type: PATH_ASSESSMENT_DECIDERtask_type: PATH_DECIDERtask_type: RULE_BASED_STOP_DECIDERtask_type: ST_BOUNDS_DECIDERtask_type: SPEED_BOUNDS_PRIORI_DECIDERtask_type: SPEED_HEURISTIC_OPTIMIZERtask_type: SPEED_DECIDERtask_type: SPEED_BOUNDS_FINAL_DECIDER# task_type: PIECEWISE_JERK_SPEED_OPTIMIZERtask_type: PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZERtask_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_DECIDERlane_change_decider_config {enable_lane_change_urgency_check: falseenable_prioritize_change_lane: falseenable_remove_change_lane: falsereckless_change_lane: falsechange_lane_success_freeze_time: 1.5change_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.// 得到当前参考线的idauto 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_FINISHEDUpdateStatus(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_LANEUpdateStatus(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: falseenable_prioritize_change_lane: falseenable_remove_change_lane: falsereckless_change_lane: falsechange_lane_success_freeze_time: 1.5change_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_lineis_prioritize_change_lane == false: prioritizenon_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<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();// 遍历当前目标的预测轨迹点集，或得预测轨迹的边界点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;}
}