使用element做的网站,建设银行人力资源招聘网站,软件商店最新版下载,宁波制作网站企业LaneChangeDecider 是lanefollow 场景下#xff0c;所调用的第一个task#xff0c;它的作用主要有两点#xff1a;判断当前是否进行变道#xff0c;以及变道的状态#xff0c;并将结果存在变量lane_change_status中#xff1b;变道过程中将目标车道的reference line放置到…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_ptrDependencyInjector 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_ptrDependencyInjector injector_;
};modules/planning/tasks/deciders/decider.h
class Decider : public Task {public:explicit Decider(const TaskConfig config);Decider(const TaskConfig config,const std::shared_ptrDependencyInjector 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::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);}//判断是否是强制换道功能如果是调用优先换道功能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::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();// 如果没有配置变道优先则退出该函数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_limitsdouble::max();double end_s -std::numeric_limitsdouble::max();double start_l std::numeric_limitsdouble::max();double end_l -std::numeric_limitsdouble::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;}
}
