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

在Frame類中，主要的工作還是對(duì)障礙物預(yù)測(cè)軌跡(由Predition模塊得到的未來(lái)5s內(nèi)障礙物運(yùn)動(dòng)軌跡)、無(wú)人車(chē)參考線(ReferenceLineProvider類提供)以及當(dāng)前路況(停車(chē)標(biāo)志、人行橫道、減速帶等)信息進(jìn)行融合。

實(shí)際情況下，能影響無(wú)人車(chē)運(yùn)動(dòng)的不一定只有障礙物，同時(shí)還有各個(gè)路況，舉個(gè)例子：

a. 障礙物影響

情況1：無(wú)人車(chē)車(chē)后的障礙物，對(duì)無(wú)人車(chē)沒(méi)太大影響，可以忽略

情況2：無(wú)人車(chē)前面存在障礙物，無(wú)人車(chē)就需要停車(chē)或者超車(chē)

b. 路況影響

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

情況4：前方存在人行橫道，若有人，那么需要停車(chē)；若沒(méi)人，那么無(wú)人車(chē)可以駛過(guò)

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

障礙物信息的獲取策略

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

依據(jù)交通規(guī)則對(duì)障礙物設(shè)定標(biāo)簽(原始感知障礙物&&路況障礙物)

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

在這個(gè)步驟中，主要的工作是獲取Prediction模塊發(fā)布的障礙物預(yù)測(cè)軌跡數(shù)據(jù)，并且進(jìn)行后處理工作。首先回顧一下Prediction模塊發(fā)布的數(shù)據(jù)格式：

/// file in apollo/modules/prediction/proto/prediction_obstacle.proto message Trajectory {optional double probability = 1; // probability of this trajectory，障礙物該軌跡運(yùn)動(dòng)方案的概率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()形式獲取所有障礙物的軌跡信息，對(duì)于每個(gè)障礙物prediction_obstacle，可以使用prediction_obstacle.trajectory()獲取他所有可能運(yùn)動(dòng)方案/軌跡。

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

/// 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_) { // 滯后預(yù)測(cè)策略，獲取障礙物軌跡信息lag_predictor_->GetLaggedPrediction(&prediction_);} else { // 不采用滯后預(yù)測(cè)策略，直接取最近一次Prediction模塊發(fā)布的障礙物信息prediction_.CopyFrom(AdapterManager::GetPrediction()->GetLatestObserved());}}... }

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

最近一次發(fā)布的數(shù)據(jù)直接加入PredictionObstacles容器中

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

從上面的代碼可以看到，滯后預(yù)測(cè)對(duì)于最近一次發(fā)布的數(shù)據(jù)處理比較簡(jiǎn)單，障礙物信息有效只需要滿足兩個(gè)條件：

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

Prediction發(fā)布的歷史信息后處理

/// file in apollo/modules/planning/common/lag_prediction.cc void LagPrediction::GetLaggedPrediction(PredictionObstacles* obstacles) const {// Step A 最近一次發(fā)布的障礙物軌跡預(yù)測(cè)信息處理...// Step B 過(guò)往發(fā)布的歷史障礙物軌跡預(yù)測(cè)信息處理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) { // 對(duì)于每一次發(fā)布的信息進(jìn)行處理for (const auto& obstacle : (*it)->prediction_obstacle()) { // 獲取該次發(fā)布的歷史數(shù)據(jù)中，每個(gè)障礙物的運(yùn)動(dòng)軌跡信息const auto& perception = obstacle.perception_obstacle();auto id = perception.id();if (perception.confidence() < FLAGS_perception_confidence_threshold && // 障礙物置信度必須大于0.5，獲取必須是車(chē)輛VEHICLE類，否則不作處理perception.type() != PerceptionObstacle::VEHICLE) {continue;}if (protected_obstacles.count(id) > 0) { // 如果障礙物在最近一次發(fā)布的信息中出現(xiàn)了，那就忽略，因?yàn)橹豢紤]最新的障礙物信息continue; // don't need to count the already added protected obstacle}auto& info = obstacle_lag_info[id]; ++info.count; // 記錄障礙物在所有歷史信息中出現(xiàn)的次數(shù)if ((*it)->header().timestamp_sec() > info.last_observed_time) { // 保存最近一次出現(xiàn)的信息，因?yàn)橹豢紤]最新的障礙物信息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_) { // 歷史信息中如果障礙物出現(xiàn)次數(shù)小于min_appear_num_/3次，次數(shù)太少，可忽略。continue;}if (apply_lag && iter.second.last_observed_seq > max_disappear_num_) { // 歷史信息中如果障礙物最近一次發(fā)布距離現(xiàn)在過(guò)遠(yuǎn)，可忽略。continue;}AddObstacleToPrediction(timestamp - iter.second.last_observed_time,*(iter.second.obstacle_ptr), obstacles);} }

所以最后做一個(gè)總結(jié)，對(duì)于歷史發(fā)布數(shù)據(jù)，如何判斷這些障礙物軌跡信息是否有效。兩個(gè)步驟：

步驟1：記錄歷史發(fā)布數(shù)據(jù)中每個(gè)障礙物出現(xiàn)的次數(shù)(在最近依次發(fā)布中出現(xiàn)的障礙物忽略，因?yàn)椴皇亲钚碌臄?shù)據(jù)了)，必須滿足兩個(gè)條件：

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

步驟2：對(duì)于步驟1中得到的障礙物信息，進(jìn)行篩選，信息有效需要滿足兩個(gè)條件：

