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();
// 默认设置false
prev_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 status
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();
}
这里关于参考线不止一条时的主要换道逻辑参考知乎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 now
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_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;
}
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 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;
}
/*
根据前面计算的阈值,判断障碍物是否安全,采用的是滞回区间的方法,如果障碍物小于安全距离,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;
}
}