LaneChangeDecider 是lanefollow 場景下，所調(diào)用的第一個(gè)task，它的作用主要有兩點(diǎn)：判斷當(dāng)前是否進(jìn)行變道，以及變道的狀態(tài)，并將結(jié)果存在變量lane_change_status中；變道過程中將目標(biāo)車道的reference line放置到首位，變道結(jié)束后將當(dāng)前新車道的reference line放置到首位

LaneChangeDecider的具體邏輯如下：

1、PublicRoadPlanner 的 LaneFollowStage 配置了以下幾個(gè)task 來實(shí)現(xiàn)具體的規(guī)劃邏輯，LaneChangeDecider是第一個(gè)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階段會(huì)依次調(diào)用每個(gè) task 的 Execute() 函數(shù)，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);// 調(diào)用 子類 modules/planning/tasks/deciders/lane_change_decider/lane_change_decider.cc  類LaneChangeDecider中的 Process 方法return Process(frame, reference_line_info);
}

由以上分析可知，LaneChangeDecider 的主要決策邏輯在Process() 方法中，Process() 的代碼及注釋如下，先上整體代碼，再詳細(xì)講解其中的每個(gè)模塊：

// 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拿到車輛此時(shí)所在的區(qū)域參考線個(gè)數(shù)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);}//判斷是否是強(qiáng)制換道功能，如果是，調(diào)用優(yōu)先換道功能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];* }** **/// 獲取換道信息，記錄當(dāng)前時(shí)間戳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);// /判斷傳進(jìn)來的referenceLineinfo是否是變道參考線，如果是則通過if (current_reference_line_info->IsChangeLanePath()) {// IsClearToChangeLane()檢查該參考線是否滿足變道條件// IsClearToChangeLane 只考慮傳入的參考線上的動(dòng)態(tài)障礙物，不考慮虛的和靜態(tài)的障礙物prev_status->set_is_clear_to_change_lane(IsClearToChangeLane(current_reference_line_info));}// 頭次進(jìn)入task，車道換道狀態(tài)應(yīng)該為空，默認(rèn)設(shè)置為換道結(jié)束狀態(tài)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();}// 判斷參考線數(shù)量bool has_change_lane = reference_line_info->size() > 1;ADEBUG << "has_change_lane: " << has_change_lane;// 如果只有一條參考線（比如往某個(gè)方向只有一條車道），那就通過updatestatus將車輛狀態(tài)設(shè)置為CHANGE_LANE_FINISHED，// 這也符合我們認(rèn)知，單向只有一條車道，還換什么道，所以車輛就該一直處于換到結(jié)束的狀態(tài)if (!has_change_lane) {// 沒有換道參考線（參考線數(shù)量小于1條）：如果上個(gè)周期狀態(tài)是已經(jīng)換道完成或者換道失敗，則返回進(jìn)入下個(gè)task或者下個(gè)周期const auto& path_id = reference_line_info->front().Lanes().Id();if (prev_status->status() == ChangeLaneStatus::CHANGE_LANE_FINISHED) {} // 如果上個(gè)周期狀態(tài)是正在換道，更新?lián)Q道狀態(tài)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處理不止一條參考線的情況，正常道路都不止一條參考線，//  主要邏輯為狀態(tài)切換，實(shí)際操作還是通過 updatestatus 來實(shí)時(shí)更新車輛的換道狀態(tài)。} else {  // has change lane in reference lines.// 得到當(dāng)前參考線的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) {// 換道開始的參考線是否和當(dāng)前參考線未同一條線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.";// 更新?lián)Q道狀態(tài)為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()) {// 當(dāng)前時(shí)間減去上次換道的時(shí)間間隔小于1s // RemoveChangeLane(reference_line_info);PrioritizeChangeLane(false, reference_line_info);ADEBUG << "freezed after failed";} else {// 當(dāng)前時(shí)間減去上次換道的時(shí)間間隔大于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) {// 當(dāng)前時(shí)間減去上次換道的時(shí)間間隔小于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 {// 當(dāng)前時(shí)間減去上次換道的時(shí)間間隔大于等于1.5s PrioritizeChangeLane(true, reference_line_info);// 更改換道狀態(tài)為 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、其中l(wèi)ane_change_decider_config 配置文件很關(guān)鍵，決定了整個(gè)函數(shù)的流程走向，它定義在以下兩個(gè)文件中：
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、判斷是否為可變車道時(shí)調(diào)用了 IsChangeLanePath()，它的邏輯也很簡單， 如果自車在當(dāng)前ReferenceLine 的車道segment上，則為FALSE；如果自車不在當(dāng)前ReferenceLine 的車道segment上，則為TRUE。