• 信息隊(duì)列中歷史數(shù)據(jù)大于3(min_appear_num_)，并且每個(gè)障礙物出現(xiàn)次數(shù)大于3(min_appear_num_)
• 信息隊(duì)列中歷史數(shù)據(jù)大于3(min_appear_num_)，并且障礙物信息上一次發(fā)布距離最近一次發(fā)布不大于5(max_disappear_num_)，需要保證數(shù)據(jù)的最近有效性。

二. 無(wú)人車(chē)與障礙物相對(duì)位置的設(shè)置–ReferenceLineInfo類初始化

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

/// file in apollo/modules/planning/common/frame.cc Status Frame::Init() {// Step A prediction，障礙物預(yù)測(cè)軌跡信息獲取，采用滯后預(yù)測(cè)策略...// Step B.1 檢查當(dāng)前時(shí)刻(relative_time=0.0s)，無(wú)人車(chē)位置和障礙物位置是否重疊(相撞)，如果是，可以直接退出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 如果當(dāng)前時(shí)刻不沖突，檢查未來(lái)時(shí)刻無(wú)人車(chē)可以在參考線上前進(jìn)的位置，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類的初始化工作，其實(shí)比較簡(jiǎn)單，主要有兩個(gè)過(guò)程：

• 根據(jù)無(wú)人車(chē)規(guī)劃路徑ReferenceLine實(shí)例化ReferenceLineInfo類，數(shù)量與ReferenceLine一致
• 根據(jù)障礙物軌跡初始化ReferenceLineInfo::path_decision_

實(shí)例化ReferenceLineInfo類

/// file in apollo/modules/planning/common/frame.cc bool Frame::CreateReferenceLineInfo() {// Step A 從ReferenceLineProvider中獲取無(wú)人車(chē)的短期內(nèi)規(guī)劃路徑ReferenceLine，并進(jìn)行收縮操作std::list<ReferenceLine> reference_lines;std::list<hdmap::RouteSegments> segments;if (!reference_line_provider_->GetReferenceLines(&reference_lines, &segments)) { return false;}// 對(duì)于每條ReferenceLine實(shí)例化生成一個(gè)對(duì)應(yīng)的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) {}

這個(gè)規(guī)程比較簡(jiǎn)單，就是從ReferenceLineProvider中提取無(wú)人車(chē)短期內(nèi)的規(guī)劃路徑，如果不了解，可以查看(組件)指引線提供器: ReferenceLineProvider。然后由一條ReferenceLine&&segments、車(chē)輛狀態(tài)和規(guī)劃起始點(diǎn)生成對(duì)應(yīng)的ReferenceLineInfo

初始化ReferenceLineInfo類

/// file in apollo/modules/planning/common/frame.cc bool Frame::CreateReferenceLineInfo() {// Step A 從ReferenceLineProvider中獲取無(wú)人車(chē)的短期內(nèi)規(guī)劃路徑ReferenceLine，并進(jìn)行收縮操作...// 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)函數(shù)中，主要做的工作也是比較簡(jiǎn)單，這里做一下總結(jié)：

• 檢查無(wú)人車(chē)是否在參考線上

需要滿足無(wú)人車(chē)對(duì)應(yīng)的邊界框start_s和end_s在參考線[0, total_length]區(qū)間內(nèi)

• 檢查無(wú)人車(chē)是否離參考線過(guò)遠(yuǎn)

需要滿足無(wú)人車(chē)start_l和end_l在[-kOutOfReferenceLineL, kOutOfReferenceLineL]區(qū)間內(nèi)，其中kOutOfReferenceLineL取10

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

除了將障礙物信息加入到類中，還有一個(gè)重要的工作就是確定某個(gè)時(shí)間點(diǎn)無(wú)人車(chē)能前進(jìn)到的位置，如下圖所示。

可以看到這個(gè)過(guò)程其實(shí)就是根據(jù)障礙物的軌跡(某個(gè)相對(duì)時(shí)間點(diǎn)，障礙物運(yùn)動(dòng)到哪個(gè)坐標(biāo)位置)，并結(jié)合無(wú)人車(chē)查詢得到的理想路徑，得到某個(gè)時(shí)間點(diǎn)low_t和high_t無(wú)人車(chē)行駛距離的下界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));...// 計(jì)算障礙物框的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);// 計(jì)算障礙物是否對(duì)無(wú)人車(chē)行駛有影響：無(wú)光障礙物滿足以下條件：// 1. 障礙物在ReferenceLine以外，忽略// 2. 車(chē)輛和障礙物都在車(chē)道上，但是障礙物在無(wú)人車(chē)后面，忽略if (IsUnrelaventObstacle(path_obstacle)) {// 忽略障礙物} else {// 構(gòu)建障礙物在參考線上的邊界框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函數(shù)，我們簡(jiǎn)單的進(jìn)行代碼段分析：

Step 1. 首先還是對(duì)障礙物軌跡點(diǎn)兩兩選擇，每?jī)蓚€(gè)點(diǎn)可以構(gòu)建上圖中的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總長(zhǎng)度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);... // 計(jì)算object_boundary，由object_moving_box旋轉(zhuǎn)一個(gè)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. 判斷障礙物和車(chē)輛水平Lateral距離，如果障礙物在參考線兩側(cè)，那么障礙物可以忽略；如果障礙物在參考線后面，也可忽略

// 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()) > // 障礙物在參考線左側(cè)，那么無(wú)人車(chē)可以直接通過(guò)障礙物，可忽略障礙物skip_l_distance ||std::fmax(object_boundary.start_l(), object_boundary.end_l()) < // 障礙物在參考線右側(cè)，那么無(wú)人車(chē)可以直接通過(guò)障礙物，可忽略障礙物-skip_l_distance) {continue;}if (object_boundary.end_s() < 0) { // 障礙物在參考線后面，可忽略障礙物continue;}

