障礙物&參考線&交通規則融合器：Frame類

在Frame類中，主要的工作還是對障礙物預測軌跡(由Predition模塊得到的未來5s內障礙物運動軌跡)、無人車參考線(ReferenceLineProvider類提供)以及當前路況(停車標志、人行橫道、減速帶等)信息進行融合。

實際情況下，能影響無人車運動的不一定只有障礙物，同時還有各個路況，舉個例子：

a. 障礙物影響

情況1：無人車車后的障礙物，對無人車沒太大影響，可以忽略

情況2：無人車前面存在障礙物，無人車就需要停車或者超車

b. 路況影響

情況3：前方存在禁停區或者交叉路口(不考慮信號燈)，那么無人車在參考線上行駛，禁停區區域不能停車

情況4：前方存在人行橫道，若有人，那么需要停車；若沒人，那么無人車可以駛過

所以綜上所述，其實這章節最重要的工作就是結合路況和障礙物軌跡，給每個障礙物(為了保持一致，路況也需要封裝成障礙物形式)一個標簽，這個標簽表示該障礙物存在情況下對無人車的影響，例如有些障礙物可忽略，有些障礙物會促使無人車超車，有些障礙物促使無人車需要停車等。

障礙物信息的獲取策略

無人車參考線ReferenceLineInof初始化(感知模塊障礙物獲取)

依據交通規則對障礙物設定標簽(原始感知障礙物&&路況障礙物)

一. 障礙物信息的獲取策略–滯后預測(Lagged Prediction)

在這個步驟中，主要的工作是獲取Prediction模塊發布的障礙物預測軌跡數據，并且進行后處理工作。首先回顧一下Prediction模塊發布的數據格式：

/// file in apollo/modules/prediction/proto/prediction_obstacle.proto message Trajectory {optional double probability = 1; // probability of this trajectory，障礙物該軌跡運動方案的概率repeated apollo.common.TrajectoryPoint trajectory_point = 2; }message PredictionObstacle {optional apollo.perception.PerceptionObstacle perception_obstacle = 1;optional double timestamp = 2; // GPS time in seconds// the length of the time for this prediction (e.g. 10s)optional double predicted_period = 3;// can have multiple trajectories per obstaclerepeated Trajectory trajectory = 4; }message PredictionObstacles {// timestamp is included in headeroptional apollo.common.Header header = 1;// make prediction for multiple obstaclesrepeated PredictionObstacle prediction_obstacle = 2;// perception error codeoptional apollo.common.ErrorCode perception_error_code = 3;// start timestampoptional double start_timestamp = 4;// end timestampoptional double end_timestamp = 5; }

很明顯的看到可以使用prediction_obstacles.prediction_obstacle()形式獲取所有障礙物的軌跡信息，對于每個障礙物prediction_obstacle，可以使用prediction_obstacle.trajectory()獲取他所有可能運動方案/軌跡。

此外，可以使用const auto& prediction = *(AdapterManager::GetPrediction());來獲取Adapter中所有已發布的歷史消息，最常見的肯定是取最近發布的PredictionObstacles(prediction.GetLatestObserved())，但是Apollo中采用更為精確地障礙物預測獲取方式–滯后預測(Lagged Prediction)，除了使用Prediction模塊最近一次發布的信息，同時還是用歷史信息中的障礙物軌跡預測數據。

/// file in apollo/modules/planning/planning.cc void Planning::RunOnce() {const uint32_t frame_num = AdapterManager::GetPlanning()->GetSeqNum() + 1;status = InitFrame(frame_num, stitching_trajectory.back(), start_timestamp, vehicle_state); }/// file in apollo/modules/planning/common/frame.cc Status Frame::Init() {// predictionif (FLAGS_enable_prediction && AdapterManager::GetPrediction() && !AdapterManager::GetPrediction()->Empty()) {if (FLAGS_enable_lag_prediction && lag_predictor_) { // 滯后預測策略，獲取障礙物軌跡信息lag_predictor_->GetLaggedPrediction(&prediction_);} else { // 不采用滯后預測策略，直接取最近一次Prediction模塊發布的障礙物信息prediction_.CopyFrom(AdapterManager::GetPrediction()->GetLatestObserved());}}... }

采用滯后預測策略獲取障礙物軌跡信息的主要步驟可分為：

最近一次發布的數據直接加入PredictionObstacles容器中

/// file in apollo/modules/planning/common/lag_prediction.cc void LagPrediction::GetLaggedPrediction(PredictionObstacles* obstacles) const {// Step A. 最近一次發布的障礙物軌跡預測信息處理const auto adc_position = AdapterManager::GetLocalization()->GetLatestObserved().pose().position();const auto latest_prediction = (*prediction.begin()); // 記錄最近一次Prediction模塊發布的信息const double timestamp = latest_prediction->header().timestamp_sec(); // 最近一次發布的時間戳std::unordered_set<int> protected_obstacles;for (const auto& obstacle : latest_prediction->prediction_obstacle()) { // 獲取最近一次發布的數據中，每個障礙物的運動軌跡信息const auto& perception = obstacle.perception_obstacle(); double distance = common::util::DistanceXY(perception.position(), adc_position);if (perception.confidence() < FLAGS_perception_confidence_threshold && // 障礙物置信度必須大于0.5，獲取必須是車輛VEHICLE類，否則不作處理perception.type() != PerceptionObstacle::VEHICLE) {continue;}if (distance < FLAGS_lag_prediction_protection_distance) { // 障礙物與車輛之間的距離小于30m，才設置有效protected_obstacles.insert(obstacle.perception_obstacle().id());// add protected obstacleAddObstacleToPrediction(0.0, obstacle, obstacles);}}... }

從上面的代碼可以看到，滯后預測對于最近一次發布的數據處理比較簡單，障礙物信息有效只需要滿足兩個條件：

• 障礙物置信度(Perception模塊CNN分割獲得)必須大于0.5，或者障礙物是車輛類
• 障礙物與車輛之間的距離小于30m

Prediction發布的歷史信息后處理

/// file in apollo/modules/planning/common/lag_prediction.cc void LagPrediction::GetLaggedPrediction(PredictionObstacles* obstacles) const {// Step A 最近一次發布的障礙物軌跡預測信息處理...// Step B 過往發布的歷史障礙物軌跡預測信息處理std::unordered_map<int, LagInfo> obstacle_lag_info;int index = 0; // data in begin() is the most recent datafor (auto it = prediction.begin(); it != prediction.end(); ++it, ++index) { // 對于每一次發布的信息進行處理for (const auto& obstacle : (*it)->prediction_obstacle()) { // 獲取該次發布的歷史數據中，每個障礙物的運動軌跡信息const auto& perception = obstacle.perception_obstacle();auto id = perception.id();if (perception.confidence() < FLAGS_perception_confidence_threshold && // 障礙物置信度必須大于0.5，獲取必須是車輛VEHICLE類，否則不作處理perception.type() != PerceptionObstacle::VEHICLE) {continue;}if (protected_obstacles.count(id) > 0) { // 如果障礙物在最近一次發布的信息中出現了，那就忽略，因為只考慮最新的障礙物信息continue; // don't need to count the already added protected obstacle}auto& info = obstacle_lag_info[id]; ++info.count; // 記錄障礙物在所有歷史信息中出現的次數if ((*it)->header().timestamp_sec() > info.last_observed_time) { // 保存最近一次出現的信息，因為只考慮最新的障礙物信息info.last_observed_time = (*it)->header().timestamp_sec();info.last_observed_seq = index;info.obstacle_ptr = &obstacle;}}}bool apply_lag = std::distance(prediction.begin(), prediction.end()) >= static_cast<int32_t>(min_appear_num_);for (const auto& iter : obstacle_lag_info) {if (apply_lag && iter.second.count < min_appear_num_) { // 歷史信息中如果障礙物出現次數小于min_appear_num_/3次，次數太少，可忽略。continue;}if (apply_lag && iter.second.last_observed_seq > max_disappear_num_) { // 歷史信息中如果障礙物最近一次發布距離現在過遠，可忽略。continue;}AddObstacleToPrediction(timestamp - iter.second.last_observed_time,*(iter.second.obstacle_ptr), obstacles);} }

所以最后做一個總結，對于歷史發布數據，如何判斷這些障礙物軌跡信息是否有效。兩個步驟：

步驟1：記錄歷史發布數據中每個障礙物出現的次數(在最近依次發布中出現的障礙物忽略，因為不是最新的數據了)，必須滿足兩個條件：

• 障礙物置信度(Perception模塊CNN分割獲得)必須大于0.5，或者障礙物是車輛類
• 障礙物與車輛之間的距離小于30m

步驟2：對于步驟1中得到的障礙物信息，進行篩選，信息有效需要滿足兩個條件：

• 信息隊列中歷史數據大于3(min_appear_num_)，并且每個障礙物出現次數大于3(min_appear_num_)
• 信息隊列中歷史數據大于3(min_appear_num_)，并且障礙物信息上一次發布距離最近一次發布不大于5(max_disappear_num_)，需要保證數據的最近有效性。

二. 無人車與障礙物相對位置的設置–ReferenceLineInfo類初始化

