文章目录
- 前言
- LANE_CHANGE_DECIDER功能简介
- LANE_CHANGE_DECIDER相关配置
- LANE_CHANGE_DECIDER总体流程
- LANE_CHANGE_DECIDER相关子函数
- PrioritizeChangeLane
- UpdateStatus
- IsClearToChangeLane
- HysteresisFilter
- 参考
前言
在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_STAGE
enabled: true
task_type: LANE_CHANGE_DECIDER
task_type: PATH_REUSE_DECIDER
task_type: PATH_LANE_BORROW_DECIDER
task_type: PATH_BOUNDS_DECIDER
task_type: PIECEWISE_JERK_PATH_OPTIMIZER
task_type: PATH_ASSESSMENT_DECIDER
task_type: PATH_DECIDER
task_type: RULE_BASED_STOP_DECIDER
task_type: SPEED_BOUNDS_PRIORI_DECIDER
task_type: SPEED_HEURISTIC_OPTIMIZER
task_type: SPEED_DECIDER
task_type: SPEED_BOUNDS_FINAL_DECIDER
task_type: PIECEWISE_JERK_SPEED_OPTIMIZER
# task_type: PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER
task_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_DECIDER
lane_change_decider_config {
enable_lane_change_urgency_check: false
enable_prioritize_change_lane: false
enable_remove_change_lane: false
reckless_change_lane: false
change_lane_success_freeze_time: 1.5
change_lane_fail_freeze_time: 1.0
}
}
// modules/planning/conf/scenario/lane_follow_config.pb.txt
task_config: {
task_type: LANE_CHANGE_DECIDER
lane_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.proto
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();
// 判断当前参考线是否安全可用
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的ID
const 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) {
// 更新当前时刻，变道完成状态，以及当前道路的ID
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);
}
// 返回LaneChangeDecider::Process 的状态为OK
return Status::OK();
} else { // has change lane in reference lines.
// 获取自车当前所在车道的ID
auto 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 failed
if (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 now
if (!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_line
is_prioritize_change_lane == false: prioritize
non_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;
}
// 初始化SL
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();
// 获取动态障碍物的边界点并转化为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 confs
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;
// 根据方向、自车与运动障碍物之间速度关系设置安全距离
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解析
![java八股文面试[JVM]——JVM参数](https://img-blog.csdnimg.cn/aad554f27ae740339b5c2167bc5cf9b6.png#pic_center)
![LeetCode[面试题04.12]求和路径](https://img-blog.csdnimg.cn/f3f623fe80fe49bc8e34eff7821d011e.png)
![[JavaWeb]【十三】web后端开发-原理篇](https://img-blog.csdnimg.cn/344a1e69a0264e5d9413a3466436acda.png)