Step 3. 計(jì)算low_t和high_t時(shí)刻的行駛上下界邊界框

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) { // 采用漸進(jìn)逼近的方法，逐漸計(jì)算邊界框的下界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) { // 采用漸進(jìn)逼近的方法，逐漸計(jì)算邊界框的上界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( // 計(jì)算low_t時(shí)刻的上下界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( // 計(jì)算high_t時(shí)刻的上下界std::make_pair(STPoint{low_s - adc_start_s, high_t},STPoint{high_s - adc_start_s, high_t}));}}

Step 4. 計(jì)算完所有障礙物軌跡段的上下界框以后，根據(jù)時(shí)間t進(jìn)行排序

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;}

總結(jié)一下**無(wú)人車(chē)參考線ReferenceLineInof初始化(加入障礙物軌跡信息)**這步的功能，給定了無(wú)人車(chē)的規(guī)劃軌跡ReferenceLine和障礙物的預(yù)測(cè)軌跡PredictionObstacles，這個(gè)過(guò)程其實(shí)就是計(jì)算障礙物在無(wú)人車(chē)規(guī)劃軌跡上的重疊部分的位置s，以及駛到這個(gè)重疊部分的時(shí)間點(diǎn)t，為第三部分每個(gè)障礙物在每個(gè)車(chē)輛上的初步?jīng)Q策進(jìn)行規(guī)劃。

三. 依據(jù)交通規(guī)則對(duì)障礙物設(shè)定標(biāo)簽

二中已經(jīng)給出了每個(gè)障礙物在所有的參考線上的重疊部分的位置與時(shí)間點(diǎn)，這些重疊部分就是無(wú)人車(chē)需要考慮的規(guī)劃矯正問(wèn)題，防止交通事故。因?yàn)檎系K物運(yùn)動(dòng)可能跨越多個(gè)ReferenceLine，所以這里需要對(duì)于每個(gè)障礙物進(jìn)行標(biāo)定，是否可忽略，是否會(huì)超車(chē)等。交通規(guī)則判定情況一共11種，在文件modules/planning/conf/traffic_rule_config.pb.txt中定義，這里我們將一一列舉。

后車(chē)情況處理–BACKSIDE_VEHICLE

變道情況處理–CHANGE_LANE

人行橫道情況處理–CROSSWALK

目的地情況處理–DESTINATION

前車(chē)情況處理–FRONT_VEHICLE

禁停區(qū)情況處理–KEEP_CLEAR

尋找停車(chē)點(diǎn)狀態(tài)–PULL_OVER

參考線結(jié)束情況處理–REFERENCE_LINE_END

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

信號(hào)燈情況處理–SIGNAL_LIGHT

停車(chē)情況處理–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()) { // 對(duì)于每條參考線進(jìn)行障礙物的決策。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 后車(chē)情況處理–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; // 從這里可以看到，對(duì)于后車(chē)的處理主要是忽略ignore.mutable_ignore();const double adc_length_s = adc_sl_boundary.end_s() - adc_sl_boundary.start_s(); // 計(jì)算"車(chē)長(zhǎng)""for (const auto* path_obstacle : path_decision->path_obstacles().Items()) { // 對(duì)于每個(gè)與參考線有重疊的障礙物進(jìn)行規(guī)則設(shè)置... } }

從上述代碼可以看到后車(chē)情況處理僅僅針對(duì)當(dāng)前無(wú)人車(chē)所在的參考線，那些臨近的參考線不做考慮。Apollo主要的處理方式其實(shí)是忽略后車(chē)，可以分一下情況：

• 前車(chē)在這里不做考慮，由前車(chē)情況處理FRONT_VEHICLE來(lái)完成

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

• 參考線上沒(méi)有障礙物運(yùn)動(dòng)軌跡，直接忽略

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; }

• 忽略從無(wú)人車(chē)后面來(lái)的車(chē)輛

// 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; }

從代碼中可以看到，通過(guò)計(jì)算min_s，也就是障礙物軌跡距離無(wú)人車(chē)最近的距離，小于半車(chē)長(zhǎng)度，說(shuō)明車(chē)輛在無(wú)人車(chē)后面，可暫時(shí)忽略。

• 忽略后面不會(huì)超車(chē)的車(chē)輛

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;} }

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

3.2 變道情況處理–CHANGE_LANE

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

/// 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();}// 計(jì)算警示障礙物&超車(chē)障礙物guard_obstacles_.clear();overtake_obstacles_.clear();if (!FilterObstacles(reference_line_info)) {return Status(common::PLANNING_ERROR, "Failed to filter obstacles");}// 創(chuàng)建警示障礙物類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();}}}// 設(shè)置超車(chē)標(biāo)志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(); }

超車(chē)&警示障礙物計(jì)算

• 沒(méi)有軌跡的障礙物忽略

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

• 無(wú)人車(chē)前方的車(chē)輛忽略，對(duì)變道沒(méi)有影響

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

• 跟車(chē)在一定距離(10m)內(nèi)，將其標(biāo)記為超車(chē)障礙物

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); }

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

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

• 障礙物最后一規(guī)劃點(diǎn)在參考線上，但是超過(guò)了無(wú)人車(chē)一定距離，對(duì)變道無(wú)影響

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; }

創(chuàng)建警示障礙物類