從**1. 障礙物信息的獲取策略–滯后預測(Lagged Prediction)**中可以得到障礙物短期(未來5s)內的運動軌跡；從ReferenceLineProvider類中我們可以得到車輛的理想規劃軌跡。下一步就是將障礙物的軌跡信息加入到這條規劃好的參考線ReferenceLine中，確定在什么時間點，無人車能前進到什么位置，需要保證在這個時間點上，障礙物與無人車不相撞。這個工作依舊是在Frame::Init()中完成，主要是完成ReferenceLineInfo類的生成，這個類綜合了障礙物預測軌跡與無人車規劃軌跡的信息，同時也是最后路徑規劃的基礎類。

/// file in apollo/modules/planning/common/frame.cc Status Frame::Init() {// Step A prediction，障礙物預測軌跡信息獲取，采用滯后預測策略...// Step B.1 檢查當前時刻(relative_time=0.0s)，無人車位置和障礙物位置是否重疊(相撞)，如果是，可以直接退出const auto *collision_obstacle = FindCollisionObstacle(); if (collision_obstacle) { AERROR << "Found collision with obstacle: " << collision_obstacle->Id();return Status(ErrorCode::PLANNING_ERROR, "Collision found with " + collision_obstacle->Id());}// Step B.2 如果當前時刻不沖突，檢查未來時刻無人車可以在參考線上前進的位置，ReferenceLineInfo生成if (!CreateReferenceLineInfo()) { //AERROR << "Failed to init reference line info";return Status(ErrorCode::PLANNING_ERROR, "failed to init reference line info");}return Status::OK(); }

如何完成ReferenceLineInfo類的初始化工作，其實比較簡單，主要有兩個過程：

• 根據無人車規劃路徑ReferenceLine實例化ReferenceLineInfo類，數量與ReferenceLine一致
• 根據障礙物軌跡初始化ReferenceLineInfo::path_decision_

實例化ReferenceLineInfo類

/// file in apollo/modules/planning/common/frame.cc bool Frame::CreateReferenceLineInfo() {// Step A 從ReferenceLineProvider中獲取無人車的短期內規劃路徑ReferenceLine，并進行收縮操作std::list<ReferenceLine> reference_lines;std::list<hdmap::RouteSegments> segments;if (!reference_line_provider_->GetReferenceLines(&reference_lines, &segments)) { return false;}// 對于每條ReferenceLine實例化生成一個對應的ReferenceLineInforeference_line_info_.clear();auto ref_line_iter = reference_lines.begin();auto segments_iter = segments.begin();while (ref_line_iter != reference_lines.end()) {if (segments_iter->StopForDestination()) {is_near_destination_ = true;}reference_line_info_.emplace_back(vehicle_state_, planning_start_point_,*ref_line_iter, *segments_iter);++ref_line_iter;++segments_iter;} }/// file in apollo/modules/planning/common/reference_line_info.cc ReferenceLineInfo::ReferenceLineInfo(const common::VehicleState& vehicle_state,const TrajectoryPoint& adc_planning_point,const ReferenceLine& reference_line,const hdmap::RouteSegments& segments): vehicle_state_(vehicle_state),adc_planning_point_(adc_planning_point),reference_line_(reference_line),lanes_(segments) {}

這個規程比較簡單，就是從ReferenceLineProvider中提取無人車短期內的規劃路徑，如果不了解，可以查看(組件)指引線提供器: ReferenceLineProvider。然后由一條ReferenceLine&&segments、車輛狀態和規劃起始點生成對應的ReferenceLineInfo

初始化ReferenceLineInfo類

/// file in apollo/modules/planning/common/frame.cc bool Frame::CreateReferenceLineInfo() {// Step A 從ReferenceLineProvider中獲取無人車的短期內規劃路徑ReferenceLine，并進行收縮操作...// Step B RerfenceLineInfo初始化bool has_valid_reference_line = false;for (auto &ref_info : reference_line_info_) {if (!ref_info.Init(obstacles())) {continue;} else {has_valid_reference_line = true;}}return has_valid_reference_line; }

在ReferenceLineInfo::Init(const std::vector<const Obstacle*>& obstacles)函數中，主要做的工作也是比較簡單，這里做一下總結：

• 檢查無人車是否在參考線上

需要滿足無人車對應的邊界框start_s和end_s在參考線[0, total_length]區間內

• 檢查無人車是否離參考線過遠

需要滿足無人車start_l和end_l在[-kOutOfReferenceLineL, kOutOfReferenceLineL]區間內，其中kOutOfReferenceLineL取10

• 將障礙物信息加入到ReferenceLineInfo類中

除了將障礙物信息加入到類中，還有一個重要的工作就是確定某個時間點無人車能前進到的位置，如下圖所示。

可以看到這個過程其實就是根據障礙物的軌跡(某個相對時間點，障礙物運動到哪個坐標位置)，并結合無人車查詢得到的理想路徑，得到某個時間點low_t和high_t無人車行駛距離的下界low_s-adc_start_s和上界high_s-adc_start_s

/// file in apollo/modules/planning/common/reference_line_info.cc // AddObstacle is thread safe PathObstacle* ReferenceLineInfo::AddObstacle(const Obstacle* obstacle) {// 封裝成PathObstacle并加入PathDecisionauto* path_obstacle = path_decision_.AddPathObstacle(PathObstacle(obstacle));...// 計算障礙物框的start_s, end_s, start_l和end_lSLBoundary perception_sl;if (!reference_line_.GetSLBoundary(obstacle->PerceptionBoundingBox(), &perception_sl)) {return path_obstacle;}path_obstacle->SetPerceptionSlBoundary(perception_sl);// 計算障礙物是否對無人車行駛有影響：無光障礙物滿足以下條件：// 1. 障礙物在ReferenceLine以外，忽略// 2. 車輛和障礙物都在車道上，但是障礙物在無人車后面，忽略if (IsUnrelaventObstacle(path_obstacle)) {// 忽略障礙物} else {// 構建障礙物在參考線上的邊界框path_obstacle->BuildReferenceLineStBoundary(reference_line_, adc_sl_boundary_.start_s());}return path_obstacle; } /// file in apollo/modules/planning/common/path_obstacle.cc void PathObstacle::BuildReferenceLineStBoundary(const ReferenceLine& reference_line, const double adc_start_s) {if (obstacle_->IsStatic() || obstacle_->Trajectory().trajectory_point().empty()) {...} else {if (BuildTrajectoryStBoundary(reference_line, adc_start_s, &reference_line_st_boundary_)) {...} else {ADEBUG << "No st_boundary for obstacle " << id_;}} }

可以觀察PathObstacle::BuildTrajectoryStBoundary函數，我們簡單的進行代碼段分析：

Step 1. 首先還是對障礙物軌跡點兩兩選擇，每兩個點可以構建上圖中的object_moving_box以及object_boundary。

