解析百度Apollo之決策規劃模組

paulquei發表於2019-02-24

本文是Apollo專案系列文章中的一篇，會解析自動駕駛系統中最核心的模組 – 決策規劃模組。

前言

Apollo系統中的Planning模組實際上是整合了決策和規劃兩個功能，該模組是自動駕駛系統中最核心的模組之一（另外三個核心模組是：定位，感知和控制）。

關於決策規劃的理論值得我們研究好久。所以接下來會通過幾篇文章來專門講解Planning模組。

這些文章會以百度Apollo的Planning模組實現為基礎。當然，我也希望我們能夠不單單侷限於Apollo專案實現。如果可以，儘可能的一起了解一下目前這些技術的其他研究成果。

本文是講解Planning模組的第一篇文章，會對Planning模組的整體架構做一些解析。

其他關於Apollo相關的文章如下：

本文以Apollo專案2019年初的版本為基礎進行講解。

版本：3.5
獲取程式碼時間：2019年1月24日

Apollo系統與Planning模組

下圖是Apollo系統的整體架構圖。從這幅圖中我們可以看出，整個系統可以分為5層。從下至上依次是：

車輛認證平臺：經過Apollo認證的電子線控車輛。
硬體開發平臺：包含了計算單元以及各種感測器，例如：GPS，攝像頭，雷達，鐳射雷達等等。
開放軟體平臺：這就是Apollo開原始碼的主體，也是自動駕駛最核心的部分。
雲服務平臺：包含了各種雲端服務，實現自動駕駛的車輛一定不是孤立的，而是跑在基於網際網路的雲端上的。
量產交付方案：專門為各種場景量產的解決方案。

我們可以再將最核心的開放軟體平臺這一層放大近看一下。

下面這幅圖描述了這其中的模組和它們之間的互動關係。

這其中的黃線代表了資料流，黑線代表了控制流。

Planning模組負責整個車輛的駕駛決策，而駕駛決策需要根據當前所處的地理位置，周邊道路和交通情況來決定。Planning不直接控制車輛硬體，而是藉助於控制模組來完成。

從這幅圖中可以看出，對於Planning模組來說：

它的上游模組是：定位，地圖，導航，感知和預測模組。
它的下游模組是控制模組。

模組概述

決策規劃模組的主要責任是：根據導航資訊以及車輛的當前狀態，在有限的時間範圍內，計算出一條合適的軌跡供車輛行駛。

這裡有好幾個地方值得注意：

車輛的行駛路線通常由Routing模組提供，Routing模組會根據目的地以及地圖搜尋出一條代價儘可能小的路線。關於Routing模組我們已經在另外一篇文章中詳細講解過（《解析百度Apollo之Routing模組》），這裡不再贅述。
車輛的當前狀態包含了很多因素，例如：車輛自身的狀態（包括姿態，速度，角速度等等），當前所處的位置，周邊物理世界的靜態環境以及交通狀態等等。
Planning模組的響應速度必須是穩定可靠的（當然，其他模組也是一樣）。正常人類的反應速度是300ms，而自動駕駛車輛想要做到安全可靠，其反應時間必須短於100ms。所以，Planning模組通常以10Hz的頻率執行著。如果其中某一個演算法的時間耗費了太長時間，就可能造成其他模組的處理延遲，最終可能造成嚴重的後果。例如：沒有即時剎車，或者轉彎。
”合適的軌跡“有多個層次的含義。首先，”軌跡“不同於“路徑”，“軌跡”不僅僅包含了行駛路線，還要包含每個時刻的車輛的速度，加速度，方向轉向等資訊。其次，這條軌跡必須是底層控制可以執行的。因為車輛在運動過程中，具有一定的慣性，車輛的轉彎角度也是有限的。在計算行駛軌跡時，這些因素都要考慮。最後，從人類的體驗上來說，猛加速，急剎車或者急轉彎都會造成非常不好的乘坐體驗，因此這些也需要考慮。這就是為什麼決定規劃模組需要花很多的精力來優化軌跡，Apollo系統中的實現自然也不例外。

模組架構

Apollo的之前版本，包括3.0都是用了相同的配置和引數規劃不同的場景，這種方法雖然線性且實現簡單，但不夠靈活或用於特定場景。隨著Apollo的成熟並承擔不同的道路條件和駕駛用例，Apollo專案組採用了更加模組化、適用於特定場景和整體的方法來規劃其軌跡。

在這個方法中，每個駕駛用例都被當作不同的駕駛場景。這樣做非常有用，因為與先前的版本相比，現在在特定場景中報告的問題可以在不影響其他場景的工作的情況下得到修復，其中問題修復影響其他駕駛用例，因為它們都被當作單個駕駛場景處理。

Apollo 3.5中Planning模組的架構如下圖所示：

這其中主要的元件包括：