創(chuàng)建警示障礙物類本質(zhì)其實(shí)就是預(yù)測(cè)障礙物未來(lái)一段時(shí)間內(nèi)的運(yùn)動(dòng)軌跡，代碼在ChangeLane::CreateGuardObstacle中很明顯的給出了障礙物軌跡的預(yù)測(cè)方法。預(yù)測(cè)的軌跡是在原始軌跡上進(jìn)行拼接，即在最后一個(gè)軌跡點(diǎn)后面再次進(jìn)行預(yù)測(cè)，這次預(yù)測(cè)的假設(shè)是，障礙物沿著參考線形式。幾個(gè)注意點(diǎn)：

預(yù)測(cè)長(zhǎng)度：障礙物預(yù)測(cè)軌跡重點(diǎn)到無(wú)人車(chē)前方100m(config_.change_lane().guard_distance())這段距離

障礙物速度假定：這段距離內(nèi)，默認(rèn)障礙物速度和最后一個(gè)軌跡點(diǎn)速度一致extend_v，并且驗(yàn)證參考線前進(jìn)

預(yù)測(cè)頻率: 沒(méi)兩個(gè)點(diǎn)之間的距離為障礙物長(zhǎng)度，所以兩個(gè)點(diǎn)之間的相對(duì)時(shí)間差為：time_delta = kStepDistance / extend_v

創(chuàng)建障礙物超車(chē)標(biāo)簽

/// 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(), // 設(shè)置變道過(guò)程中，障礙物運(yùn)動(dòng)距離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); // 設(shè)置變道完成后，障礙物在參考線上的位置overtake.mutable_overtake()->set_time_buffer(config_.change_lane().min_overtake_time()); // 設(shè)置變道需要的最小時(shí)間overtake.mutable_overtake()->set_distance_s(distance); // 設(shè)置變道過(guò)程中，障礙物前進(jìn)的距離overtake.mutable_overtake()->set_fence_heading(point.heading());overtake.mutable_overtake()->mutable_fence_point()->set_x(point.x()); // 設(shè)置變道完成后，障礙物的坐標(biāo)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

對(duì)于人行橫道部分，根據(jù)禮讓規(guī)則當(dāng)行人或者非機(jī)動(dòng)車(chē)距離很遠(yuǎn)，無(wú)人車(chē)可以開(kāi)過(guò)人行橫道；當(dāng)人行橫道上有人經(jīng)過(guò)時(shí)，必須停車(chē)讓行。

/// file in apollo/modules/planning/tasks/traffic_decider/crosswalk.cc Status Crosswalk::ApplyRule(Frame* const frame, ReferenceLineInfo* const reference_line_info) {// 檢查是否存在人行橫道區(qū)域if (!FindCrosswalks(reference_line_info)) {return Status::OK();}// 為每個(gè)障礙物做標(biāo)記，障礙物存在時(shí)，無(wú)人車(chē)應(yīng)該停車(chē)還是直接駛過(guò)MakeDecisions(frame, reference_line_info);return Status::OK(); }

檢查在每個(gè)人行橫道區(qū)域內(nèi)，是否存在障礙物需要無(wú)人車(chē)停車(chē)讓行

• 如果車(chē)輛已經(jīng)駛過(guò)人行橫道一部分了，那么就忽略，不需要停車(chē)

// 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()) { // 車(chē)頭駛過(guò)人行橫道一定距離，min_pass_s_distance：1.0continue; }

遍歷每個(gè)人行道區(qū)域的障礙物(行人和非機(jī)動(dòng)車(chē))，并將人行橫道區(qū)域擴(kuò)展，提高安全性。

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

// 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; }

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

• 如果橫向距離大于疏松距離。如果軌跡相交，那么需要停車(chē)；反之直接駛過(guò)(此時(shí)軌跡相交無(wú)所謂，因?yàn)闄M向距離比較遠(yuǎn))

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;} }

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

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;} }

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

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

如果存在障礙物需要無(wú)人車(chē)停車(chē)讓行，最后可以計(jì)算無(wú)人車(chē)的加速度(是否來(lái)的及減速，若無(wú)人車(chē)速度很快減速不了，那么干脆直接駛過(guò))。計(jì)算加速的公式還是比較簡(jiǎn)單

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

s為當(dāng)前到車(chē)輛停止駛過(guò)的距離，物理公式，由函數(shù)util::GetADCStopDeceleration完成。

對(duì)那些影響無(wú)人車(chē)行駛的障礙物構(gòu)建虛擬墻障礙物類以及設(shè)置停車(chē)標(biāo)簽

什么是構(gòu)建虛擬墻類，其實(shí)很簡(jiǎn)單，單一的障礙物是一個(gè)很小的框，那么無(wú)人車(chē)在行駛過(guò)程中必須要與障礙物保持一定的距離，那么只要以障礙物為中心，構(gòu)建一個(gè)長(zhǎng)度為0.1，寬度為車(chē)道寬度的虛擬墻，只要保證無(wú)人車(chē)和這個(gè)虛擬墻障礙物不相交，就能確保安全。

// 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; }

雖然看起來(lái)函數(shù)跳轉(zhuǎn)比較多，但是其實(shí)與二中的障礙物PredictionObstacle封裝成Obstacle一樣，無(wú)非是多加了一個(gè)區(qū)域框Box2d而已。最后就是對(duì)這些虛擬墻添加停車(chē)標(biāo)志

// build stop decision const double stop_s = // 計(jì)算停車(chē)位置的累計(jì)距離，stop_distance：1m，人行橫道前1m處停車(chē)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); // 設(shè)置停車(chē)點(diǎn)的角度/方向 stop_decision->mutable_stop_point()->set_x(stop_point.x()); // 設(shè)置停車(chē)點(diǎn)的坐標(biāo) 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); // 設(shè)置促使無(wú)人車(chē)停車(chē)的障礙物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