bool PathObstacle::BuildTrajectoryStBoundary(const ReferenceLine& reference_line, const double adc_start_s,StBoundary* const st_boundary) {for (int i = 1; i < trajectory_points.size(); ++i) {const auto& first_traj_point = trajectory_points[i - 1];const auto& second_traj_point = trajectory_points[i];const auto& first_point = first_traj_point.path_point();const auto& second_point = second_traj_point.path_point();double total_length = object_length + common::util::DistanceXY(first_point, second_point); //object_moving_box總長度common::math::Vec2d center((first_point.x() + second_point.x()) / 2.0, // object_moving_box中心(first_point.y() + second_point.y()) / 2.0);common::math::Box2d object_moving_box(center, first_point.theta(), total_length, object_width);... // 計算object_boundary，由object_moving_box旋轉一個heading得到，記錄障礙物形式段的start_s, end_s, start_l和end_lif (!reference_line.GetApproximateSLBoundary(object_moving_box, start_s, end_s, &object_boundary)) { return false;}} }

Step 2. 判斷障礙物和車輛水平Lateral距離，如果障礙物在參考線兩側，那么障礙物可以忽略；如果障礙物在參考線后面，也可忽略

// skip if object is entirely on one side of reference line.constexpr double kSkipLDistanceFactor = 0.4;const double skip_l_distance =(object_boundary.end_s() - object_boundary.start_s()) *kSkipLDistanceFactor + adc_width / 2.0;if (std::fmin(object_boundary.start_l(), object_boundary.end_l()) > // 障礙物在參考線左側，那么無人車可以直接通過障礙物，可忽略障礙物skip_l_distance ||std::fmax(object_boundary.start_l(), object_boundary.end_l()) < // 障礙物在參考線右側，那么無人車可以直接通過障礙物，可忽略障礙物-skip_l_distance) {continue;}if (object_boundary.end_s() < 0) { // 障礙物在參考線后面，可忽略障礙物continue;}

Step 3. 計算low_t和high_t時刻的行駛上下界邊界框

const double delta_t = second_traj_point.relative_time() - first_traj_point.relative_time(); // 0.1sdouble low_s = std::max(object_boundary.start_s() - adc_half_length, 0.0);bool has_low = false;double high_s = std::min(object_boundary.end_s() + adc_half_length, FLAGS_st_max_s);bool has_high = false;while (low_s + st_boundary_delta_s < high_s && !(has_low && has_high)) {if (!has_low) { // 采用漸進逼近的方法，逐漸計算邊界框的下界auto low_ref = reference_line.GetReferencePoint(low_s);has_low = object_moving_box.HasOverlap({low_ref, low_ref.heading(), adc_length, adc_width});low_s += st_boundary_delta_s;}if (!has_high) { // 采用漸進逼近的方法，逐漸計算邊界框的上界auto high_ref = reference_line.GetReferencePoint(high_s);has_high = object_moving_box.HasOverlap({high_ref, high_ref.heading(), adc_length, adc_width});high_s -= st_boundary_delta_s;}}if (has_low && has_high) {low_s -= st_boundary_delta_s;high_s += st_boundary_delta_s;double low_t = (first_traj_point.relative_time() +std::fabs((low_s - object_boundary.start_s()) / object_s_diff) * delta_t);polygon_points.emplace_back( // 計算low_t時刻的上下界std::make_pair(STPoint{low_s - adc_start_s, low_t},STPoint{high_s - adc_start_s, low_t}));double high_t =(first_traj_point.relative_time() +std::fabs((high_s - object_boundary.start_s()) / object_s_diff) * delta_t);if (high_t - low_t > 0.05) {polygon_points.emplace_back( // 計算high_t時刻的上下界std::make_pair(STPoint{low_s - adc_start_s, high_t},STPoint{high_s - adc_start_s, high_t}));}}

Step 4. 計算完所有障礙物軌跡段的上下界框以后，根據時間t進行排序

if (!polygon_points.empty()) {std::sort(polygon_points.begin(), polygon_points.end(),[](const std::pair<STPoint, STPoint>& a,const std::pair<STPoint, STPoint>& b) {return a.first.t() < b.first.t();});auto last = std::unique(polygon_points.begin(), polygon_points.end(),[](const std::pair<STPoint, STPoint>& a,const std::pair<STPoint, STPoint>& b) {return std::fabs(a.first.t() - b.first.t()) <kStBoundaryDeltaT;});polygon_points.erase(last, polygon_points.end());if (polygon_points.size() > 2) {*st_boundary = StBoundary(polygon_points);}} else {return false;}

總結一下**無人車參考線ReferenceLineInof初始化(加入障礙物軌跡信息)**這步的功能，給定了無人車的規劃軌跡ReferenceLine和障礙物的預測軌跡PredictionObstacles，這個過程其實就是計算障礙物在無人車規劃軌跡上的重疊部分的位置s，以及駛到這個重疊部分的時間點t，為第三部分每個障礙物在每個車輛上的初步決策進行規劃。

三. 依據交通規則對障礙物設定標簽

二中已經給出了每個障礙物在所有的參考線上的重疊部分的位置與時間點，這些重疊部分就是無人車需要考慮的規劃矯正問題，防止交通事故。因為障礙物運動可能跨越多個ReferenceLine，所以這里需要對于每個障礙物進行標定，是否可忽略，是否會超車等。交通規則判定情況一共11種，在文件modules/planning/conf/traffic_rule_config.pb.txt中定義，這里我們將一一列舉。

后車情況處理–BACKSIDE_VEHICLE

變道情況處理–CHANGE_LANE

人行橫道情況處理–CROSSWALK

目的地情況處理–DESTINATION

前車情況處理–FRONT_VEHICLE

禁停區情況處理–KEEP_CLEAR

尋找停車點狀態–PULL_OVER

參考線結束情況處理–REFERENCE_LINE_END

重新路由查詢情況處理–REROUTING

信號燈情況處理–SIGNAL_LIGHT

停車情況處理–STOP_SIGN

/// file in apollo/modules/planning/planning.cc void Planning::RunOnce() {const uint32_t frame_num = AdapterManager::GetPlanning()->GetSeqNum() + 1;status = InitFrame(frame_num, stitching_trajectory.back(), start_timestamp, vehicle_state);for (auto& ref_line_info : frame_->reference_line_info()) {TrafficDecider traffic_decider;traffic_decider.Init(traffic_rule_configs_);auto traffic_status = traffic_decider.Execute(frame_.get(), &ref_line_info);if (!traffic_status.ok() || !ref_line_info.IsDrivable()) {ref_line_info.SetDrivable(false);continue;}} }/// file in apollo/modules/planning/tasks/traffic_decider/traffic_decider.cc Status TrafficDecider::Execute(Frame *frame, ReferenceLineInfo *reference_line_info) {for (const auto &rule_config : rule_configs_.config()) { // 對于每條參考線進行障礙物的決策。if (!rule_config.enabled()) {continue;}auto rule = s_rule_factory.CreateObject(rule_config.rule_id(), rule_config);if (!rule) {continue;}rule->ApplyRule(frame, reference_line_info);}BuildPlanningTarget(reference_line_info);return Status::OK(); }

3.1 后車情況處理–BACKSIDE_VEHICLE

/// file in apollo/modules/planning/tasks/traffic_decider/backside_vehicle.cc Status BacksideVehicle::ApplyRule(Frame* const, ReferenceLineInfo* const reference_line_info) {auto* path_decision = reference_line_info->path_decision();const auto& adc_sl_boundary = reference_line_info->AdcSlBoundary();if (reference_line_info->Lanes().IsOnSegment()) { // The lane keeping reference line.MakeLaneKeepingObstacleDecision(adc_sl_boundary, path_decision);}return Status::OK(); }void BacksideVehicle::MakeLaneKeepingObstacleDecision(const SLBoundary& adc_sl_boundary, PathDecision* path_decision) {ObjectDecisionType ignore; // 從這里可以看到，對于后車的處理主要是忽略ignore.mutable_ignore();const double adc_length_s = adc_sl_boundary.end_s() - adc_sl_boundary.start_s(); // 計算"車長""for (const auto* path_obstacle : path_decision->path_obstacles().Items()) { // 對于每個與參考線有重疊的障礙物進行規則設置... } }

從上述代碼可以看到后車情況處理僅僅針對當前無人車所在的參考線，那些臨近的參考線不做考慮。Apollo主要的處理方式其實是忽略后車，可以分一下情況：

• 前車在這里不做考慮，由前車情況處理FRONT_VEHICLE來完成

if (path_obstacle->PerceptionSLBoundary().end_s() >= adc_sl_boundary.end_s()) { // don't ignore such vehicles.continue; }

• 參考線上沒有障礙物運動軌跡，直接忽略

if (path_obstacle->reference_line_st_boundary().IsEmpty()) {path_decision->AddLongitudinalDecision("backside_vehicle/no-st-region", path_obstacle->Id(), ignore);path_decision->AddLateralDecision("backside_vehicle/no-st-region", path_obstacle->Id(), ignore);continue; }

• 忽略從無人車后面來的車輛

// Ignore the car comes from back of ADC if (path_obstacle->reference_line_st_boundary().min_s() < -adc_length_s) { path_decision->AddLongitudinalDecision("backside_vehicle/st-min-s < adc", path_obstacle->Id(), ignore);path_decision->AddLateralDecision("backside_vehicle/st-min-s < adc", path_obstacle->Id(), ignore);continue; }

從代碼中可以看到，通過計算min_s，也就是障礙物軌跡距離無人車最近的距離，小于半車長度，說明車輛在無人車后面，可暫時忽略。

• 忽略后面不會超車的車輛

const double lane_boundary = config_.backside_vehicle().backside_lane_width(); // 4mif (path_obstacle->PerceptionSLBoundary().start_s() < adc_sl_boundary.end_s()) {if (path_obstacle->PerceptionSLBoundary().start_l() > lane_boundary ||path_obstacle->PerceptionSLBoundary().end_l() < -lane_boundary) {continue;}path_decision->AddLongitudinalDecision("backside_vehicle/sl < adc.end_s", path_obstacle->Id(), ignore);path_decision->AddLateralDecision("backside_vehicle/sl < adc.end_s", path_obstacle->Id(), ignore);continue;} }

從代碼中可以看到，第一個if可以選擇那些在無人車后(至少不超過無人車)的車輛，第二個if，可以計算車輛與無人車的橫向距離，如果超過閾值，那么就說明車輛橫向距離無人車足夠遠，有可能會進行超車，這種情況不能忽略，留到后面的交通規則去處理；若小于這個閾值，則可以間接說明不太會超車，后車可能跟隨無人車前進。

3.2 變道情況處理–CHANGE_LANE

在變道情況下第一步要找到那些需要警惕的障礙物(包括跟隨這輛和超車車輛)，這些障礙物軌跡是會影響無人車的變道軌跡，然后設置每個障礙物設立一個超車的警示(障礙物和無人車的位置與速度等信息)供下一步規劃階段參考。Apollo對于每條參考線每次只考慮距離無人車最近的能影響變道的障礙物，同時設置那些超車的障礙物。

/// file in apollo/modules/planning/tasks/traffic_decider/change_lane.cc Status ChangeLane::ApplyRule(Frame* const frame, ReferenceLineInfo* const reference_line_info) {// 如果是直行道，不需要變道，則忽略if (reference_line_info->Lanes().IsOnSegment()) {return Status::OK();}// 計算警示障礙物&超車障礙物guard_obstacles_.clear();overtake_obstacles_.clear();if (!FilterObstacles(reference_line_info)) {return Status(common::PLANNING_ERROR, "Failed to filter obstacles");}// 創建警示障礙物類if (config_.change_lane().enable_guard_obstacle() && !guard_obstacles_.empty()) {for (const auto path_obstacle : guard_obstacles_) {auto* guard_obstacle = frame->Find(path_obstacle->Id());if (guard_obstacle && CreateGuardObstacle(reference_line_info, guard_obstacle)) {AINFO << "Created guard obstacle: " << guard_obstacle->Id();}}}// 設置超車標志if (!overtake_obstacles_.empty()) {auto* path_decision = reference_line_info->path_decision();const auto& reference_line = reference_line_info->reference_line();for (const auto* path_obstacle : overtake_obstacles_) {auto overtake = CreateOvertakeDecision(reference_line, path_obstacle);path_decision->AddLongitudinalDecision(TrafficRuleConfig::RuleId_Name(Id()), path_obstacle->Id(), overtake);}}return Status::OK(); }

超車&警示障礙物計算

• 沒有軌跡的障礙物忽略

if (!obstacle->HasTrajectory()) {continue; }

• 無人車前方的車輛忽略，對變道沒有影響

if (path_obstacle->PerceptionSLBoundary().start_s() > adc_sl_boundary.end_s()) {continue; }

• 跟車在一定距離(10m)內，將其標記為超車障礙物

if (path_obstacle->PerceptionSLBoundary().end_s() <adc_sl_boundary.start_s() -std::max(config_.change_lane().min_overtake_distance(), // min_overtake_distance: 10mobstacle->Speed() * min_overtake_time)) { // min_overtake_time: 2sovertake_obstacles_.push_back(path_obstacle); }

• 障礙物速度很小或者障礙物最后不在參考線上，對變道沒影響，可忽略

if (last_point.v() < config_.change_lane().min_guard_speed()) {continue; } if (!reference_line.IsOnRoad(last_point.path_point())) {continue; }

• 障礙物最后一規劃點在參考線上，但是超過了無人車一定距離，對變道無影響

SLPoint last_sl; if (!reference_line.XYToSL(last_point.path_point(), &last_sl)) {continue; } if (last_sl.s() < 0 || last_sl.s() > adc_sl_boundary.end_s() + kGuardForwardDistance) {continue; }

創建警示障礙物類

創建警示障礙物類本質其實就是預測障礙物未來一段時間內的運動軌跡，代碼在ChangeLane::CreateGuardObstacle中很明顯的給出了障礙物軌跡的預測方法。預測的軌跡是在原始軌跡上進行拼接，即在最后一個軌跡點后面再次進行預測，這次預測的假設是，障礙物沿著參考線形式。幾個注意點：

預測長度：障礙物預測軌跡重點到無人車前方100m(config_.change_lane().guard_distance())這段距離

障礙物速度假定：這段距離內，默認障礙物速度和最后一個軌跡點速度一致extend_v，并且驗證參考線前進

預測頻率: 沒兩個點之間的距離為障礙物長度，所以兩個點之間的相對時間差為：time_delta = kStepDistance / extend_v

創建障礙物超車標簽

/// file in apollo/modules/planning/tasks/traffic_decider/change_lane.cc ObjectDecisionType ChangeLane::CreateOvertakeDecision(const ReferenceLine& reference_line, const PathObstacle* path_obstacle) const {ObjectDecisionType overtake;overtake.mutable_overtake();const double speed = path_obstacle->obstacle()->Speed();double distance = std::max(speed * config_.change_lane().min_overtake_time(), // 設置變道過程中，障礙物運動距離config_.change_lane().min_overtake_distance());overtake.mutable_overtake()->set_distance_s(distance); double fence_s = path_obstacle->PerceptionSLBoundary().end_s() + distance; auto point = reference_line.GetReferencePoint(fence_s); // 設置變道完成后，障礙物在參考線上的位置overtake.mutable_overtake()->set_time_buffer(config_.change_lane().min_overtake_time()); // 設置變道需要的最小時間overtake.mutable_overtake()->set_distance_s(distance); // 設置變道過程中，障礙物前進的距離overtake.mutable_overtake()->set_fence_heading(point.heading());overtake.mutable_overtake()->mutable_fence_point()->set_x(point.x()); // 設置變道完成后，障礙物的坐標overtake.mutable_overtake()->mutable_fence_point()->set_y(point.y());overtake.mutable_overtake()->mutable_fence_point()->set_z(0.0);return overtake; }

3.3 人行橫道情況處理–CROSSWALK

對于人行橫道部分，根據禮讓規則當行人或者非機動車距離很遠，無人車可以開過人行橫道；當人行橫道上有人經過時，必須停車讓行。

/// file in apollo/modules/planning/tasks/traffic_decider/crosswalk.cc Status Crosswalk::ApplyRule(Frame* const frame, ReferenceLineInfo* const reference_line_info) {// 檢查是否存在人行橫道區域if (!FindCrosswalks(reference_line_info)) {return Status::OK();}// 為每個障礙物做標記，障礙物存在時，無人車應該停車還是直接駛過MakeDecisions(frame, reference_line_info);return Status::OK(); }

檢查在每個人行橫道區域內，是否存在障礙物需要無人車停車讓行

• 如果車輛已經駛過人行橫道一部分了，那么就忽略，不需要停車

// skip crosswalk if master vehicle body already passes the stop line double stop_line_end_s = crosswalk_overlap->end_s; if (adc_front_edge_s - stop_line_end_s > config_.crosswalk().min_pass_s_distance()) { // 車頭駛過人行橫道一定距離，min_pass_s_distance：1.0continue; }

遍歷每個人行道區域的障礙物(行人和非機動車)，并將人行橫道區域擴展，提高安全性。

• 如果障礙物不在擴展后的人行橫道內，則忽略。(可能在路邊或者其他區域)

// expand crosswalk polygon // note: crosswalk expanded area will include sideway area Vec2d point(perception_obstacle.position().x(),perception_obstacle.position().y()); const Polygon2d crosswalk_poly = crosswalk_ptr->polygon(); bool in_crosswalk = crosswalk_poly.IsPointIn(point); const Polygon2d crosswalk_exp_poly = crosswalk_poly.ExpandByDistance(config_.crosswalk().expand_s_distance()); bool in_expanded_crosswalk = crosswalk_exp_poly.IsPointIn(point);if (!in_expanded_crosswalk) {continue; }

計算障礙物到參考線的橫向距離obstacle_l_distance，是否在參考線上(附近)is_on_road，障礙物軌跡是否與參考線相交is_path_cross

• 如果橫向距離大于疏松距離。如果軌跡相交，那么需要停車；反之直接駛過(此時軌跡相交無所謂，因為橫向距離比較遠)

if (obstacle_l_distance >= config_.crosswalk().stop_loose_l_distance()) { // stop_loose_l_distance: 5.0m// (1) when obstacle_l_distance is big enough(>= loose_l_distance), // STOP only if path crossesif (is_path_cross) {stop = true;} }

• 如果橫向距離小于緊湊距離。如果障礙物在參考線或者軌跡相交，那么需要停車；反之直接駛過

else if (obstacle_l_distance <= config_.crosswalk().stop_strick_l_distance()) { // stop_strick_l_distance: 4.0m// (2) when l_distance <= strick_l_distance + on_road(not on sideway),// always STOP// (3) when l_distance <= strick_l_distance + not on_road(on sideway),// STOP only if path crossesif (is_on_road || is_path_cross) {stop = true;} }

• 如果橫向距離在緊湊距離和疏松距離之間，直接停車

else {// TODO(all)// (4) when l_distance is between loose_l and strick_l// use history decision of this crosswalk to smooth unsteadinessstop = true; }

如果存在障礙物需要無人車停車讓行，最后可以計算無人車的加速度(是否來的及減速，若無人車速度很快減速不了，那么干脆直接駛過)。計算加速的公式還是比較簡單

$$0?v^2=2as$$

s為當前到車輛停止駛過的距離，物理公式，由函數util::GetADCStopDeceleration完成。

對那些影響無人車行駛的障礙物構建虛擬墻障礙物類以及設置停車標簽

什么是構建虛擬墻類，其實很簡單，單一的障礙物是一個很小的框，那么無人車在行駛過程中必須要與障礙物保持一定的距離，那么只要以障礙物為中心，構建一個長度為0.1，寬度為車道寬度的虛擬墻，只要保證無人車和這個虛擬墻障礙物不相交，就能確保安全。

// create virtual stop wall std::string virtual_obstacle_id =CROSSWALK_VO_ID_PREFIX + crosswalk_overlap->object_id; auto* obstacle = frame->CreateStopObstacle(reference_line_info, virtual_obstacle_id, crosswalk_overlap->start_s); if (!obstacle) {AERROR << "Failed to create obstacle[" << virtual_obstacle_id << "]";return -1; } PathObstacle* stop_wall = reference_line_info->AddObstacle(obstacle); if (!stop_wall) {AERROR << "Failed to create path_obstacle for: " << virtual_obstacle_id;return -1; }

雖然看起來函數跳轉比較多，但是其實與二中的障礙物PredictionObstacle封裝成Obstacle一樣，無非是多加了一個區域框Box2d而已。最后就是對這些虛擬墻添加停車標志

// build stop decision const double stop_s = // 計算停車位置的累計距離，stop_distance：1m，人行橫道前1m處停車crosswalk_overlap->start_s - config_.crosswalk().stop_distance(); auto stop_point = reference_line.GetReferencePoint(stop_s); double stop_heading = reference_line.GetReferencePoint(stop_s).heading();ObjectDecisionType stop; auto stop_decision = stop.mutable_stop(); stop_decision->set_reason_code(StopReasonCode::STOP_REASON_CROSSWALK); stop_decision->set_distance_s(-config_.crosswalk().stop_distance()); stop_decision->set_stop_heading(stop_heading); // 設置停車點的角度/方向 stop_decision->mutable_stop_point()->set_x(stop_point.x()); // 設置停車點的坐標 stop_decision->mutable_stop_point()->set_y(stop_point.y()); stop_decision->mutable_stop_point()->set_z(0.0);for (auto pedestrian : pedestrians) {stop_decision->add_wait_for_obstacle(pedestrian); // 設置促使無人車停車的障礙物id }auto* path_decision = reference_line_info->path_decision(); path_decision->AddLongitudinalDecision( TrafficRuleConfig::RuleId_Name(config_.rule_id()), stop_wall->Id(), stop);

3.4 目的地情況處理–DESTINATION

到達目的地情況下，障礙物促使無人車采取的行動無非是靠邊停車或者尋找合適的停車點(距離目的地還有一小段距離，不需要激勵停車)。決策規則比較簡單：

主要的決策邏輯

• 若當前正處于PULL_OVER狀態，則繼續保持

auto* planning_state = GetPlanningStatus()->mutable_planning_state(); if (planning_state->has_pull_over() && planning_state->pull_over().in_pull_over()) {PullOver(nullptr);ADEBUG << "destination: continue PULL OVER";return 0; }

• 檢查車輛當前是否需要進入ULL_OVER，主要參考和目的地之間的距離以及是否允許使用PULL_OVER

const auto& routing = AdapterManager::GetRoutingResponse()->GetLatestObserved(); const auto& routing_end = *routing.routing_request().waypoint().rbegin(); double dest_lane_s = std::max( // stop_distance: 0.5，目的地0.5m前停車0.0, routing_end.s() - FLAGS_virtual_stop_wall_length -config_.destination().stop_distance()); common::PointENU dest_point; if (CheckPullOver(reference_line_info, routing_end.id(), dest_lane_s, &dest_point)) {PullOver(&dest_point); } else {Stop(frame, reference_line_info, routing_end.id(), dest_lane_s); }

CheckPullOver檢查機制(Apollo在DESTINATION中不啟用PULL_OVER)

• 若在目的地情況下不啟用PULL_OVER機制，則返回false

if (!config_.destination().enable_pull_over()) {return false; }

• 若目的地不在參考線上，返回false

const auto dest_lane = HDMapUtil::BaseMapPtr()->GetLaneById(hdmap::MakeMapId(lane_id)); const auto& reference_line = reference_line_info->reference_line(); // check dest OnRoad double dest_lane_s = std::max(0.0, lane_s - FLAGS_virtual_stop_wall_length -config_.destination().stop_distance()); *dest_point = dest_lane->GetSmoothPoint(dest_lane_s); if (!reference_line.IsOnRoad(*dest_point)) {return false; }

• 若無人車與目的地距離太遠，則返回false

// check dest within pull_over_plan_distance common::SLPoint dest_sl; if (!reference_line.XYToSL({dest_point->x(), dest_point->y()}, &dest_sl)) {return false; } double adc_front_edge_s = reference_line_info->AdcSlBoundary().end_s(); double distance_to_dest = dest_sl.s() - adc_front_edge_s; // pull_over_plan_distance: 55m if (distance_to_dest > config_.destination().pull_over_plan_distance()) {// to far, not sending pull-over yetreturn false; }

障礙物PULL_OVER和STOP標簽設定

int Destination::PullOver(common::PointENU* const dest_point) {auto* planning_state = GetPlanningStatus()->mutable_planning_state();if (!planning_state->has_pull_over() || !planning_state->pull_over().in_pull_over()) {planning_state->clear_pull_over();auto pull_over = planning_state->mutable_pull_over();pull_over->set_in_pull_over(true);pull_over->set_reason(PullOverStatus::DESTINATION);pull_over->set_status_set_time(Clock::NowInSeconds());if (dest_point) {pull_over->mutable_inlane_dest_point()->set_x(dest_point->x());pull_over->mutable_inlane_dest_point()->set_y(dest_point->y());}}return 0; }

Stop標簽設定和人行橫道情況處理中STOP一致，創建虛擬墻，并封裝成新的PathObstacle加入該ReferenceLineInfo的PathDecision中。

3.5 前車情況處理–FRONT_VEHICLE

在存在前車的情況下，障礙物對無人車的決策影響有兩個，一是跟車等待機會超車(也有可能一直跟車，視障礙物信息決定)；而是停車(障礙物是靜態的，無人車必須停車)

跟車等待機會超車處理

當前車是運動的，而且超車條件良好，那么就存在超車的可能，這里定義的超車指代的需要無人車調整橫向距離以后超車，直接駛過前方障礙物的屬于正常行駛，可忽略障礙物。要完成超車行為需要經過4個步驟：

• 正常駕駛(DRIVE)
• 等待超車(WAIT)
• 超車(SIDEPASS)
• 正常行駛(DRIVE)

若距離前過遠，那么無人車就處理第一個階段正常行駛，若距離比較近，但是不滿足超車條件，那么無人車將一直跟隨前車正常行駛；若距離近但是無量速度很小(堵車)，那么就需要等待；若滿足條件了，就立即進入到超車過程，超車完成就又回到正常行駛狀態。

Step 1. 檢查超車條件–FrontVehicle::FindPassableObstacle

這個過程其實就是尋找有沒有影響無人車正常行駛的障礙物(無人車可能需要超車)，遍歷PathDecision中的所有PathObstacle，依次檢查以下條件：

• 是否是虛擬或者靜態障礙物。若是，那么就需要采取停車策略，超車部分對這些不作處理

if (path_obstacle->obstacle()->IsVirtual() || !path_obstacle->obstacle()->IsStatic()) {ADEBUG << "obstacle_id[" << obstacle_id << "] type[" << obstacle_type_name<< "] VIRTUAL or NOT STATIC. SKIP";continue; }

• 檢車障礙物與無人車位置關系，障礙物在無人車后方，直接忽略。

const auto& obstacle_sl = path_obstacle->PerceptionSLBoundary(); if (obstacle_sl.start_s() <= adc_sl_boundary.end_s()) {ADEBUG << "obstacle_id[" << obstacle_id << "] type[" << obstacle_type_name<< "] behind ADC. SKIP";continue; }

• 檢查橫向與縱向距離，若縱向距離過遠則可以忽略，依舊進行正常行駛；若側方距離過大，可直接忽略障礙物，直接正常向前行駛。

const double side_pass_s_threshold = config_.front_vehicle().side_pass_s_threshold(); // side_pass_s_threshold: 15m if (obstacle_sl.start_s() - adc_sl_boundary.end_s() > side_pass_s_threshold) {continue; } const double side_pass_l_threshold = config_.front_vehicle().side_pass_l_threshold(); // side_pass_l_threshold: 1m if (obstacle_sl.start_l() > side_pass_l_threshold || obstacle_sl.end_l() < -side_pass_l_threshold) {continue; }

FrontVehicle::FindPassableObstacle函數最后會返回第一個找到的限制無人車行駛的障礙物

Step 2. 設定超車各階段標志–FrontVehicle::ProcessSidePass

• 若上階段處于SidePassStatus::UNKNOWN狀態，設置為正常行駛

• 若上階段處于SidePassStatus::DRIVE狀態，并且障礙物阻塞(存在需要超車條件)，那么設置為等待超車；否則前方沒有障礙物，繼續正常行駛

case SidePassStatus::DRIVE: {constexpr double kAdcStopSpeedThreshold = 0.1; // unit: m/sconst auto& adc_planning_point = reference_line_info->AdcPlanningPoint();if (!passable_obstacle_id.empty() && // 前方有障礙物則需要等待adc_planning_point.v() < kAdcStopSpeedThreshold) {sidepass_status->set_status(SidePassStatus::WAIT);sidepass_status->set_wait_start_time(Clock::NowInSeconds());}break; }

• 若上階段處于SidePassStatus::WAIT狀態，若前方已無障礙物，設置為正常行駛；否則視情況而定：

a. 前方已無妨礙物，設置為正常行駛

b. 前方有障礙物，檢查已等待的時間，超過閾值，尋找左右有效車道超車；若無有效車道，則一直堵車等待。

double wait_start_time = sidepass_status->wait_start_time(); double wait_time = Clock::NowInSeconds() - wait_start_time; // 計算已等待的時間if (wait_time > config_.front_vehicle().side_pass_wait_time()) { // 超過閾值，尋找其他車道超車。side_pass_wait_time：30s }

首先查詢HDMap，計算當前ReferenceLine所在的車道Lane。若存在坐車道，那么設置做超車；若不存在坐車道，那么檢查右車道，右車道如果是機動車道(CITY_DRIVING)，不是非機動車道(BIKING)，不是步行街(SIDEWALK)，也不是停車道(PAKING)，那么可以進行右超車。

if (enter_sidepass_mode) {sidepass_status->set_status(SidePassStatus::SIDEPASS);sidepass_status->set_pass_obstacle_id(passable_obstacle_id);sidepass_status->clear_wait_start_time();sidepass_status->set_pass_side(side); // side知識左超車還是右超車。取值為ObjectSidePass::RIGHT或ObjectSidePass::LEFT }

• 若上階段處于SidePassStatus::SIDEPASS狀態，若前方已無障礙物，設置為正常行駛；反之繼續處于超車過程。

case SidePassStatus::SIDEPASS: {if (passable_obstacle_id.empty()) {sidepass_status->set_status(SidePassStatus::DRIVE);}break; }

停車處理

• 首先檢查障礙物是不是靜止物體(非1中的堵車情況)。若是虛擬的或者動態障礙物，則忽略，由超車模塊處理

if (path_obstacle->obstacle()->IsVirtual() || !path_obstacle->obstacle()->IsStatic()) {continue; }

• 檢查障礙物和無人車位置，若障礙物在無人車后，忽略，由后車模塊處理

const auto& obstacle_sl = path_obstacle->PerceptionSLBoundary(); if (obstacle_sl.end_s() <= adc_sl.start_s()) {continue; }

• 障礙物已經在超車模塊中被標記，迫使無人車超車，那么此時就不需要考慮該障礙物

// check SIDE_PASS decision if (path_obstacle->LateralDecision().has_sidepass()) {continue; }

• 檢查是否必須要停車條件，車道沒有足夠的寬度來允許無人車超車

double left_width = 0.0; double right_width = 0.0; reference_line.GetLaneWidth(obstacle_sl.start_s(), &left_width, &right_width);double left_driving_width = left_width - obstacle_sl.end_l() - // 計算障礙物左側空余距離config_.front_vehicle().nudge_l_buffer(); double right_driving_width = right_width + obstacle_sl.start_l() - // 計算障礙物右側空余距離，+號是因為車道線FLU坐標系左側l是負軸，右側是正軸config_.front_vehicle().nudge_l_buffer(); if ((left_driving_width < adc_width && right_driving_width < adc_width) ||(obstacle_sl.start_l() <= 0.0 && obstacle_sl.end_l() >= 0.0)) {// build stop decisiondouble stop_distance = path_obstacle->MinRadiusStopDistance(vehicle_param);const double stop_s = obstacle_sl.start_s() - stop_distance;auto stop_point = reference_line.GetReferencePoint(stop_s);double stop_heading = reference_line.GetReferencePoint(stop_s).heading();ObjectDecisionType stop;auto stop_decision = stop.mutable_stop();if (obstacle_type == PerceptionObstacle::UNKNOWN_MOVABLE ||obstacle_type == PerceptionObstacle::BICYCLE ||obstacle_type == PerceptionObstacle::VEHICLE) {stop_decision->set_reason_code(StopReasonCode::STOP_REASON_HEAD_VEHICLE);} else {stop_decision->set_reason_code(StopReasonCode::STOP_REASON_OBSTACLE);}stop_decision->set_distance_s(-stop_distance);stop_decision->set_stop_heading(stop_heading);stop_decision->mutable_stop_point()->set_x(stop_point.x());stop_decision->mutable_stop_point()->set_y(stop_point.y());stop_decision->mutable_stop_point()->set_z(0.0);path_decision->AddLongitudinalDecision("front_vehicle", path_obstacle->Id(), stop); }

3.6 禁停區情況處理–KEEP_CLEAR

禁停區分為兩類，第一類是傳統的禁停區，第二類是交叉路口。那么對于禁停區做的處理和對于人行橫道上障礙物構建虛擬墻很相似。具體做法是在參考線上構建一塊禁停區，從縱向的start_s到end_s(這里的start_s和end_s是禁停區start_s和end_s在參考線上的投影點)。禁停區寬度是參考線的道路寬。

具體的處理情況為(禁停區和交叉路口處理一致)：

檢查無人車是否已經駛入禁停區或者交叉路口，是則可直接忽略。

// check const double adc_front_edge_s = reference_line_info->AdcSlBoundary().end_s(); if (adc_front_edge_s - keep_clear_overlap->start_s >config_.keep_clear().min_pass_s_distance()) { // min_pass_s_distance：2.0mreturn false; }

創建新的禁停區障礙物，并且打上標簽為不能停車

// create virtual static obstacle auto* obstacle = frame->CreateStaticObstacle(reference_line_info, virtual_obstacle_id, keep_clear_overlap->start_s,keep_clear_overlap->end_s); if (!obstacle) {return false; } auto* path_obstacle = reference_line_info->AddObstacle(obstacle); if (!path_obstacle) {return false; } path_obstacle->SetReferenceLineStBoundaryType(StBoundary::BoundaryType::KEEP_CLEAR);

這里額外補充一下創建禁停區障礙物流程，主要還是計算禁停區障礙物的標定框，即center和length，width

/// file in apollo/modules/planning/common/frame.cc const Obstacle *Frame::CreateStaticObstacle(ReferenceLineInfo *const reference_line_info,const std::string &obstacle_id,const double obstacle_start_s,const double obstacle_end_s) {const auto &reference_line = reference_line_info->reference_line();// 計算禁停區障礙物start_xy，需要映射到ReferenceLinecommon::SLPoint sl_point;sl_point.set_s(obstacle_start_s);sl_point.set_l(0.0);common::math::Vec2d obstacle_start_xy;if (!reference_line.SLToXY(sl_point, &obstacle_start_xy)) {return nullptr;}// 計算禁停區障礙物end_xy，需要映射到ReferenceLinesl_point.set_s(obstacle_end_s);sl_point.set_l(0.0);common::math::Vec2d obstacle_end_xy;if (!reference_line.SLToXY(sl_point, &obstacle_end_xy)) {return nullptr;}// 計算禁停區障礙物左側寬度和右側寬度，與參考線一致double left_lane_width = 0.0;double right_lane_width = 0.0;if (!reference_line.GetLaneWidth(obstacle_start_s, &left_lane_width, &right_lane_width)) {return nullptr;}// 最終可以得到禁停區障礙物的標定框common::math::Box2d obstacle_box{common::math::LineSegment2d(obstacle_start_xy, obstacle_end_xy),left_lane_width + right_lane_width};// CreateStaticVirtualObstacle函數是將禁停區障礙物封裝成PathObstacle放入PathDecision中return CreateStaticVirtualObstacle(obstacle_id, obstacle_box); }

3.7 尋找停車點狀態–PULL_OVER

尋找停車點本質上是尋找停車的位置，如果當前已經有停車位置(也就是上個狀態就是PULL_OVER)，那么就只需要更新狀態信息即可；若不存在，那么就需要計算停車點的位置，然后構建停車區障礙物(同人行橫道虛擬墻障礙物和禁停區障礙物)，然后創建障礙物的PULL_OVER標簽。

Status PullOver::ApplyRule(Frame* const frame, ReferenceLineInfo* const reference_line_info) {frame_ = frame;reference_line_info_ = reference_line_info;if (!IsPullOver()) {return Status::OK();}// 檢查上時刻是不是PULL_OVER，如果是PULL_OVER，那么說明已經有停車點stop_pointif (CheckPullOverComplete()) {return Status::OK();}common::PointENU stop_point;if (GetPullOverStop(&stop_point) != 0) { // 獲取停車位置失敗，無人車將在距離終點的車道上進行停車BuildInLaneStop(stop_point);ADEBUG << "Could not find a safe pull over point. STOP in-lane";} else {BuildPullOverStop(stop_point); // 獲取停車位置成功，無人車將在距離終點的車道上靠邊進行停車}return Status::OK(); }

代碼也是比較容易理解，這里我們挑幾個要點象征性的解釋一下

獲取停車點–GetPullOverStop函數

• 如果狀態信息中已經存在了停車點位置并且有效，那么就可以直接拿來用

if (pull_over_status.has_start_point() && pull_over_status.has_stop_point()) {// reuse existing/previously-set stop pointstop_point->set_x(pull_over_status.stop_point().x());stop_point->set_y(pull_over_status.stop_point().y()); }

• 如果不存在，那么就需要尋找停車位置–FindPullOverStop函數

停車位置需要在目的地點前PARKING_SPOT_LONGITUDINAL_BUFFER(默認1m)，距離路測buffer_to_boundary(默認0.5m)處停車。但是停車條件必須滿足在路的最邊上，也就意味著這條lane的右側lane不能是機動車道(CITY_DRIVING)。Apollo采用的方法為采樣檢測，從車頭到終點位置，每隔kDistanceUnit(默認5m)進行一次停車條件檢查，滿足則直接停車。

int PullOver::FindPullOverStop(PointENU* stop_point) {const auto& reference_line = reference_line_info_->reference_line();const double adc_front_edge_s = reference_line_info_->AdcSlBoundary().end_s();double check_length = 0.0;double total_check_length = 0.0;double check_s = adc_front_edge_s; // check_s為當前車輛車頭的累計距離constexpr double kDistanceUnit = 5.0;while (check_s < reference_line.Length() && // 在當前車道上，向前采樣方式進行停車位置檢索，前向檢索距離不超過max_check_distance(默認60m)total_check_length < config_.pull_over().max_check_distance()) {check_s += kDistanceUnit;total_check_length += kDistanceUnit;...} }

檢查該點(check_s)位置右車道，如果右車道還是機動車道，那么改點不能停車，至少需要變道，繼續前向搜索。

// check rightmost driving lane: // NONE/CITY_DRIVING/BIKING/SIDEWALK/PARKING bool rightmost_driving_lane = true; for (auto& neighbor_lane_id : lane->lane().right_neighbor_forward_lane_id()) { // const auto neighbor_lane = HDMapUtil::BaseMapPtr()->GetLaneById(neighbor_lane_id);...const auto& lane_type = neighbor_lane->lane().type();if (lane_type == hdmap::Lane::CITY_DRIVING) { // rightmost_driving_lane = false;break;} } if (!rightmost_driving_lane) {check_length = 0.0;continue; }

如果右側車道不是機動車道，那么路測允許停車。停車位置需要在目的地點前PARKING_SPOT_LONGITUDINAL_BUFFER(默認1m)，距離路測buffer_to_boundary(默認0.5m)處停車?？v向與停車點的距離以車頭為基準；側方與停車點距離取{車頭距離車道邊線，車尾距離車道邊線，車身中心距離車道邊線}距離最小值為基準。

// all the lane checks have passed check_length += kDistanceUnit; if (check_length >= config_.pull_over().plan_distance()) {PointENU point;// check corresponding parking_spotif (FindPullOverStop(check_s, &point) != 0) {// parking_spot not valid/availablecheck_length = 0.0;continue;}stop_point->set_x(point.x());stop_point->set_y(point.y());return 0; }

在1中如果找到了停車位置，那么就直接對停車位置構建一個PathObstacle，然后設置他的標簽Stop即可。創建停車區障礙物的方式跟上述一樣，這里也不重復講解。該功能由函數BuildPullOverStop完成。

在1中如果找不到停車位置，那么就去尋找歷史狀態中的數據，找到了就根據2中停車，找不到強行在車道上停車。該功能由函數BuildInLaneStop完成

• 先去尋找歷史數據的inlane_dest_point，也就是歷史數據是否允許在車道上停車
• 如果沒找到，那么去尋找停車位置，如果找到了就可以進行2中的停車
• 如果仍然沒找到停車位置，去尋找類中使用過的inlane_adc_potiion_stop_point_，如果找到了可以進行2中的停車
• 如果依舊沒找到那么只能強行在距離終點plan_distance處，在車道上強行停車，并更新inlane_adc_potiion_stop_point_，供下次使用。

3.8 參考線結束情況處理–REFERENCE_LINE_END

當參考線結束，一般就需要重新路由查詢，所以無人車需要停車，這種情況下如果程序正常，一般是前方沒有路了，需要重新查詢一點到目的地新的路由，具體的代碼也是跟人行橫道上的不可忽略障礙物一樣，在參考線終點前構建一個停止墻障礙物，并設置齊標簽為停車Stop。

Status ReferenceLineEnd::ApplyRule(Frame* frame, ReferenceLineInfo* const reference_line_info) {const auto& reference_line = reference_line_info->reference_line();// 檢查參考線剩余的長度，足夠則可忽略這個情況，min_reference_line_remain_length：50mdouble remain_s = reference_line.Length() - reference_line_info->AdcSlBoundary().end_s();if (remain_s > config_.reference_line_end().min_reference_line_remain_length()) {return Status::OK();}// create avirtual stop wall at the end of reference line to stop the adcstd::string virtual_obstacle_id = REF_LINE_END_VO_ID_PREFIX + reference_line_info->Lanes().Id();double obstacle_start_s = reference_line.Length() - 2 * FLAGS_virtual_stop_wall_length; // 在參考線終點前，創建停止墻障礙物auto* obstacle = frame->CreateStopObstacle(reference_line_info, virtual_obstacle_id, obstacle_start_s);if (!obstacle) {return Status(common::PLANNING_ERROR, "Failed to create reference line end obstacle");}PathObstacle* stop_wall = reference_line_info->AddObstacle(obstacle);if (!stop_wall) {return Status(common::PLANNING_ERROR, "Failed to create path obstacle for reference line end obstacle");}// build stop decision，設置障礙物停止標簽const double stop_line_s = obstacle_start_s - config_.reference_line_end().stop_distance();auto stop_point = reference_line.GetReferencePoint(stop_line_s);ObjectDecisionType stop;auto stop_decision = stop.mutable_stop();stop_decision->set_reason_code(StopReasonCode::STOP_REASON_DESTINATION);stop_decision->set_distance_s(-config_.reference_line_end().stop_distance());stop_decision->set_stop_heading(stop_point.heading());stop_decision->mutable_stop_point()->set_x(stop_point.x());stop_decision->mutable_stop_point()->set_y(stop_point.y());stop_decision->mutable_stop_point()->set_z(0.0);auto* path_decision = reference_line_info->path_decision();path_decision->AddLongitudinalDecision(TrafficRuleConfig::RuleId_Name(config_.rule_id()), stop_wall->Id(), stop);return Status::OK(); }

3.9 重新路由查詢情況處理–REROUTING

根據具體路況進行處理，根據代碼可以分為以下情況：

• 若當前參考線為直行，非轉彎。那么暫時就不需要重新路由，等待新的路由
• 若當前車輛不在參考線上，不需要重新路由，等待新的路由
• 若當前車輛可以退出了，不需要重新路由，等待新的路由
• 若當前通道Passage終點不在參考線上，不需要重新路由，等待新的路由
• 若參考線的終點距離無人車過遠，不需要重新路由，等待新的路由
• 若上時刻進行過路由查詢，距離當前時間過短，不需要重新路由，等待新的路由
• 其他情況，手動發起路由查詢需求

其實這個模塊我還是有點不是特別敢肯定，只能做保留的解釋。首先代碼Frame::Rerouting做的工作僅僅重新路由，得到當前位置到目的地的一個路況，這個過程并沒有產生新的參考線，因為參考線的產生依賴于ReferenceLineProvider線程。所以說對于第二點，車輛不在參考線上，即使重新路由了，但是沒有生成矯正的新參考線，所以重新路由也是無用功，反之還不如等待ReferenceLineProvider去申請重新路由并生成對應的參考線。所以說2,3,4等情況的重點在于缺乏參考線，而不在于位置偏離了。

3.10 信號燈情況處理–SIGNAL_LIGHT

信號燈處理相對來說比較簡單，無非是有紅燈就停車；有黃燈速度小，就停車；有綠燈，或者黃燈速度大就直接駛過。具體的處理步驟分為：

檢查當前路況下是否有信號燈區域–由函數FindValidSignalLight完成

signal_lights_from_path_.clear(); for (const hdmap::PathOverlap& signal_light : signal_lights) {if (signal_light.start_s + config_.signal_light().min_pass_s_distance() >reference_line_info->AdcSlBoundary().end_s()) {signal_lights_from_path_.push_back(signal_light);} }

獲取TrafficLight Perception發布的信號等信息–由函數ReadSignals完成

const TrafficLightDetection& detection =AdapterManager::GetTrafficLightDetection()->GetLatestObserved(); for (int j = 0; j < detection.traffic_light_size(); j++) {const TrafficLight& signal = detection.traffic_light(j);detected_signals_[signal.id()] = &signal; }

決策–由函數MakeDecisions完成

for (auto& signal_light : signal_lights_from_path_) {// 1. 如果信號燈是紅燈，并且加速度不是很大// 2. 如果信號燈是未知，并且加速度不是很大// 3. 如果信號燈是黃燈，并且加速度不是很大// 以上三種情況，無人車停車，停車標簽與前面一致if ((signal.color() == TrafficLight::RED &&stop_deceleration < config_.signal_light().max_stop_deceleration()) ||(signal.color() == TrafficLight::UNKNOWN &&stop_deceleration < config_.signal_light().max_stop_deceleration()) ||(signal.color() == TrafficLight::YELLOW &&stop_deceleration < config_.signal_light().max_stop_deacceleration_yellow_light())) {if (BuildStopDecision(frame, reference_line_info, &signal_light)) {has_stop = true;signal_debug->set_is_stop_wall_created(true);}}// 設置交叉口區域，以及是否有權力通行，停車表明無法通行。if (has_stop) {reference_line_info->SetJunctionRightOfWay(signal_light.start_s,false); // not protected} else {reference_line_info->SetJunctionRightOfWay(signal_light.start_s, true);// is protected}}

3.11 停車情況處理–STOP_SIGN

停車情況相對來說比較復雜，根據代碼將停車分為：尋找下一個最近的停車信號，決策處理。尋找下一個停車點比較簡單，由函數FindNextStopSign完成，這里直接跳過。接下來分析決策部分，可以分為以下幾步：

獲取等待車輛列表–由函數GetWatchVehicles完成。

這個過程其實就是獲取無人車前方的等待車輛，存儲形式為：

typedef std::unordered_map<std::string, std::vector<std::string>> StopSignLaneVehicles;

map中第一個string是車道id，第二個vector<string>是這個車道上在無人車前方的等待車輛id。整體的查詢是直接在PlanningStatus.stop_sign(停車狀態)中獲取，第一次為空，后續不為空。

int StopSign::GetWatchVehicles(const StopSignInfo& stop_sign_info,StopSignLaneVehicles* watch_vehicles) {watch_vehicles->clear();StopSignStatus stop_sign_status = GetPlanningStatus()->stop_sign();// 遍歷所有的車道for (int i = 0; i < stop_sign_status.lane_watch_vehicles_size(); ++i) {auto lane_watch_vehicles = stop_sign_status.lane_watch_vehicles(i);std::string associated_lane_id = lane_watch_vehicles.lane_id();std::string s;// 獲取每個車道的等候車輛for (int j = 0; j < lane_watch_vehicles.watch_vehicles_size(); ++j) {std::string vehicle = lane_watch_vehicles.watch_vehicles(j);s = s.empty() ? vehicle : s + "," + vehicle;(*watch_vehicles)[associated_lane_id].push_back(vehicle);}}return 0; }

檢查與更新停車狀態PlanningStatus.stop_sign–由函數ProcessStopStatus完成

停車過程可以分為5個階段：正常行駛DRIVE–開始停車STOP–等待緩沖狀態WAIT–緩慢前進CREEP–徹底停車DONE。

• 更新停車狀態。如果無人車距離最近一個停車區域過遠，那么狀態為正常行駛

// adjust status double adc_front_edge_s = reference_line_info->AdcSlBoundary().end_s(); double stop_line_start_s = next_stop_sign_overlap_.start_s; if (stop_line_start_s - adc_front_edge_s > // max_valid_stop_distance: 3.5mconfig_.stop_sign().max_valid_stop_distance()) {stop_status_ = StopSignStatus::DRIVE; }

• 如果停車狀態是正常行駛DRIVE。

這種情況下，如果車輛速度很大，或者與停車區域距離過遠，那么繼續設置為行駛DRIVE；反之就進入停車狀態STOP。狀態檢查由CheckADCkStop完成。

• 如果停車狀態是開始停車STOP。

這種情況下，如果從開始停車到當前經歷的等待時間沒有超過閾值stop_duration(默認1s)，繼續保持STOP狀態。反之，如果前方等待車輛不為空，那么就進入下一階段WAIT緩沖階段；如果前方車輛為空，那么可以直接進入到緩慢前進CREEP狀態或者停車完畢狀態。

• 如果停車狀態是等待緩沖WAIT

這種情況下，如果等待時間沒有超過一個閾值wait_timeout(默認8s)或者前方存在等待車輛，繼續保持等待狀態。反之可以進入到緩慢前進或者停車完畢狀態

• 如果停車狀態是緩慢前進CREEP

這種情況下，只需要檢查無人車車頭和停車區域的距離，如果大于某個值那么說明可以繼續緩慢前進，保持狀態不變，反之就可以完全停車了。

更新前方等待車輛

a.當前狀態是DRIVE，那么需要將障礙物都加入到前方等待車輛列表中，因為這些障礙物到時候都會排在無人車前方等待。

if (stop_status_ == StopSignStatus::DRIVE) {for (const auto* path_obstacle : path_decision->path_obstacles().Items()) {// add to watch_vehicles if adc is still proceeding to stop signAddWatchVehicle(*path_obstacle, &watch_vehicles);} }

b.如果無人車當前狀態是等待或者停車，刪除部分排隊等待車輛–RemoveWatchVehicle函數完成

這種情況下，如果障礙物已經駛過停車區域，那么對其刪除；否則繼續保留。

double stop_line_end_s = over_lap_info->lane_overlap_info().end_s(); double obstacle_end_s = obstacle_s + perception_obstacle.length() / 2; double distance_pass_stop_line = obstacle_end_s - stop_line_end_s; // 如果障礙物已經駛過停車區一定距離，可以將障礙物從等待車輛中刪除。 if (distance_pass_stop_line > config_.stop_sign().min_pass_s_distance() && !is_path_cross) {erase = true; } else {// passes associated lane (in junction)if (!is_path_cross) {erase = true;} } // check if obstacle stops if (erase) {for (StopSignLaneVehicles::iterator it = watch_vehicles->begin();it != watch_vehicles->end(); ++it) {std::vector<std::string>& vehicles = it->second;vehicles.erase(std::remove(vehicles.begin(), vehicles.end(), obstacle_id), vehicles.end());} }

• 對剩下來的障礙物重新組成一個新的等待隊列–ClearWatchVehicle函數完成

for (StopSignLaneVehicles::iterator it = watch_vehicles->begin();it != watch_vehicles->end();/*no increment*/) {std::vector<std::string>& vehicle_ids = it->second;// clean obstacles not in current perceptionfor (auto obstacle_it = vehicle_ids.begin(); obstacle_it != vehicle_ids.end();) {// 如果新的隊列中已經不存在該障礙物了，那么直接將障礙物從這條車道中刪除if (obstacle_ids.count(*obstacle_it) == 0) { obstacle_it = vehicle_ids.erase(obstacle_it);} else {++obstacle_it;}}if (vehicle_ids.empty()) { // 如果這整條車道上都不存在等待車輛了，直接把這條車道刪除watch_vehicles->erase(it++);} else {++it;} }

• 更新車輛狀態PlanningStatus.stop_sign

這部分由函數UpdateWatchVehicles完成，主要是將3中得到的新的等待車輛隊列更新至stop_sign。

int StopSign::UpdateWatchVehicles(StopSignLaneVehicles* watch_vehicles) {auto* stop_sign_status = GetPlanningStatus()->mutable_stop_sign();stop_sign_status->clear_lane_watch_vehicles();for (auto it = watch_vehicles->begin(); it != watch_vehicles->end(); ++it) {auto* lane_watch_vehicles = stop_sign_status->add_lane_watch_vehicles();lane_watch_vehicles->set_lane_id(it->first);std::string s;for (size_t i = 0; i < it->second.size(); ++i) {std::string vehicle = it->second[i];s = s.empty() ? vehicle : s + "," + vehicle;lane_watch_vehicles->add_watch_vehicles(vehicle);}}return 0; }

c. 如果當前車輛狀態是緩慢前進狀態CREEP

這種情況下，可以直接創建停車的標簽。

---

最后總結一下，障礙物和路況對無人車的決策影響分為兩類，一類是縱向影響LongitudinalDecision，一類是側向影響LateralDecision。

縱向影響：

const std::unordered_map<ObjectDecisionType::ObjectTagCase, int, athObstacle::ObjectTagCaseHash>PathObstacle::s_longitudinal_decision_safety_sorter_ = {{ObjectDecisionType::kIgnore, 0}, // 忽略，優先級0{ObjectDecisionType::kOvertake, 100}, // 超車，優先級100{ObjectDecisionType::kFollow, 300}, // 跟隨，優先級300{ObjectDecisionType::kYield, 400}, // 減速，優先級400{ObjectDecisionType::kStop, 500}}; // 停車，優先級500

側向影響：

const std::unordered_map<ObjectDecisionType::ObjectTagCase, int, PathObstacle::ObjectTagCaseHash>PathObstacle::s_lateral_decision_safety_sorter_ = {{ObjectDecisionType::kIgnore, 0}, // 忽略，優先級0{ObjectDecisionType::kNudge, 100}, // 微調，優先級100{ObjectDecisionType::kSidepass, 200}}; // 繞行，優先級200

當有一個障礙物在11中路況下多次出發無人車決策時該怎么辦？

縱向決策合并，lhs為第一次決策，rhs為第二次決策，如何合并兩次決策

ObjectDecisionType PathObstacle::MergeLongitudinalDecision(const ObjectDecisionType& lhs, const ObjectDecisionType& rhs) {if (lhs.object_tag_case() == ObjectDecisionType::OBJECT_TAG_NOT_SET) {return rhs;}if (rhs.object_tag_case() == ObjectDecisionType::OBJECT_TAG_NOT_SET) {return lhs;}const auto lhs_val =FindOrDie(s_longitudinal_decision_safety_sorter_, lhs.object_tag_case());const auto rhs_val =FindOrDie(s_longitudinal_decision_safety_sorter_, rhs.object_tag_case());if (lhs_val < rhs_val) { // 優先選取優先級大的決策return rhs;} else if (lhs_val > rhs_val) {return lhs;} else {if (lhs.has_ignore()) {return rhs;} else if (lhs.has_stop()) { // 如果優先級相同，都是停車，選取停車距離小的決策，防止安全事故return lhs.stop().distance_s() < rhs.stop().distance_s() ? lhs : rhs;} else if (lhs.has_yield()) { // 如果優先級相同，都是減速，選取減速距離小的決策，防止安全事故return lhs.yield().distance_s() < rhs.yield().distance_s() ? lhs : rhs;} else if (lhs.has_follow()) { // 如果優先級相同，都是跟隨，選取跟隨距離小的決策，防止安全事故return lhs.follow().distance_s() < rhs.follow().distance_s() ? lhs : rhs;} else if (lhs.has_overtake()) { // 如果優先級相同，都是超車，選取超車距離大的決策，防止安全事故return lhs.overtake().distance_s() > rhs.overtake().distance_s() ? lhs : rhs;} else {DCHECK(false) << "Unknown decision";}}return lhs; // stop compiler complaining }

側向合并，lhs為第一次決策，rhs為第二次決策，如何合并兩次決策

ObjectDecisionType PathObstacle::MergeLateralDecision(const ObjectDecisionType& lhs, const ObjectDecisionType& rhs) {if (lhs.object_tag_case() == ObjectDecisionType::OBJECT_TAG_NOT_SET) {return rhs;}if (rhs.object_tag_case() == ObjectDecisionType::OBJECT_TAG_NOT_SET) {return lhs;}const auto lhs_val =FindOrDie(s_lateral_decision_safety_sorter_, lhs.object_tag_case());const auto rhs_val =FindOrDie(s_lateral_decision_safety_sorter_, rhs.object_tag_case());if (lhs_val < rhs_val) { // 優先選取優先級大的決策 return rhs;} else if (lhs_val > rhs_val) {return lhs;} else {if (lhs.has_ignore() || lhs.has_sidepass()) {return rhs;} else if (lhs.has_nudge()) { // 如果優先級相同，都是微調，選取側向微調大的決策return std::fabs(lhs.nudge().distance_l()) >std::fabs(rhs.nudge().distance_l())? lhs: rhs;}}return lhs; }

如有侵權，請聯系本人刪除！

參考資料：
https://github.com/YannZyl/Apollo-Note/blob/master/docs/planning/frame.md

總結

以上是生活随笔為你收集整理的障碍物参考线交通规则融合器：Frame类的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。