當前位置：首頁 > 编程资源 > 编程问答 >内容正文

编程问答

Apollo Planning决策规划算法代码详细解析 (1)：Scenario选择

發布時間：2023/12/8 编程问答 26 豆豆

生活随笔收集整理的這篇文章主要介紹了 Apollo Planning决策规划算法代码详细解析 (1)：Scenario选择小編覺得挺不錯的,現在分享給大家,幫大家做個參考.

本文重點講解Apollo代碼中怎樣配置Scenario以及選擇當前Scenario，Scenario場景決策是Apollo規劃算法的第一步，本文會對代碼進行詳細解析，也會梳理整個決策流程，碼字不易，喜歡的朋友們麻煩點個關注與贊。?

Apollo Planning決策規劃系列文章：

Apollo Planning決策規劃代碼詳細解析 (2)：Scenario執行

Apollo Planning決策規劃代碼詳細解析 (3)：stage執行

Apollo Planning決策規劃代碼詳細解析 (4)：Stage邏輯詳解

Apollo Planning決策規劃代碼詳細解析 (5)：規劃算法流程介紹

Apollo Planning決策規劃代碼詳細解析 (6): LaneChangeDecider

Apollo Planning決策規劃代碼詳細解析 (7): PathReuseDecider

Apollo Planning決策規劃代碼詳細解析 (8): PathLaneBorrowDecide

Apollo Planning決策規劃代碼詳細解析 (9): PathBoundsDecider

Apollo Planning決策規劃代碼詳細解析 (10)：PiecewiseJerkPathOptimizer

Apollo Planning決策規劃代碼詳細解析 (11): PathAssessmentDecider

Apollo Planning決策規劃代碼詳細解析 (12): PathDecider

Apollo Planning決策規劃代碼詳細解析 (13): RuleBasedStopDecider

Apollo Planning決策規劃算法代碼詳細解析 (14):SPEED_BOUNDS_PRIORI_DECIDER

Apollo Planning決策規劃算法代碼解析(15): 速度動態規劃SPEED_HEURISTIC_OPTIMIZER 上

Apollo Planning決策規劃算法代碼解析 (16):SPEED_HEURISTIC_OPTIMIZER 速度動態規劃中

Apollo Planning決策規劃算法代碼解析 (17):SPEED_HEURISTIC_OPTIMIZER 速度動態規劃下

算法介紹文章：?

Apollo決策規劃算法Planning : Piecewise Jerk Path Optimizer的python實現

仿真技術介紹文章：?

prescan聯合simulink進行FCW的仿真_自動駕駛 Player的博客-CSDN博客

?如果對apollo規劃算法感興趣，想學習完整的系列文章，可以訂閱下面專欄：https://blog.csdn.net/nn243823163/category_11685852.htmlhttps://blog.csdn.net/nn243823163/category_11685852.html

正文如下：?

本文重點講解Apollo代碼中怎樣配置Scenario以及選擇當前Scenario，Scenario決策是Apollo規劃算法的第一步，本文會對代碼進行詳細解析，也會梳理整個決策流程，碼字不易，喜歡的朋友們麻煩點個關注與贊。??

在本文你將學到下面這些內容：

規劃器planer的種類；

規劃器planer的主要函數及邏輯；

場景管理類ScenarioManager的運行機制；

場景注冊方法；

場景決策流程，如何選擇當前場景

詳細的apollo決策規劃代碼分析

代碼具體過程如下：

0、規劃算法的入口

（1）規劃模塊的入口函數是PlanningComponent的Proc。

（2）以規劃模式OnLanePlanning，執行RunOnce。在RunOnce中先執行交通規則，再規劃軌跡。規劃軌跡的函數是Plan。

1、Scenario的判斷在Planer中進行，目前Apollo共有下面這些planer，其中最常用的就是EM規劃器，即PublicRoadPlanner，本系列主要介紹PublicRoadPlanner這個Planer。

2、Apollo會根據配置調用PublicRoadPlanner這個planer，關于配置方法，之后會在另外一篇博文進行更新。PublicRoadPlanner主要有init()與plan()兩個重要的函數，inti()是規劃器的初始化，plan就是具體的規劃過程。

class PublicRoadPlanner : public PlannerWithReferenceLine {public:/*** @brief Constructor*/PublicRoadPlanner() = delete;explicit PublicRoadPlanner(const std::shared_ptr<DependencyInjector>& injector): PlannerWithReferenceLine(injector) {}/*** @brief Destructor*/virtual ~PublicRoadPlanner() = default;void Stop() override {}std::string Name() override { return "PUBLIC_ROAD"; }common::Status Init(const PlanningConfig& config) override;/*** @brief Override function Plan in parent class Planner.* @param planning_init_point The trajectory point where planning starts.* @param frame Current planning frame.* @return OK if planning succeeds; error otherwise.*/common::Status Plan(const common::TrajectoryPoint& planning_init_point,Frame* frame,ADCTrajectory* ptr_computed_trajectory) override; };