到達(dá)目的地情況下，障礙物促使無(wú)人車(chē)采取的行動(dòng)無(wú)非是靠邊停車(chē)或者尋找合適的停車(chē)點(diǎn)(距離目的地還有一小段距離，不需要激勵(lì)停車(chē))。決策規(guī)則比較簡(jiǎn)單：

主要的決策邏輯

• 若當(dāng)前正處于PULL_OVER狀態(tài)，則繼續(xù)保持

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; }

• 檢查車(chē)輛當(dāng)前是否需要進(jìn)入U(xiǎn)LL_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前停車(chē)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檢查機(jī)制(Apollo在DESTINATION中不啟用PULL_OVER)

• 若在目的地情況下不啟用PULL_OVER機(jī)制，則返回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; }

• 若無(wú)人車(chē)與目的地距離太遠(yuǎn)，則返回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標(biāo)簽設(shè)定

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標(biāo)簽設(shè)定和人行橫道情況處理中STOP一致，創(chuàng)建虛擬墻，并封裝成新的PathObstacle加入該ReferenceLineInfo的PathDecision中。

3.5 前車(chē)情況處理–FRONT_VEHICLE

在存在前車(chē)的情況下，障礙物對(duì)無(wú)人車(chē)的決策影響有兩個(gè)，一是跟車(chē)等待機(jī)會(huì)超車(chē)(也有可能一直跟車(chē)，視障礙物信息決定)；而是停車(chē)(障礙物是靜態(tài)的，無(wú)人車(chē)必須停車(chē))

跟車(chē)等待機(jī)會(huì)超車(chē)處理

當(dāng)前車(chē)是運(yùn)動(dòng)的，而且超車(chē)條件良好，那么就存在超車(chē)的可能，這里定義的超車(chē)指代的需要無(wú)人車(chē)調(diào)整橫向距離以后超車(chē)，直接駛過(guò)前方障礙物的屬于正常行駛，可忽略障礙物。要完成超車(chē)行為需要經(jīng)過(guò)4個(gè)步驟：

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

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

Step 1. 檢查超車(chē)條件–FrontVehicle::FindPassableObstacle

這個(gè)過(guò)程其實(shí)就是尋找有沒(méi)有影響無(wú)人車(chē)正常行駛的障礙物(無(wú)人車(chē)可能需要超車(chē))，遍歷PathDecision中的所有PathObstacle，依次檢查以下條件：

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

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

• 檢車(chē)障礙物與無(wú)人車(chē)位置關(guān)系，障礙物在無(wú)人車(chē)后方，直接忽略。

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; }

• 檢查橫向與縱向距離，若縱向距離過(guò)遠(yuǎn)則可以忽略，依舊進(jìn)行正常行駛；若側(cè)方距離過(guò)大，可直接忽略障礙物，直接正常向前行駛。

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函數(shù)最后會(huì)返回第一個(gè)找到的限制無(wú)人車(chē)行駛的障礙物

Step 2. 設(shè)定超車(chē)各階段標(biāo)志–FrontVehicle::ProcessSidePass

• 若上階段處于SidePassStatus::UNKNOWN狀態(tài)，設(shè)置為正常行駛

• 若上階段處于SidePassStatus::DRIVE狀態(tài)，并且障礙物阻塞(存在需要超車(chē)條件)，那么設(shè)置為等待超車(chē)；否則前方?jīng)]有障礙物，繼續(xù)正常行駛

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狀態(tài)，若前方已無(wú)障礙物，設(shè)置為正常行駛；否則視情況而定：

a. 前方已無(wú)妨礙物，設(shè)置為正常行駛

b. 前方有障礙物，檢查已等待的時(shí)間，超過(guò)閾值，尋找左右有效車(chē)道超車(chē)；若無(wú)有效車(chē)道，則一直堵車(chē)等待。

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

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

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知識(shí)左超車(chē)還是右超車(chē)。取值為ObjectSidePass::RIGHT或ObjectSidePass::LEFT }

• 若上階段處于SidePassStatus::SIDEPASS狀態(tài)，若前方已無(wú)障礙物，設(shè)置為正常行駛；反之繼續(xù)處于超車(chē)過(guò)程。

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

停車(chē)處理

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

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

• 檢查障礙物和無(wú)人車(chē)位置，若障礙物在無(wú)人車(chē)后，忽略，由后車(chē)模塊處理

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

• 障礙物已經(jīng)在超車(chē)模塊中被標(biāo)記，迫使無(wú)人車(chē)超車(chē)，那么此時(shí)就不需要考慮該障礙物

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

• 檢查是否必須要停車(chē)條件，車(chē)道沒(méi)有足夠的寬度來(lái)允許無(wú)人車(chē)超車(chē)

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() - // 計(jì)算障礙物左側(cè)空余距離config_.front_vehicle().nudge_l_buffer(); double right_driving_width = right_width + obstacle_sl.start_l() - // 計(jì)算障礙物右側(cè)空余距離，+號(hào)是因?yàn)檐?chē)道線FLU坐標(biāo)系左側(cè)l是負(fù)軸，右側(cè)是正軸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 禁停區(qū)情況處理–KEEP_CLEAR

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

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

檢查無(wú)人車(chē)是否已經(jīng)駛?cè)虢^(qū)或者交叉路口，是則可直接忽略。

// 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; }

創(chuàng)建新的禁停區(qū)障礙物，并且打上標(biāo)簽為不能停車(chē)

// 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);