Apollo FSM：一個有限狀態機，與高清地圖確定車輛狀態給定其位置和路線。
Planning Dispatcher：根據車輛的狀態和其他相關資訊，呼叫合適的Planner。
Planner：獲取所需的上下文資料和其他資訊，確定相應的車輛意圖，執行該意圖所需的規劃任務並生成規劃軌跡。它還將更新未來作業的上下文。
Deciders和Optimizers：一組實現決策任務和各種優化的無狀態庫。優化器特別優化車輛的軌跡和速度。決策者是基於規則的分類決策者，他們建議何時換車道、何時停車、何時爬行（慢速行進）或爬行何時完成。
黃色框：這些框被包含在未來的場景和/或開發人員中，以便基於現實世界的驅動用例貢獻他們自己的場景。

整體Pipeline

有相關專業知識的人都會知道，決策規劃模組的主體實現通常都是較為複雜的。

Apollo系統中的實現自然也不例外，這裡先通過一幅圖說明其整體的Pipeline。然後在下文中我們再逐步熟悉每個細節。

這裡有三個主要部分需要說明：

PncMap：全稱是Planning and Control Map。這個部分的實現並不在Planning內部，而是位於/modules/map/pnc_map/目錄下。但是由於該實現與Planning模組緊密相關，因此這裡放在一起討論。該模組的主要作用是：根據Routing提供的資料，生成Planning模組需要的路徑資訊。
Frame：Frame中包含了Planning一次計算迴圈中需要的所有資料。例如：地圖，車輛狀態，參考線，障礙物資訊等等。ReferenceLine是車輛行駛的參考線，TrafficDecider與交通規則相關，這兩個都是Planning中比較重要的子模組，因此會在下文中專門講解。
EM Planner：下文中我們會看到，Apollo系統中內建了好幾個Planner，但目前預設使用的是EM Planner，這也是專門為開放道路設計的。該模組的實現可以說是整個Planning模組的靈魂所在。因此其演算法值得專門用另外一篇文章來講解。讀者也可以閱讀其官方論文來了解：Baidu Apollo EM Motion Planner。

基礎資料結構

Planning模組是一個比較大的模組，因此這其中有很多的資料結構需要在內部實現中流轉。

這些資料結構集中定義在兩個地方：

proto目錄：該目錄下都是通過Protocol Buffers格式定義的結構。這些結構會在編譯時生成C++需要的檔案。這些結構沒有業務邏輯，就是專門用來儲存資料的。（實際上不只是Planning，幾乎每個大的模組都會有自己的proto資料夾。）
common目錄：這裡是C++定義的資料結構。很顯然，通過C++定義資料結構的好處是這些類的實現中可以包含一定的處理邏輯。

proto

proto目錄下的檔案如下所示：

apollo/modules/planning/proto/
├── auto_tuning_model_input.proto
├── auto_tuning_raw_feature.proto
├── decider_config.proto
├── decision.proto
├── dp_poly_path_config.proto
├── dp_st_speed_config.proto
├── lattice_sampling_config.proto
├── lattice_structure.proto
├── navi_obstacle_decider_config.proto
├── navi_path_decider_config.proto
├── navi_speed_decider_config.proto
├── pad_msg.proto
├── planner_open_space_config.proto
├── planning.proto
├── planning_config.proto
├── planning_internal.proto
├── planning_stats.proto
├── planning_status.proto
├── poly_st_speed_config.proto
├── poly_vt_speed_config.proto
├── proceed_with_caution_speed_config.proto
├── qp_piecewise_jerk_path_config.proto
├── qp_problem.proto
├── qp_spline_path_config.proto
├── qp_st_speed_config.proto
├── reference_line_smoother_config.proto
├── side_pass_path_decider_config.proto
├── sl_boundary.proto
├── spiral_curve_config.proto
├── st_boundary_config.proto
├── traffic_rule_config.proto
└── waypoint_sampler_config.proto

我們在Routing模組講解的文章中已經提到，通過proto格式定義資料結構好處有兩個：

自動生成C++需要的資料結構。
可以方便的從文字檔案匯入和匯出。下文將看到，Planning模組中有很多配置檔案就是和這裡的proto結構相對應的。

common

common目錄下的標頭檔案如下：

apollo/modules/planning/common/
├── change_lane_decider.h
├── decision_data.h
├── distance_estimator.h
├── ego_info.h
├── frame.h
├── frame_manager.h
├── indexed_list.h
├── indexed_queue.h
├── lag_prediction.h
├── local_view.h
├── obstacle.h
├── obstacle_blocking_analyzer.h
├── path
│   ├── discretized_path.h
│   ├── frenet_frame_path.h
│   └── path_data.h
├── path_decision.h
├── planning_context.h
├── planning_gflags.h
├── reference_line_info.h
├── speed
│   ├── speed_data.h
│   ├── st_boundary.h
│   └── st_point.h
├── speed_limit.h
├── speed_profile_generator.h
├── threshold.h
├── trajectory
│   ├── discretized_trajectory.h
│   ├── publishable_trajectory.h
│   └── trajectory_stitcher.h
└── trajectory_info.h

這裡有如下一些結構值得我們注意：

