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