這里額外補(bǔ)充一下創(chuàng)建禁停區(qū)障礙物流程，主要還是計(jì)算禁停區(qū)障礙物的標(biāo)定框，即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();// 計(jì)算禁停區(qū)障礙物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;}// 計(jì)算禁停區(qū)障礙物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;}// 計(jì)算禁停區(qū)障礙物左側(cè)寬度和右側(cè)寬度，與參考線一致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;}// 最終可以得到禁停區(qū)障礙物的標(biāo)定框common::math::Box2d obstacle_box{common::math::LineSegment2d(obstacle_start_xy, obstacle_end_xy),left_lane_width + right_lane_width};// CreateStaticVirtualObstacle函數(shù)是將禁停區(qū)障礙物封裝成PathObstacle放入PathDecision中return CreateStaticVirtualObstacle(obstacle_id, obstacle_box); }

3.7 尋找停車(chē)點(diǎn)狀態(tài)–PULL_OVER

尋找停車(chē)點(diǎn)本質(zhì)上是尋找停車(chē)的位置，如果當(dāng)前已經(jīng)有停車(chē)位置(也就是上個(gè)狀態(tài)就是PULL_OVER)，那么就只需要更新?tīng)顟B(tài)信息即可；若不存在，那么就需要計(jì)算停車(chē)點(diǎn)的位置，然后構(gòu)建停車(chē)區(qū)障礙物(同人行橫道虛擬墻障礙物和禁停區(qū)障礙物)，然后創(chuàng)建障礙物的PULL_OVER標(biāo)簽。

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

代碼也是比較容易理解，這里我們挑幾個(gè)要點(diǎn)象征性的解釋一下

獲取停車(chē)點(diǎn)–GetPullOverStop函數(shù)

• 如果狀態(tài)信息中已經(jīng)存在了停車(chē)點(diǎn)位置并且有效，那么就可以直接拿來(lái)用

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()); }

• 如果不存在，那么就需要尋找停車(chē)位置–FindPullOverStop函數(shù)

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

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為當(dāng)前車(chē)輛車(chē)頭的累計(jì)距離constexpr double kDistanceUnit = 5.0;while (check_s < reference_line.Length() && // 在當(dāng)前車(chē)道上，向前采樣方式進(jìn)行停車(chē)位置檢索，前向檢索距離不超過(guò)max_check_distance(默認(rèn)60m)total_check_length < config_.pull_over().max_check_distance()) {check_s += kDistanceUnit;total_check_length += kDistanceUnit;...} }

檢查該點(diǎn)(check_s)位置右車(chē)道，如果右車(chē)道還是機(jī)動(dòng)車(chē)道，那么改點(diǎn)不能停車(chē)，至少需要變道，繼續(xù)前向搜索。

// 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; }

如果右側(cè)車(chē)道不是機(jī)動(dòng)車(chē)道，那么路測(cè)允許停車(chē)。停車(chē)位置需要在目的地點(diǎn)前PARKING_SPOT_LONGITUDINAL_BUFFER(默認(rèn)1m)，距離路測(cè)buffer_to_boundary(默認(rèn)0.5m)處停車(chē)。縱向與停車(chē)點(diǎn)的距離以車(chē)頭為基準(zhǔn)；側(cè)方與停車(chē)點(diǎn)距離取{車(chē)頭距離車(chē)道邊線，車(chē)尾距離車(chē)道邊線，車(chē)身中心距離車(chē)道邊線}距離最小值為基準(zhǔn)。

// 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中如果找到了停車(chē)位置，那么就直接對(duì)停車(chē)位置構(gòu)建一個(gè)PathObstacle，然后設(shè)置他的標(biāo)簽Stop即可。創(chuàng)建停車(chē)區(qū)障礙物的方式跟上述一樣，這里也不重復(fù)講解。該功能由函數(shù)BuildPullOverStop完成。

在1中如果找不到停車(chē)位置，那么就去尋找歷史狀態(tài)中的數(shù)據(jù)，找到了就根據(jù)2中停車(chē)，找不到強(qiáng)行在車(chē)道上停車(chē)。該功能由函數(shù)BuildInLaneStop完成

• 先去尋找歷史數(shù)據(jù)的inlane_dest_point，也就是歷史數(shù)據(jù)是否允許在車(chē)道上停車(chē)
• 如果沒(méi)找到，那么去尋找停車(chē)位置，如果找到了就可以進(jìn)行2中的停車(chē)
• 如果仍然沒(méi)找到停車(chē)位置，去尋找類中使用過(guò)的inlane_adc_potiion_stop_point_，如果找到了可以進(jìn)行2中的停車(chē)
• 如果依舊沒(méi)找到那么只能強(qiáng)行在距離終點(diǎn)plan_distance處，在車(chē)道上強(qiáng)行停車(chē)，并更新inlane_adc_potiion_stop_point_，供下次使用。

3.8 參考線結(jié)束情況處理–REFERENCE_LINE_END

當(dāng)參考線結(jié)束，一般就需要重新路由查詢，所以無(wú)人車(chē)需要停車(chē)，這種情況下如果程序正常，一般是前方?jīng)]有路了，需要重新查詢一點(diǎn)到目的地新的路由，具體的代碼也是跟人行橫道上的不可忽略障礙物一樣，在參考線終點(diǎn)前構(gòu)建一個(gè)停止墻障礙物，并設(shè)置齊標(biāo)簽為停車(chē)Stop。