名稱	說明
`EgoInfo`類	包含了自車資訊，例如：當前位置點，車輛狀態，外圍Box等。
`Frame`類	包含了一次Planning計算迴圈中的所有資訊。
`FrameManager`類	Frame的管理器，每個Frame會有一個整數型id。
`LocalView`類	Planning計算需要的輸入，下文將看到其定義。
`Obstacle`類	描述一個特定的障礙物。障礙物會有一個唯一的id來區分。
`PlanningContext`類	Planning全域性相關的資訊，例如：是否正在變道。這是一個單例。
`ReferenceLineInfo`類	車輛行駛的參考線，下文會專門講解。
`path`資料夾	描述車輛路線資訊。包含：PathData，DiscretizedPath，FrenetFramePath三個類。
`speed`資料夾	描述車輛速度資訊。包含SpeedData，STPoint，StBoundary三個類。
`trajectory`資料夾	描述車輛軌跡資訊。包含DiscretizedTrajectory，PublishableTrajectory，TrajectoryStitcher三個類。
`planning_gflags.h`	定義了模組需要的許多常量，例如各個配置檔案的路徑。

你暫時不用記住所有這些類，在後面的文章中，我們會逐漸知道它們的作用。

模組配置

在下文中大家會發現，Planning模組中有很多處的邏輯是通過配置檔案控制的。通過將這部分內容從程式碼中剝離，可以方便的直接對配置檔案進行調整，而不用編譯原始碼。這對於系統除錯和測試來說，是非常方便的。

Apollo系統中，很多模組都是類似的設計。因此每個模組都會將配置檔案集中放在一起，也就是每個模組下的conf目錄。

Planning模組的配置檔案如下所示：

apollo/modules/planning/conf/
├── adapter.conf
├── cosTheta_smoother_config.pb.txt
├── navi_traffic_rule_config.pb.txt
├── planner_open_space_config.pb.txt
├── planning.conf
├── planning_config.pb.txt
├── planning_config_navi.pb.txt
├── planning_navi.conf
├── qp_spline_smoother_config.pb.txt
├── scenario
│   ├── lane_follow_config.pb.txt
│   ├── side_pass_config.pb.txt
│   ├── stop_sign_unprotected_config.pb.txt
│   ├── traffic_light_protected_config.pb.txt
│   └── traffic_light_unprotected_right_turn_config.pb.txt
├── spiral_smoother_config.pb.txt
└── traffic_rule_config.pb.txt

這裡的絕大部分檔案都是.pb.txt字尾的。因為這些檔案是和上面提到的proto結構相對應的。因此可以直接被proto檔案生成的資料結構讀取。對於不熟悉的讀者可以閱讀Protocal Buffer的文件：google.protobuf.text_format。

讀者暫時不用太在意這些檔案的內容。隨著對於Planning模組實現的熟悉，再回過來看這些配置檔案，就很容易理解每個配置檔案的作用了。下文中，對一些關鍵內容我們會專門提及。

Planner

Planning與Planner

Apollo 3.5廢棄了原先的ROS，引入了新的執行環境：Cyber RT。

Cyber RT以元件的方式來管理各個模組，元件的實現會基於該框架提供的基類：apollo::cyber::Component。

Planning模組自然也不例外。其實現類是下面這個：

class PlanningComponent final
    : public cyber::Component<prediction::PredictionObstacles,
             canbus::Chassis, localization::LocalizationEstimate>

在PlanningComponent的實現中，會根據具體的配置選擇Planning的入口。Planning的入口通過PlanningBase類來描述的。

PlanningBase只是一個抽象類，該類有三個子類：

OpenSpacePlanning
NaviPlanning
StdPlanning

PlanningComponent::Init()方法中會根據配置選擇具體的Planning入口：

