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

前言

在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_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: SPEED_BOUNDS_PRIORI_DECIDERtask_type: SPEED_HEURISTIC_OPTIMIZERtask_type: SPEED_DECIDERtask_type: SPEED_BOUNDS_FINAL_DECIDERtask_type: PIECEWISE_JERK_SPEED_OPTIMIZER# task_type: PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZERtask_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_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}
}
// modules/planning/conf/scenario/lane_follow_config.pb.txttask_config: {task_type: LANE_CHANGE_DECIDERlane_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.protoif (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的IDconst 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) {// 更新当前时刻，变道完成状态，以及当前道路的IDUpdateStatus(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 的状态为OKreturn Status::OK();} else {  // has change lane in reference lines.// 获取自车当前所在车道的IDauto 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 failedif (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 nowif (!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_lineis_prioritize_change_lane == false: prioritizenon_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;}// 初始化SLdouble 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 confsstatic 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解析