Apollo Planning学习（3）-------LANE_CHANGE

LANE_CHANGE_DECIDER

在这个decider可能会有很多人陷入误区，认为Apollo在规划中换道的时候是有一个主动请求的，这里引用知乎上iGear大佬的解释：Apollo的都是自己计算换道时机和换道安全，一般没有主动换道请求，一般人可能会认为换道要有明确的时机，类似于有一个明确的状态，这个状态位true了就要换道。但Apollo里面没有明确的换道时机，就像人开车一样，觉得安全且有必要就去执行某一个动作。这里大家可以反复看看Apollo的规划流程。这节的lane_change_decider只是计算换道状态，后面我们还会计算换道的boundary，然后再规划换道轨迹，不是一个逻辑或者一个task就能实现换道的。就算计算出来了换道boundary也不一定就能生成换道轨迹，生成不了换道轨迹还是实现不了换道。不是先有了换道的必要性才去计算这些，而是根据这些计算来判断最终是否安全且有必要去换道。（个人被这句话点醒，膜拜大佬）
也就是说可以将该decider的大致作用是对记录ADC此时的换道状态，并不决定ADC是否进行lanechange。车的换道状态信息存于 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);
}

lane_change_decider在\modules\planning\tasks\deciders\lane_change_decider目录下，
且process()是处理lane_change_decider的主要处理逻辑函数

// added a dummy parameter to enable this task in ExecuteTaskOnReferenceLine
// 添加了一个伪参数以在ExecuteTaskOnReferenceLine中启用此任务
// 这个ExecuteTaskOnReferenceLine在\modules\planning\scenarios\stage.cc目录下有具体内容
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();// 通过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();}auto* prev_status = injector_->planning_context()->mutable_planning_status()->mutable_change_lane();double now = Clock::NowInSeconds();// 默认设置falseprev_status->set_is_clear_to_change_lane(false);
//此处判断传进来的referenceLineinfo是否是变道参考线，如果是则通过
// IsChangeLanePath():判断是否是可变车道，如果车不在车道片段上，则该车道为可变道车道。if (current_reference_line_info->IsChangeLanePath()) {//IsClearToChangeLane()检查该参考线是否满足变道条件，
//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.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 statusif (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();
}

这里关于参考线不止一条时的主要换道逻辑参考知乎iGear大佬的一张逻辑图如下：

解析函数LaneChangeDecider::PrioritizeChangeLane

//当is_prioritize_change_lane为true,则遍历存储referenceLineInfo的链表，把
//当前车辆不位于的的那条（俗称换到参考线）放到链表的第一个位置
//当is_prioritize_change_lane为false,则遍历存储referenceLineInfo的链表，把
//当前车辆所位于的的那条放到链表的第一个位置
//首先获取第一条参考线的迭代器，然后遍历所有的参考线
//注意，可变车道为按迭代器的顺序求取，一旦发现可变车道，即推出循环。
//很多博主认为此函数意义不大，本人没什么见解只是学习ing
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;}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;}reference_line_info->splice(reference_line_info->begin(),*reference_line_info, iter);ADEBUG << "reference_line_info->IsChangeLanePath(): "<< reference_line_info->begin()->IsChangeLanePath();
}

解析函数LaneChangeDecider::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<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()) {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// 基于预测轨迹的与ADC方向是否相同的原始估计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;}/*
根据前面计算的阈值，判断障碍物是否安全，采用的是滞回区间的方法，如果障碍物小于安全距离，laneChangeBlocking为true。
如果障碍物大于安全距离，laneChangeBlocking为false。通过滞回区间进行滤波。
一旦发现有block的障碍物，函数就返回，就认为该Reference 非clear(安全)。
*/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;
}

关于LaneChangeDecider::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) {return obstacle_distance < safe_distance + distance_buffer;} else {return obstacle_distance < safe_distance - distance_buffer;}
}

Apollo Planning学习（3）-------LANE_CHANGE_DECIDER相关推荐

Apollo Planning学习（9）-------速度规划
顺着之前学习public road planner 的路径规划中lane follow的task,在得到的规划路径上再进行速度规划.大致思路为先利用ST Graph,将障碍物.限速等投影在ST图上,利 ...
Apollo Planning学习（1）-------模块流程
通过上面图可以看出PlanningComponent继承于cyber::Component,是事件触发机制,即当同时收到预测.底盘和定位信息时,planning模块才会运行.Prediction和Lo ...
百度apollo planning代码学习-Apollo\modules\planning\math\piecewise_jerk\PiecewiseJerkPathProblem类代码详解
概述 PiecewiseJerkPathProblem类是apollo planning模块下modules\planning\math\piecewise_jerk\piecewise_jerk_p ...
Apollo星火计划学习笔记——第一讲使用Apollo学习自动驾驶
引言如何学习自动驾驶? 自动驾驶是集车辆.计算机.电子电气.人工智能.通信等多学科应用为一体的的复杂系统.针对自身专业背景结合自动驾驶发展进行学习: 自动驾驶是通过智能系统来驾驶汽车从而取代了驾驶员 ...
Apollo代码学习(二)—车辆运动学模型
Apollo代码学习-车辆运动学模型前言车辆模型单车模型(Bicycle Model) 车辆运动学模型阿克曼转向几何(Ackerman turning geometry) 小结 Apollo( ...
Apollo Planning决策规划算法代码详细解析 (1)：Scenario选择
本文重点讲解Apollo代码中怎样配置Scenario以及选择当前Scenario,Scenario场景决策是Apollo规划算法的第一步,本文会对代码进行详细解析,也会梳理整个决策流程,码字不易,喜 ...
Apollo星火计划学习笔记——Apollo开放空间规划算法原理与实践
文章目录前言 1. 开放空间规划算法总体介绍 1.1 Task: OPEN_SPACE_ROI_DECIDER 1.2 Task: OPEN_SPACE_TRAJECTORY_PROVIDER 1. ...
Apollo星火计划学习笔记——Apollo路径规划算法原理与实践
文章目录前言 1. 路径规划算法总体介绍 1.1 Task: LANE_CHANGE_DECIDER 1.2 Task: PATH_REUSE_DECIDER 1.3 Task: PATH_BORR ...
Apollo代码学习(六)—模型预测控制(MPC)_follow轻尘的博客-CSDN博客_mpc代码
Apollo代码学习(六)-模型预测控制(MPC)_follow轻尘的博客-CSDN博客_mpc代码

Apollo Planning学习（3）-------LANE_CHANGE_DECIDER

LANE_CHANGE_DECIDER

Apollo Planning学习（3）-------LANE_CHANGE_DECIDER相关推荐

最新文章

热门文章