3、scenario的選擇在Plan() 函數的update階段，主要用的是ScenarioManager類的updata函數。ScenarioManager并不屬于某個特定的planer，這個類只針對于scenario，每個planer都可以調用它來管理場景。下面代碼片段是PublicRoadPlanner的Plan()函數。

Status PublicRoadPlanner::Plan(const TrajectoryPoint& planning_start_point,Frame* frame,ADCTrajectory* ptr_computed_trajectory) {// 決策當前應該執行哪個場景scenario_manager_.Update(planning_start_point, *frame);// 獲取當前場景scenario_ = scenario_manager_.mutable_scenario();// 處理當前場景auto result = scenario_->Process(planning_start_point, frame);// 打印debug信息if (FLAGS_enable_record_debug) {auto scenario_debug = ptr_computed_trajectory->mutable_debug()->mutable_planning_data()->mutable_scenario();scenario_debug->set_scenario_type(scenario_->scenario_type());scenario_debug->set_stage_type(scenario_->GetStage());scenario_debug->set_msg(scenario_->GetMsg());}// 場景處理成功if (result == scenario::Scenario::STATUS_DONE) {// only updates scenario manager when previous scenario's status is// STATUS_DONEscenario_manager_.Update(planning_start_point, *frame);} // 場景處理失敗else if (result == scenario::Scenario::STATUS_UNKNOWN) {return Status(common::PLANNING_ERROR, "scenario returned unknown");}return Status::OK(); }

4、ScenarioManager會實例化一個全局的scenario_manager_對象來進行場景管理，在PublicRoadPlanner初始化時會調用配置文件里的參數來建立這個對象。

Status PublicRoadPlanner::Init(const PlanningConfig& config) {config_ = config;scenario_manager_.Init(config);return Status::OK(); }

調用ScenarioManager類的init()函數，并且根據當前planer的配置來注冊場景。

bool ScenarioManager::Init(const std::set<ScenarioConfig::ScenarioType>& supported_scenarios) {// 注冊場景RegisterScenarios();default_scenario_type_ = ScenarioConfig::LANE_FOLLOW;supported_scenarios_ = supported_scenarios;// 創建場景，默認為lane_followcurrent_scenario_ = CreateScenario(default_scenario_type_);return true; }

目前PublicRoadPlanner支持下面這些場景

// 還是在"/conf/planning_config.pb.txt"中 standard_planning_config {planner_type: PUBLIC_ROADplanner_type: OPEN_SPACEplanner_public_road_config {// 支持的場景scenario_type: LANE_FOLLOW // 車道線保持scenario_type: SIDE_PASS // 超車scenario_type: STOP_SIGN_UNPROTECTED // 停止scenario_type: TRAFFIC_LIGHT_PROTECTED // 紅綠燈scenario_type: TRAFFIC_LIGHT_UNPROTECTED_LEFT_TURN // 紅綠燈左轉scenario_type: TRAFFIC_LIGHT_UNPROTECTED_RIGHT_TURN // 紅綠燈右轉scenario_type: VALET_PARKING // 代客泊車}

5、ScenarioManager類的Update()函數，用來決策當前處在什么場景。如果進入了新的場景，會創建一個新的對象來進行之后的規劃邏輯。

void ScenarioManager::Update(const common::TrajectoryPoint& ego_point,const Frame& frame) {ACHECK(!frame.reference_line_info().empty());Observe(frame);ScenarioDispatch(frame); }

場景決策邏輯在ScenarioDispatch(frame)當中，會根據配置選擇基于規則還是基于學習的決策方法。

void ScenarioManager::ScenarioDispatch(const Frame& frame) {ACHECK(!frame.reference_line_info().empty());ScenarioConfig::ScenarioType scenario_type;int history_points_len = 0;if (injector_->learning_based_data() &&injector_->learning_based_data()->GetLatestLearningDataFrame()) {history_points_len = injector_->learning_based_data()->GetLatestLearningDataFrame()->adc_trajectory_point_size();}if ((planning_config_.learning_mode() == PlanningConfig::E2E ||planning_config_.learning_mode() == PlanningConfig::E2E_TEST) &&history_points_len >= FLAGS_min_past_history_points_len) {scenario_type = ScenarioDispatchLearning();} else {scenario_type = ScenarioDispatchNonLearning(frame);}ADEBUG << "select scenario: "<< ScenarioConfig::ScenarioType_Name(scenario_type);// update PlanningContextUpdatePlanningContext(frame, scenario_type);if (current_scenario_->scenario_type() != scenario_type) {current_scenario_ = CreateScenario(scenario_type);} }

6、ScenarioDispatchNonLearning()函數默認從lanefollow場景開始判斷，首先根據駕駛員的意圖來安排場景，如果不是默認的lanefollow場景，直接輸出當前場景；如果是lanefollow場景，會依次判斷是否屬于別的場景；即剩余場景之間的跳轉必須經過lanefollow這個場景。

ScenarioConfig::ScenarioType ScenarioManager::ScenarioDispatchNonLearning(const Frame& frame) {// default: LANE_FOLLOWScenarioConfig::ScenarioType scenario_type = default_scenario_type_;// Pad Msg scenarioscenario_type = SelectPadMsgScenario(frame);const auto vehicle_state_provider = injector_->vehicle_state();common::VehicleState vehicle_state = vehicle_state_provider->vehicle_state();const common::PointENU& target_point =frame.local_view().routing->routing_request().dead_end_info().target_point();const common::VehicleState& car_position = frame.vehicle_state();if (scenario_type == default_scenario_type_) {// check current_scenario (not switchable)switch (current_scenario_->scenario_type()) {case ScenarioConfig::LANE_FOLLOW:case ScenarioConfig::PULL_OVER:break;case ScenarioConfig::BARE_INTERSECTION_UNPROTECTED:case ScenarioConfig::EMERGENCY_PULL_OVER:case ScenarioConfig::PARK_AND_GO:case ScenarioConfig::STOP_SIGN_PROTECTED:case ScenarioConfig::STOP_SIGN_UNPROTECTED:case ScenarioConfig::TRAFFIC_LIGHT_PROTECTED:case ScenarioConfig::TRAFFIC_LIGHT_UNPROTECTED_LEFT_TURN:case ScenarioConfig::TRAFFIC_LIGHT_UNPROTECTED_RIGHT_TURN:case ScenarioConfig::VALET_PARKING:case ScenarioConfig::DEADEND_TURNAROUND:// transfer dead_end to lane follow, should enhance transfer logicif (JudgeReachTargetPoint(car_position, target_point)) {scenario_type = ScenarioConfig::LANE_FOLLOW;reach_target_pose_ = true;}case ScenarioConfig::YIELD_SIGN:// must continue until finishif (current_scenario_->GetStatus() !=Scenario::ScenarioStatus::STATUS_DONE) {scenario_type = current_scenario_->scenario_type();}break;default:break;}}// ParkAndGo / starting scenarioif (scenario_type == default_scenario_type_) {if (FLAGS_enable_scenario_park_and_go && !reach_target_pose_) {scenario_type = SelectParkAndGoScenario(frame);}}// intersection scenariosif (scenario_type == default_scenario_type_) {scenario_type = SelectInterceptionScenario(frame);}// pull-over scenarioif (scenario_type == default_scenario_type_) {if (FLAGS_enable_scenario_pull_over) {scenario_type = SelectPullOverScenario(frame);}}// VALET_PARKING scenarioif (scenario_type == default_scenario_type_) {scenario_type = SelectValetParkingScenario(frame);}// dead endif (scenario_type == default_scenario_type_) {scenario_type = SelectDeadEndScenario(frame);}return scenario_type; }

7、在場景判斷時，首先調用函數SelectPadMsgScenario()，根據駕駛員意圖來安排場景.

ScenarioConfig::ScenarioType ScenarioManager::SelectPadMsgScenario(const Frame& frame) {const auto& pad_msg_driving_action = frame.GetPadMsgDrivingAction();switch (pad_msg_driving_action) {case DrivingAction::PULL_OVER:if (FLAGS_enable_scenario_emergency_pull_over) {return ScenarioConfig::EMERGENCY_PULL_OVER;}break;case DrivingAction::STOP:if (FLAGS_enable_scenario_emergency_stop) {return ScenarioConfig::EMERGENCY_STOP;}break;case DrivingAction::RESUME_CRUISE:if (current_scenario_->scenario_type() ==ScenarioConfig::EMERGENCY_PULL_OVER ||current_scenario_->scenario_type() ==ScenarioConfig::EMERGENCY_STOP) {return ScenarioConfig::PARK_AND_GO;}break;default:break;}return default_scenario_type_; }

8、可以看到，除了駕駛員行為相關的兩個場景外，每次切換場景必須是從默認場景(LANE_FOLLOW)開始，即每次場景切換之后都會回到默認場景。

9、以上即為apollo場景決策邏輯。后續文章會講場景選擇之后，怎么進行下一步的規劃算法。

總結

以上是生活随笔為你收集整理的Apollo Planning决策规划算法代码详细解析 (1)：Scenario选择的全部內容，希望文章能夠幫你解決所遇到的問題。

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