bool ReferenceLineInfo::IsChangeLanePath() const {// 如果自車在當(dāng)前ReferenceLine 的車道segment上，則為FALSE// 如果自車不在當(dāng)前ReferenceLine 的車道segment上，則為TRUE。return !Lanes().IsOnSegment();
}

5、更新變道狀態(tài)時(shí)用到了 UpdateStatus() 函數(shù)，它的定義如下：

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、在調(diào)整參考線的順序時(shí)，使用了PrioritizeChangeLane() 函數(shù)，它的調(diào)整參考線順序的功能，需要配置enable_prioritize_change_lane為True，這個(gè)函數(shù)的完整代碼及注釋如下：

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();// 如果沒有配置變道優(yōu)先，則退出該函數(shù)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 根據(jù)參考線數(shù)量置位True 或 False// 1、如果 is_prioritize_change_lane 為True// 首先獲取第一條參考線的迭代器，然后遍歷所有的參考線，如果當(dāng)前的參考線為允許變道參考線，則將第一條參考線更換為當(dāng)前迭代器所指向的參考線.// 注意，可變車道為按迭代器的順序求取，一旦發(fā)現(xiàn)可變車道，即推出循環(huán)。// 2、如果 is_prioritize_change_lane 為False，// 找到第一條不可變道的參考線，將第一條參考線更新為當(dāng)前不可變道的參考線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() 判斷當(dāng)前的參考線是否變道安全，并將結(jié)果寫入lane_change_status 這個(gè)變量中

IsClearToChangeLane() 遍歷了當(dāng)前參考線上所有目標(biāo)，并根據(jù)目標(biāo)的行駛方向設(shè)置安全距離，通過安全距離判斷是否變道安全，代碼及注釋如下：

bool LaneChangeDecider::IsClearToChangeLane(ReferenceLineInfo* reference_line_info) {// 或得當(dāng)前參考線的s坐標(biāo)的最大最小值，以及自車速度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());// 遍歷每個(gè)目標(biāo)for (const auto* obstacle : reference_line_info->path_decision()->obstacles().Items()) {// a) 只對動(dòng)態(tài)障礙物進(jìn)行處理，忽略虛擬障礙物和靜態(tài)障礙物；     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();// 遍歷當(dāng)前目標(biāo)的預(yù)測軌跡點(diǎn)集，或得預(yù)測軌跡的邊界點(diǎn)for (const auto& p : obstacle->PerceptionPolygon().points()) {// 對于動(dòng)態(tài)障礙物，先進(jìn)行投影，獲取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) 忽略換道目標(biāo)參考線上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// 根據(jù)航向角判斷是否為相同方向bool same_direction = true;// d) 對于需要考慮的障礙物進(jìn)行方向粗略計(jì)算，評估是否和自車同向；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);}// 設(shè)置安全距離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) 根據(jù)方向，計(jì)算縱向上的安全距離，考慮了速度差，比較直觀。分為前方和后方兩個(gè)維度。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) 根據(jù)前面計(jì)算的閾值，判斷障礙物是否安全，采用的是滯回區(qū)間的方法，* 如果障礙物小于安全距離，laneChangeBlocking 為true。* 如果障礙物大于安全距離，laneChangeBlocking 為false。* 通過滯回區(qū)間進(jìn)行濾波。一旦發(fā)現(xiàn)有block的障礙物，函數(shù)就返回，* 就認(rèn)為該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;}
}