Status ReferenceLineEnd::ApplyRule(Frame* frame, ReferenceLineInfo* const reference_line_info) {const auto& reference_line = reference_line_info->reference_line();// 檢查參考線剩余的長(zhǎng)度，足夠則可忽略這個(gè)情況，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; // 在參考線終點(diǎn)前，創(chuàng)建停止墻障礙物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，設(shè)置障礙物停止標(biāo)簽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

根據(jù)具體路況進(jìn)行處理，根據(jù)代碼可以分為以下情況：

• 若當(dāng)前參考線為直行，非轉(zhuǎn)彎。那么暫時(shí)就不需要重新路由，等待新的路由
• 若當(dāng)前車(chē)輛不在參考線上，不需要重新路由，等待新的路由
• 若當(dāng)前車(chē)輛可以退出了，不需要重新路由，等待新的路由
• 若當(dāng)前通道Passage終點(diǎn)不在參考線上，不需要重新路由，等待新的路由
• 若參考線的終點(diǎn)距離無(wú)人車(chē)過(guò)遠(yuǎn)，不需要重新路由，等待新的路由
• 若上時(shí)刻進(jìn)行過(guò)路由查詢，距離當(dāng)前時(shí)間過(guò)短，不需要重新路由，等待新的路由
• 其他情況，手動(dòng)發(fā)起路由查詢需求

其實(shí)這個(gè)模塊我還是有點(diǎn)不是特別敢肯定，只能做保留的解釋。首先代碼Frame::Rerouting做的工作僅僅重新路由，得到當(dāng)前位置到目的地的一個(gè)路況，這個(gè)過(guò)程并沒(méi)有產(chǎn)生新的參考線，因?yàn)閰⒖季€的產(chǎn)生依賴于ReferenceLineProvider線程。所以說(shuō)對(duì)于第二點(diǎn)，車(chē)輛不在參考線上，即使重新路由了，但是沒(méi)有生成矯正的新參考線，所以重新路由也是無(wú)用功，反之還不如等待ReferenceLineProvider去申請(qǐng)重新路由并生成對(duì)應(yīng)的參考線。所以說(shuō)2,3,4等情況的重點(diǎn)在于缺乏參考線，而不在于位置偏離了。

3.10 信號(hào)燈情況處理–SIGNAL_LIGHT

信號(hào)燈處理相對(duì)來(lái)說(shuō)比較簡(jiǎn)單，無(wú)非是有紅燈就停車(chē)；有黃燈速度小，就停車(chē)；有綠燈，或者黃燈速度大就直接駛過(guò)。具體的處理步驟分為：

檢查當(dāng)前路況下是否有信號(hào)燈區(qū)域–由函數(shù)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發(fā)布的信號(hào)等信息–由函數(shù)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; }

決策–由函數(shù)MakeDecisions完成

