站长联盟,个人网站不备案,网站建设工具哪家好,dw网站建设教程文章目录 前言LANE_CHANGE_DECIDER功能简介LANE_CHANGE_DECIDER相关配置LANE_CHANGE_DECIDER总体流程LANE_CHANGE_DECIDER相关子函数PrioritizeChangeLaneUpdateStatusIsClearToChangeLaneHysteresisFilter 参考 前言
在Apollo星火计划学习笔记——Apollo路径规划算法原理与实… 文章目录 前言LANE_CHANGE_DECIDER功能简介LANE_CHANGE_DECIDER相关配置LANE_CHANGE_DECIDER总体流程LANE_CHANGE_DECIDER相关子函数PrioritizeChangeLaneUpdateStatusIsClearToChangeLaneHysteresisFilter 参考 前言
在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::listReferenceLineInfo* 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::listReferenceLineInfo* 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 truereference_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_limitsdouble::max();double end_s -std::numeric_limitsdouble::max();double start_l std::numeric_limitsdouble::max();double end_l -std::numeric_limitsdouble::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解析
