本文是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; } 參考資料與推薦讀物
- ApolloAuto/apollo
- apollo/modules/planning README
- Class Architecture and Overview – Planning Module
- A Survey of Motion Planning and Control Techniques for Self-driving Urban Vehicles
- Optimal Trajectory Generation for Dynamic Street Scenarios in a Frene ́t Frame
- 無人駕駛汽車系統入門——基於Frenet優化軌跡的無人車動作規划方法
- Apollo開發者社區 - Lattice Planner規划算法
- Apollo 2.5版導航模式的使用方法
- Apollo 規划技術詳解
- Apollo 無人駕駛免費入門課程
- Apollo 3.0 Software Architecture
原文地址：《解析百度Apollo之決策規划模塊》 by 保羅的酒吧