for (auto& signal_light : signal_lights_from_path_) {// 1. 如果信號(hào)燈是紅燈，并且加速度不是很大// 2. 如果信號(hào)燈是未知，并且加速度不是很大// 3. 如果信號(hào)燈是黃燈，并且加速度不是很大// 以上三種情況，無(wú)人車(chē)停車(chē)，停車(chē)標(biāo)簽與前面一致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);}}// 設(shè)置交叉口區(qū)域，以及是否有權(quán)力通行，停車(chē)表明無(wú)法通行。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 停車(chē)情況處理–STOP_SIGN

停車(chē)情況相對(duì)來(lái)說(shuō)比較復(fù)雜，根據(jù)代碼將停車(chē)分為：尋找下一個(gè)最近的停車(chē)信號(hào)，決策處理。尋找下一個(gè)停車(chē)點(diǎn)比較簡(jiǎn)單，由函數(shù)FindNextStopSign完成，這里直接跳過(guò)。接下來(lái)分析決策部分，可以分為以下幾步：

獲取等待車(chē)輛列表–由函數(shù)GetWatchVehicles完成。

這個(gè)過(guò)程其實(shí)就是獲取無(wú)人車(chē)前方的等待車(chē)輛，存儲(chǔ)形式為：

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

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

int StopSign::GetWatchVehicles(const StopSignInfo& stop_sign_info,StopSignLaneVehicles* watch_vehicles) {watch_vehicles->clear();StopSignStatus stop_sign_status = GetPlanningStatus()->stop_sign();// 遍歷所有的車(chē)道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;// 獲取每個(gè)車(chē)道的等候車(chē)輛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; }

檢查與更新停車(chē)狀態(tài)PlanningStatus.stop_sign–由函數(shù)ProcessStopStatus完成

停車(chē)過(guò)程可以分為5個(gè)階段：正常行駛DRIVE–開(kāi)始停車(chē)STOP–等待緩沖狀態(tài)WAIT–緩慢前進(jìn)CREEP–徹底停車(chē)DONE。

• 更新停車(chē)狀態(tài)。如果無(wú)人車(chē)距離最近一個(gè)停車(chē)區(qū)域過(guò)遠(yuǎn)，那么狀態(tài)為正常行駛

// 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; }

• 如果停車(chē)狀態(tài)是正常行駛DRIVE。

這種情況下，如果車(chē)輛速度很大，或者與停車(chē)區(qū)域距離過(guò)遠(yuǎn)，那么繼續(xù)設(shè)置為行駛DRIVE；反之就進(jìn)入停車(chē)狀態(tài)STOP。狀態(tài)檢查由CheckADCkStop完成。

• 如果停車(chē)狀態(tài)是開(kāi)始停車(chē)STOP。

這種情況下，如果從開(kāi)始停車(chē)到當(dāng)前經(jīng)歷的等待時(shí)間沒(méi)有超過(guò)閾值stop_duration(默認(rèn)1s)，繼續(xù)保持STOP狀態(tài)。反之，如果前方等待車(chē)輛不為空，那么就進(jìn)入下一階段WAIT緩沖階段；如果前方車(chē)輛為空，那么可以直接進(jìn)入到緩慢前進(jìn)CREEP狀態(tài)或者停車(chē)完畢狀態(tài)。

• 如果停車(chē)狀態(tài)是等待緩沖WAIT

這種情況下，如果等待時(shí)間沒(méi)有超過(guò)一個(gè)閾值wait_timeout(默認(rèn)8s)或者前方存在等待車(chē)輛，繼續(xù)保持等待狀態(tài)。反之可以進(jìn)入到緩慢前進(jìn)或者停車(chē)完畢狀態(tài)

• 如果停車(chē)狀態(tài)是緩慢前進(jìn)CREEP

這種情況下，只需要檢查無(wú)人車(chē)車(chē)頭和停車(chē)區(qū)域的距離，如果大于某個(gè)值那么說(shuō)明可以繼續(xù)緩慢前進(jìn)，保持狀態(tài)不變，反之就可以完全停車(chē)了。

更新前方等待車(chē)輛

a.當(dāng)前狀態(tài)是DRIVE，那么需要將障礙物都加入到前方等待車(chē)輛列表中，因?yàn)檫@些障礙物到時(shí)候都會(huì)排在無(wú)人車(chē)前方等待。

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.如果無(wú)人車(chē)當(dāng)前狀態(tài)是等待或者停車(chē)，刪除部分排隊(duì)等待車(chē)輛–RemoveWatchVehicle函數(shù)完成

這種情況下，如果障礙物已經(jīng)駛過(guò)停車(chē)區(qū)域，那么對(duì)其刪除；否則繼續(xù)保留。

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; // 如果障礙物已經(jīng)駛過(guò)停車(chē)區(qū)一定距離，可以將障礙物從等待車(chē)輛中刪除。 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());} }

• 對(duì)剩下來(lái)的障礙物重新組成一個(gè)新的等待隊(duì)列–ClearWatchVehicle函數(shù)完成

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();) {// 如果新的隊(duì)列中已經(jīng)不存在該障礙物了，那么直接將障礙物從這條車(chē)道中刪除if (obstacle_ids.count(*obstacle_it) == 0) { obstacle_it = vehicle_ids.erase(obstacle_it);} else {++obstacle_it;}}if (vehicle_ids.empty()) { // 如果這整條車(chē)道上都不存在等待車(chē)輛了，直接把這條車(chē)道刪除watch_vehicles->erase(it++);} else {++it;} }

• 更新車(chē)輛狀態(tài)PlanningStatus.stop_sign

這部分由函數(shù)UpdateWatchVehicles完成，主要是將3中得到的新的等待車(chē)輛隊(duì)列更新至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. 如果當(dāng)前車(chē)輛狀態(tài)是緩慢前進(jìn)狀態(tài)CREEP

這種情況下，可以直接創(chuàng)建停車(chē)的標(biāo)簽。

---

最后總結(jié)一下，障礙物和路況對(duì)無(wú)人車(chē)的決策影響分為兩類，一類是縱向影響LongitudinalDecision，一類是側(cè)向影響LateralDecision。

縱向影響：

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

側(cè)向影響：

const std::unordered_map<ObjectDecisionType::ObjectTagCase, int, PathObstacle::ObjectTagCaseHash>PathObstacle::s_lateral_decision_safety_sorter_ = {{ObjectDecisionType::kIgnore, 0}, // 忽略，優(yōu)先級(jí)0{ObjectDecisionType::kNudge, 100}, // 微調(diào)，優(yōu)先級(jí)100{ObjectDecisionType::kSidepass, 200}}; // 繞行，優(yōu)先級(jí)200

當(dāng)有一個(gè)障礙物在11中路況下多次出發(fā)無(wú)人車(chē)決策時(shí)該怎么辦？

縱向決策合并，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) { // 優(yōu)先選取優(yōu)先級(jí)大的決策return rhs;} else if (lhs_val > rhs_val) {return lhs;} else {if (lhs.has_ignore()) {return rhs;} else if (lhs.has_stop()) { // 如果優(yōu)先級(jí)相同，都是停車(chē)，選取停車(chē)距離小的決策，防止安全事故return lhs.stop().distance_s() < rhs.stop().distance_s() ? lhs : rhs;} else if (lhs.has_yield()) { // 如果優(yōu)先級(jí)相同，都是減速，選取減速距離小的決策，防止安全事故return lhs.yield().distance_s() < rhs.yield().distance_s() ? lhs : rhs;} else if (lhs.has_follow()) { // 如果優(yōu)先級(jí)相同，都是跟隨，選取跟隨距離小的決策，防止安全事故return lhs.follow().distance_s() < rhs.follow().distance_s() ? lhs : rhs;} else if (lhs.has_overtake()) { // 如果優(yōu)先級(jí)相同，都是超車(chē)，選取超車(chē)距離大的決策，防止安全事故return lhs.overtake().distance_s() > rhs.overtake().distance_s() ? lhs : rhs;} else {DCHECK(false) << "Unknown decision";}}return lhs; // stop compiler complaining }

側(cè)向合并，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) { // 優(yōu)先選取優(yōu)先級(jí)大的決策 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()) { // 如果優(yōu)先級(jí)相同，都是微調(diào)，選取側(cè)向微調(diào)大的決策return std::fabs(lhs.nudge().distance_l()) >std::fabs(rhs.nudge().distance_l())? lhs: rhs;}}return lhs; }

如有侵權(quán)，請(qǐng)聯(lián)系本人刪除！

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

總結(jié)

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

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。