bool PlanningComponent::Init() {
  if (FLAGS_open_space_planner_switchable) {
    planning_base_ = std::make_unique<OpenSpacePlanning>();
  } else {
    if (FLAGS_use_navigation_mode) {
      planning_base_ = std::make_unique<NaviPlanning>();
    } else {
      planning_base_ = std::make_unique<StdPlanning>();
    }
  }

目前，FLAGS_open_space_planner_switchable和FLAGS_use_navigation_mode的配置都是false，因此最終的Planning入口類是：StdPlanning。

下面這幅圖描述了上面說到的這些邏輯：

所以接下來，我們只要關注StdPlanning的實現即可。在這個類中，下面這個方法是及其重要的：

/**
* @brief main logic of the planning module,
* runs periodically triggered by timer.
*/
void RunOnce(const LocalView& local_view,
             ADCTrajectory* const trajectory_pb) override;

方法的註釋已經說明得很清楚了：這是Planning模組的主體邏輯，會被timer以固定的間隔呼叫。每次呼叫就是一個規劃週期。

PlanningCycle

很顯然，接下來我們重點要關注的就是StdPlanning::RunOnce方法的邏輯。該方法的實現較長，這裡就不貼出程式碼了，而是通過一幅圖描述其中的邏輯：

請讀者儘可能仔細的關注一下這幅圖，因為它涵蓋了下文我們要討論的所有內容。

Planner概述

在最新的Apollo原始碼中，一共包含了5個Planner的實現。它們的結構如下圖所示：

每個Planner都會有一個字串描述的唯一型別，在配置檔案中（見下文）通過這個型別來選擇相應的Planner。

這5個Planner的說明如下表所示：

名稱	加入版本	型別	說明
RTKReplayPlanner	1.0	RTK	根據錄製的軌跡來規劃行車路線。
PublicRoadPlanner	1.5	PUBLIC_ROAD	實現了EM演算法的規劃器，這是目前的預設Planner。
LatticePlanner	2.5	LATTICE	基於網格演算法的軌跡規劃器。
NaviPlanner	3.0	NAVI	基於實時相對地圖的規劃器。
OpenSpacePlanner	3.5	OPEN_SPACE	演算法源於論文：《Optimization-Based Collision Avoidance》。

RTKReplayPlanner基於錄製的軌跡，是比較原始的規劃器，所以不用多做說明。最新加入的兩個規劃器（NaviPlanner和OpenSpacePlanner）目前看來還需要更多時間的驗證，我們暫時也不會過多講解。

Apollo公開課裡對兩個較為成熟的Planner：EM Planner和Lattice Planner做了對比，我們可以一起來看一下：

EM Planner	Lattice Planner
橫向縱向分開求解	橫向縱向同時求解
引數較多（DP/QP, Path/Speed）	引數較少且統一
流程複雜	流程簡單
單週期解空間受限	簡單場景解空間較大
能適應複雜場景	適合簡單場景
適合城市道路	適合高速場景

後面的內容中，我們會盡可能集中在EM Planner演算法上。對於Lattice Planner感興趣的讀者可以繼續閱讀這篇文章：《Lattice Planner規劃演算法》。

Planner配置

Planner的配置檔案路徑是在planning_gflags.cc中指定的，相關內容如下：

// /modules/planning/common/planning_gflags.cc

DEFINE_string(planning_config_file,
              "/apollo/modules/planning/conf/planning_config.pb.txt",
              "planning config file");

接下來我們可以看一下planning_config.pb.txt中的內容：

// modules/planning/conf/planning_config.pb.txt

standard_planning_config {
  planner_type: PUBLIC_ROAD
  planner_type: OPEN_SPACE
  planner_public_road_config {
     scenario_type: LANE_FOLLOW
     scenario_type: SIDE_PASS
     scenario_type: STOP_SIGN_UNPROTECTED
  }
}

這裡設定了兩個Planner，最終選擇哪一個由下面這個函式決定：

std::unique_ptr<Planner> StdPlannerDispatcher::DispatchPlanner() {
  PlanningConfig planning_config;
  apollo::common::util::GetProtoFromFile(FLAGS_planning_config_file,
                                         &planning_config);
  if (FLAGS_open_space_planner_switchable) {
    return planner_factory_.CreateObject(
        planning_config.standard_planning_config().planner_type(1));
  }
  return planner_factory_.CreateObject(
      planning_config.standard_planning_config().planner_type(0));
}

open_space_planner_switchable決定了是否能夠切換到OpenSpacePlanner上。但目前這個配置是false：

// /modules/planning/common/planning_gflags.cc

DEFINE_bool(open_space_planner_switchable, false,
            "true for std planning being able to switch to open space planner "
            "when close enough to target parking spot");

PublicRoadPlanner

PublicRoadPlanner是目前預設的Planner，它實現了EM（Expectation Maximization）演算法。

Planner的演算法實現依賴於兩個輸入：

車輛自身狀態：通過TrajectoryPoint描述。該結構中包含了車輛的位置，速度，加速度，方向等資訊。
當前環境資訊：通過Frame描述。前面我們已經提到，Frame中包含了一次Planning計算迴圈中的所有資訊。

在Frame中有一個資料結構值得我們重點關於一下，那就是LocalView。這個類在前面我們也已經提到過。它的定義如下：

struct LocalView {
  std::shared_ptr<prediction::PredictionObstacles> prediction_obstacles;
  std::shared_ptr<canbus::Chassis> chassis;
  std::shared_ptr<localization::LocalizationEstimate> localization_estimate;
  std::shared_ptr<perception::TrafficLightDetection> traffic_light;
  std::shared_ptr<routing::RoutingResponse> routing;
  bool is_new_routing = false;
  std::shared_ptr<relative_map::MapMsg> relative_map;
};

從這個定義中可以看到，這個結構中包含了這些資訊：

障礙物的預測資訊
車輛底盤資訊
大致定位資訊
交通燈資訊
導航路由資訊
相對地圖資訊

對於每個Planner來說，其主要的邏輯都實現在Plan方法中。PublicRoadPlanner::Plan方法的實現邏輯如下：


Status PublicRoadPlanner::Plan(const TrajectoryPoint& planning_start_point,
                               Frame* frame) {
  DCHECK_NOTNULL(frame);
  scenario_manager_.Update(planning_start_point, *frame); ①
  scenario_ = scenario_manager_.mutable_scenario(); ②
  auto result = scenario_->Process(planning_start_point, frame); ③

  ...
  if (result == scenario::Scenario::STATUS_DONE) {
    scenario_manager_.Update(planning_start_point, *frame); ④
  } else if (result == scenario::Scenario::STATUS_UNKNOWN) {
    return Status(common::PLANNING_ERROR, "scenario returned unknown");
  }
  return Status::OK();
}

這段程式碼的幾個關鍵步驟是：

確定當前Scenario：因為Frame中包含了當前狀態的所有資訊，所以通過它就可以確定目前是處於哪一個場景下。
獲取當前Scenario。
通過Scenario進行具體的處理。
如果處理成功，則再次通過ScenarioManager更新。

Scenario是Apollo 3.5上新增的駕駛場景功能。前面在模組架構中我們已經提到過，接下來我們就詳細看一下這方面內容。

Scenario

場景分類

Apollo3.5聚焦在三個主要的駕駛場景，即：

車道保持

車道保持場景是預設的駕駛場景，它不僅僅包含單車道巡航。同時也包含了：

換道行駛
遵循基本的交通約定
基本轉彎

Side Pass

在這種情況下，如果在自動駕駛車輛（ADC）的車道上有靜態車輛或靜態障礙物，並且車輛不能在不接觸障礙物的情況下安全地通過車道，則執行以下策略：

檢查鄰近車道是否接近通行
如果無車輛，進行繞行，繞過當前車道進入鄰道
一旦障礙物安全通過，回到原車道上

停止標識

停止標識有兩種分離的駕駛場景：

1、未保護：在這種情況下，汽車預計會通過具有雙向停車位的十字路口。因此，我們的ADC必須爬過並測量十字路口的交通密度，然後才能繼續走上它的道路。

2、受保護：在此場景中，汽車預期通過具有四向停車位的十字路口導航。我們的ADC將必須對在它之前停下來的汽車進行測量，並在移動之前瞭解它在佇列中的位置。

場景實現

場景的實現主要包含三種類：

ScenarioManager：場景管理器類。負責註冊，選擇和建立Scenario。
Scenario：描述一個特定的場景（例如：Side Pass）。該類中包含了CreateStage方法用來建立Stage。一個Scenario可能有多個Stage物件。在Scenario中會根據配置順序依次呼叫Stage::Process方法。該方法的返回值決定了從一個Stage切換到另外一個Stage。
Stage：如上面所說，一個Scenario可能有多個Stage物件。場景功能實現的主體邏輯通常是在Stage::Process方法中。

場景配置

所有場景都是通過配置檔案來進行配置的。很顯然，首先需要在proto資料夾中定義其結構。

其內容如下所示：

// proto/planning_config.proto
message ScenarioConfig {
  
  message StageConfig {
    optional StageType stage_type = 1;
    optional bool enabled = 2 [default = true];
    repeated TaskConfig.TaskType task_type = 3;
    repeated TaskConfig task_config = 4;
  }

  optional ScenarioType scenario_type = 1;
  oneof scenario_config {
    ScenarioLaneFollowConfig lane_follow_config = 2;
    ScenarioSidePassConfig side_pass_config = 3;
    ScenarioStopSignUnprotectedConfig stop_sign_unprotected_config = 4;
    ScenarioTrafficLightProtectedConfig traffic_light_protected_config = 5;
    ScenarioTrafficLightUnprotectedRightTurnConfig traffic_light_unprotected_right_turn_config = 6;
  }

  repeated StageType stage_type = 7;
  repeated StageConfig stage_config = 8;
}

這裡定義了ScenarioConfig結構，一個ScenarioConfig中可以包含多個StageConfig。

另外，Stage和Scenario都有一個Type欄位，它們的定義如下：

enum ScenarioType {
    LANE_FOLLOW = 0;  // default scenario
    CHANGE_LANE = 1;
    SIDE_PASS = 2;  // go around an object when it blocks the road
    APPROACH = 3;   // approach to an intersection
    STOP_SIGN_PROTECTED = 4;
    STOP_SIGN_UNPROTECTED = 5;
    TRAFFIC_LIGHT_PROTECTED = 6;
    TRAFFIC_LIGHT_UNPROTECTED_LEFT_TURN = 7;
    TRAFFIC_LIGHT_UNPROTECTED_RIGHT_TURN = 8;
}
  
enum StageType {
    NO_STAGE = 0;
    
    LANE_FOLLOW_DEFAULT_STAGE = 1;
    
    STOP_SIGN_UNPROTECTED_PRE_STOP = 100;
    STOP_SIGN_UNPROTECTED_STOP = 101;
    STOP_SIGN_UNPROTECTED_CREEP = 102 ;
    STOP_SIGN_UNPROTECTED_INTERSECTION_CRUISE = 103;
    
    SIDE_PASS_APPROACH_OBSTACLE = 200;
    SIDE_PASS_GENERATE_PATH= 201;
    SIDE_PASS_STOP_ON_WAITPOINT = 202;
    SIDE_PASS_DETECT_SAFETY = 203;
    SIDE_PASS_PASS_OBSTACLE = 204;
    SIDE_PASS_BACKUP = 205;
    
    TRAFFIC_LIGHT_PROTECTED_STOP = 300;
    TRAFFIC_LIGHT_PROTECTED_INTERSECTION_CRUISE = 301;
    
    TRAFFIC_LIGHT_UNPROTECTED_RIGHT_TURN_STOP = 310;
    TRAFFIC_LIGHT_UNPROTECTED_RIGHT_TURN_CREEP = 311 ;
    TRAFFIC_LIGHT_UNPROTECTED_RIGHT_TURN_INTERSECTION_CRUISE = 312;
};

場景註冊

前面我們已經提到，ScenarioManager負責場景的註冊。實際上，註冊的方式就是讀取配置檔案：

void ScenarioManager::RegisterScenarios() {
  CHECK(Scenario::LoadConfig(FLAGS_scenario_lane_follow_config_file,
                             &config_map_[ScenarioConfig::LANE_FOLLOW]));
  CHECK(Scenario::LoadConfig(FLAGS_scenario_side_pass_config_file,
                             &config_map_[ScenarioConfig::SIDE_PASS]));
  CHECK(Scenario::LoadConfig(
      FLAGS_scenario_stop_sign_unprotected_config_file,
      &config_map_[ScenarioConfig::STOP_SIGN_UNPROTECTED]));
  CHECK(Scenario::LoadConfig(
      FLAGS_scenario_traffic_light_protected_config_file,
      &config_map_[ScenarioConfig::TRAFFIC_LIGHT_PROTECTED]));
  CHECK(Scenario::LoadConfig(
      FLAGS_scenario_traffic_light_unprotected_right_turn_config_file,
      &config_map_[ScenarioConfig::TRAFFIC_LIGHT_UNPROTECTED_RIGHT_TURN]));
}

配置檔案在上文中已經全部列出。很顯然，這裡讀取的配置檔案位於/modules/planning/conf/scenario目錄下。

場景確定

下面這個函式用來確定當前所處的場景。前面我們已經說了，確定場景的依據是Frame資料。

void ScenarioManager::Update(const common::TrajectoryPoint& ego_point,
                             const Frame& frame) {

這裡面的邏輯就不過多說明了，讀者可以自行閱讀相關程式碼。

場景配置

場景的配置檔案都位於/modules/planning/conf/scenario目錄下。在配置場景的時候，還會同時為場景配置相應的Task物件。關於這部分內容，在下文講解Task的時候再一起看。

Frenet座標系

大家最熟悉的座標系應該是橫向和縱向垂直的笛卡爾座標系。但是在自動駕駛領域，最常用的卻是Frenet座標系。基於Frenet座標系的動作規劃方法由於是由BMW的Moritz Werling提出的。

之所以這麼做，最主要的原因是因為大部分的道路都不是筆直的，而是具有一定彎曲度的弧線。

在Frenet座標系中，我們使用道路的中心線作為參考線，使用參考線的切線向量t和法線向量n建立一個座標系，如上圖的右圖所示，它以車輛自身為原點，座標軸相互垂直，分為s方向（即沿著參考線的方向，通常被稱為縱向，Longitudinal）和d方向（即參考線當前的法向，被稱為橫向，Lateral），相比於笛卡爾座標系（上圖的左圖），Frenet座標系明顯地簡化了問題。因為在公路行駛中，我們總是能夠簡單的找到道路的參考線（即道路的中心線），那麼基於參考線的位置的表示就可以簡單的使用縱向距離（即沿著道路方向的距離）和橫向距離（即偏離參考線的距離）來描述。

下面這幅圖描述了同樣一個路段，分別在笛卡爾座標系和Frenet座標系下的描述結果。很顯然，Frenet座標系要更容易理解和處理。

ReferenceLine

下面這幅圖是Apollo公開課上對於Planning模組架構的描述。

從這個圖上我們可以看出，參考線是整個決策規劃演算法的基礎。從前面的內容我們也看到了，在Planning模組的每個計算迴圈中，會先生成ReferencePath，然後在這個基礎上進行後面的處理。例如：把障礙物投影到參考線上。

在下面的內容，我們把詳細程式碼貼出來看一下。

ReferenceLineProvider

ReferenceLine由ReferenceLineProvider專門負責生成。這個類的結構如下：

建立ReferenceLine

ReferenceLine是在StdPlanning::InitFrame函式中生成的，相關程式碼如下：

Status StdPlanning::InitFrame(const uint32_t sequence_num,
                              const TrajectoryPoint& planning_start_point,
                              const double start_time,
                              const VehicleState& vehicle_state,
                              ADCTrajectory* output_trajectory) {
  frame_.reset(new Frame(sequence_num, local_view_, planning_start_point,
                         start_time, vehicle_state,
                         reference_line_provider_.get(), output_trajectory));
  ...

  std::list<ReferenceLine> reference_lines;
  std::list<hdmap::RouteSegments> segments;
  if (!reference_line_provider_->GetReferenceLines(&reference_lines,
                                                   &segments)) {

ReferenceLineInfo

在ReferenceLine之外，在common目錄下還有一個結構：ReferenceLineInfo，這個結構才是各個模組實際用到資料結構，它其中包含了ReferenceLine，但還有其他更詳細的資料。

從ReferenceLine到ReferenceLineInfo是在Frame::CreateReferenceLineInfo中完成的。

bool Frame::CreateReferenceLineInfo(
    const std::list<ReferenceLine> &reference_lines,
    const std::list<hdmap::RouteSegments> &segments) {
  reference_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;
  }
  ...

ReferenceLineInfo不僅僅包含了參考線資訊，還包含了車輛狀態，路徑資訊，速度資訊，決策資訊以及軌跡資訊等。Planning模組的演算法很多都是基於ReferenceLineInfo結構完成的。例如下面這個：

bool Stage::ExecuteTaskOnReferenceLine(
    const common::TrajectoryPoint& planning_start_point, Frame* frame) {
  for (auto& reference_line_info : *frame->mutable_reference_line_info()) {
    ...
    
    if (reference_line_info.speed_data().empty()) {
      *reference_line_info.mutable_speed_data() =
          SpeedProfileGenerator::GenerateFallbackSpeedProfile();
      reference_line_info.AddCost(kSpeedOptimizationFallbackCost);
      reference_line_info.set_trajectory_type(ADCTrajectory::SPEED_FALLBACK);
    } else {
      reference_line_info.set_trajectory_type(ADCTrajectory::NORMAL);
    }
    DiscretizedTrajectory trajectory;
    if (!reference_line_info.CombinePathAndSpeedProfile(
            planning_start_point.relative_time(),
            planning_start_point.path_point().s(), &trajectory)) {
      AERROR << "Fail to aggregate planning trajectory.";
      return false;
    }
    reference_line_info.SetTrajectory(trajectory);
    reference_line_info.SetDrivable(true);
    return true;
  }
  return true;
}

Smoother

為了保證車輛軌跡的平順，參考線必須是經過平滑的，目前Apollo中包含了這麼幾個Smoother用來做參考線的平滑：

在實現中，Smoother用到了下面兩個開源庫：

TrafficRule

行駛在城市道路上的自動駕駛車輛必定受到各種交通規則的限制。在正常情況下，車輛不應當違反交通規則。

另外，交通規則通常是多種條例，不同城市和國家地區的交通規則可能是不一樣的。

如果處理好這些交通規則就是模組實現需要考慮的了。目前Planning模組的實現中，有如下這些交通規則的實現：

TrafficRule配置

交通條例的生效並非是一成不變的，因此自然就需要有一個配置檔案來進行配置。交通規則的配置檔案是：modules/planning/conf/traffic_rule_config.pb.txt。

下面是其中的一個程式碼片段：

// modules/planning/conf/traffic_rule_config.pb.txt
...

config: {
  rule_id: SIGNAL_LIGHT
  enabled: true
  signal_light {
    stop_distance: 1.0
    max_stop_deceleration: 6.0
    min_pass_s_distance: 4.0
    max_stop_deacceleration_yellow_light: 3.0
    signal_expire_time_sec: 5.0
    max_monitor_forward_distance: 135.0
    righ_turn_creep {
      enabled: false
      min_boundary_t: 6.0
      stop_distance: 0.5
      speed_limit: 1.0
    }
  }
}

TrafficDecider

TrafficDecider是交通規則處理的入口，它負責讀取上面這個配置檔案，並執行交通規則的檢查。在上文中我們已經看到，交通規則的執行是在StdPlanning::RunOnce中完成的。具體執行的邏輯如下：

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

這段程式碼說明如下：

遍歷配置檔案中的每一條交通規則，判斷是否enable。
建立具體的交通規則物件。
執行該條交通規則邏輯。
在ReferenceLineInfo上合併處理所有交通規則最後的結果。

Task

一直到目前最新的Apollo 3.5版本為止，Planning模組最核心的演算法就是其EM Planner（實現類是PublicRoadPlanner），而EM Planner最核心的就是其決策器和優化器。

但由於篇幅所限，這部分內容本文不再繼續深入。預計後面會再通過一篇文章來講解。這裡我們僅僅粗略的瞭解一下其實現結構。

Planning中這部分邏輯實現位於tasks目錄下，無論是決策器還是優化器都是從apollo::planning::Task繼承的。該類具有下面這些子類：

Task類提供了Execute方法供子類實現，實現依賴的資料結構就是Frame和ReferenceLineInfo。

Status Task::Execute(Frame* frame, ReferenceLineInfo* reference_line_info) {
  frame_ = frame;
  reference_line_info_ = reference_line_info;
  return Status::OK();
}

有興趣的讀者可以通過閱讀子類的Execute方法來了解演算法實現。

Task配置

上文中我們已經提到，場景和Task配置是在一起的。這些配置在下面這些檔案中：

// /modules/planning/conf/scenario
.
├── lane_follow_config.pb.txt
├── side_pass_config.pb.txt
├── stop_sign_unprotected_config.pb.txt
├── traffic_light_protected_config.pb.txt
└── traffic_light_unprotected_right_turn_config.pb.txt

一個Scenario可能有多個Stage，每個Stage可以指定相應的Task，下面是一個配置示例：

// /modules/planning/conf/scenario/lane_follow_config.pb.txt

scenario_type: LANE_FOLLOW
stage_type: LANE_FOLLOW_DEFAULT_STAGE
stage_config: {
  stage_type: LANE_FOLLOW_DEFAULT_STAGE
  enabled: true
  task_type: DECIDER_RULE_BASED_STOP
  task_type: DP_POLY_PATH_OPTIMIZER
  ...

  task_config: {
    task_type: DECIDER_RULE_BASED_STOP
  }
  task_config: {
    task_type: QP_PIECEWISE_JERK_PATH_OPTIMIZER
  }
  task_config: {
    task_type: DP_POLY_PATH_OPTIMIZER
  }
  ...
}

這裡的task_type與Task實現類是一一對應的。

Task讀取

在構造Stage物件的時候，會讀取這裡的配置檔案，然後建立相應的Task：

Stage::Stage(const ScenarioConfig::StageConfig& config) : config_(config) {
  name_ = ScenarioConfig::StageType_Name(config_.stage_type());
  next_stage_ = config_.stage_type();
  std::unordered_map<TaskConfig::TaskType, const TaskConfig*, std::hash<int>>
      config_map;
  for (const auto& task_config : config_.task_config()) {
    config_map[task_config.task_type()] = &task_config;
  }
  for (int i = 0; i < config_.task_type_size(); ++i) {
    auto task_type = config_.task_type(i);
    CHECK(config_map.find(task_type) != config_map.end())
        << "Task: " << TaskConfig::TaskType_Name(task_type)
        << " used but not configured";
    auto ptr = TaskFactory::CreateTask(*config_map[task_type]);
    task_list_.push_back(ptr.get());
    tasks_[task_type] = std::move(ptr);
  }
}

Task執行

Task的執行是在Stage::Process中，通過ExecuteTaskOnReferenceLine完成的。

如果你已經搞不清楚這個邏輯關係，請回到上文看一下

bool Stage::ExecuteTaskOnReferenceLine(
    const common::TrajectoryPoint& planning_start_point, Frame* frame) {
  for (auto& reference_line_info : *frame->mutable_reference_line_info()) {
    ...

    auto ret = common::Status::OK();
    for (auto* task : task_list_) {
      ret = task->Execute(frame, &reference_line_info);
      if (!ret.ok()) {
        AERROR << "Failed to run tasks[" << task->Name()
               << "], Error message: " << ret.error_message();
        break;
      }
    }

    ...
  }
  return true;
}

參考資料與推薦讀物

原文地址：《解析百度Apollo之決策規劃模組》 by 保羅的酒吧

解析百度Apollo之Routing模組
2019-02-07
自動駕駛系統的決策規劃模組介紹
2019-07-18
自動駕駛
【node】模組解析之 http
2019-02-16
HTTP
解析百度Apollo自動駕駛平臺
2018-06-25
自動駕駛
json解析模組
2021-01-03
JSON
Python模組之urllib模組
2020-10-30
Python
python模組之collections模組
2019-01-04
Python
JavaScript 模組化解析
2018-10-22
JavaScript
模組學習之hashlib模組
2024-05-09
模組學習之logging模組
2024-05-09
python基礎之正規表示式和re模組
2020-03-12
Python
React乾貨：SPA單頁如何規劃路由、設計Store、劃分模組、按需載入
2019-01-18
React路由
Dubbo的Remoting模組解析
2023-03-08
REM
前端模組化規範
2019-02-16
前端
JS模組化規範
2018-12-24
JS
JavaScript模組化規範
2018-03-30
JavaScript
Vuex之模組
2018-09-13
Vue
python之模組
2024-11-12
Python
TypeScript 之模組
2021-12-16
TypeScript
元規劃：使用規劃器解決數學問題
2024-07-03
動態規劃之數的劃分
2018-04-29
動態規劃
python–模組之random隨機數模組
2018-10-17
Pythonrandom隨機
python–模組之os操作檔案模組
2018-11-01
Python
postman(一)：主介面模組解析
2018-12-19
Postman
python argparse(引數解析模組)
2019-02-01
Python
webpack核心模組tapable用法解析
2021-03-15
Web
構建dubbo分散式平臺-maven模組規劃和平臺功能導圖
2018-08-30
分散式Maven
解析丨自動駕駛核心技術：感知、決策與執行(中：決策篇)
2020-10-20
自動駕駛
愛爾蘭財政部策劃區塊鏈工作組
2018-03-26
區塊鏈
Commonjs規範與模組化
2019-02-16
JS
CMD 模組定義規範
2019-01-08
機器學習之決策樹
2024-06-09
機器學習
思考工具之決策矩陣 | Untools
2022-06-08
矩陣
apollo之ConfigService服務
2018-09-17
Gse
python模組之configparser
2019-02-16
Python
nodejs之-fs模組
2019-02-16
NodeJS
python模組之hashlib
2019-02-16
Python
python之shutil模組
2018-09-09
Python

解析百度Apollo之決策規劃模組

前言

Apollo系統與Planning模組

模組概述

模組架構

整體Pipeline

基礎資料結構

proto

common

模組配置

Planner

Planning與Planner

PlanningCycle

Planner概述

Planner配置

PublicRoadPlanner

Scenario

場景分類

車道保持

Side Pass

停止標識

場景實現

場景配置

場景註冊

場景確定

場景配置

Frenet座標系

ReferenceLine

ReferenceLineProvider

建立ReferenceLine

ReferenceLineInfo

Smoother

TrafficRule

TrafficRule配置

TrafficDecider

Task

Task配置

Task讀取

Task執行

參考資料與推薦讀物

相關文章