自動駕駛運動規劃(Motion Planning)

作者：由半杯茶的小酒杯發表于收藏時間：2020-01-26

1.什麼是Motion Planning

Motion Planning是在遵循道路交通規則的前提下，將自動駕駛車輛從當前位置導航到目的地的一種方法。

在實際開放道理場景下，自動駕駛要處理的場景非常繁雜：空曠的道路場景、與行人、障礙物共用道理的場景、空曠的十字路口、繁忙的十字路口、違反交通規則的行人/車輛、正常行駛的車輛/行人等等。場景雖然複雜，但都可以拆解為一系列簡單行為（behavior）的組合：

將這些簡單的行為（behavior）組合起來，就可以完成複雜的駕駛行為。

2、Motion Planning的約束條件(constraints)

Motion Planning是一個複雜的問題，它的執行過程需要滿足很多約束條件：

2.1 車輛運動學約束

車輛運動受到運動學約束，比如它不能實現瞬時側向移動，前驅的車輛必須依賴前輪的轉向才能實現變道、轉向等操作，在彎道上不能速度過快等等。通常我們採用單車模型（Bicycle Model）對車輛運動進行建模。

2.2 靜態障礙物(Static Obstacle)約束

靜態障礙物（Static Obstacle）是道路上靜止的車輛、路面中間的石墩子等車輛不可行駛的區域。Motion Planning需要避開這些靜態障礙物，避免與它們發生碰撞。解決碰撞的思路大概有兩種：

1）將靜態障礙物（Static Obstacle）在網格佔位圖中表示出來，然後檢測規劃路線是否與靜態障礙物區域相交。

2）將車輛的輪廓擴大，比如擴充套件成一個圓形，然後檢測障礙物是否與Circle發生碰撞。

2.3 動態障礙物約束

Motion Planning要實時處理行人、車輛等各種運動的障礙物，避免與障礙物發生碰撞事故。

2.4 道路交通規則約束

車輛在道路上行駛必須要遵守車道線約束規則（比如左轉專用道只能左轉、實線不能變道、路口必須遵守紅綠燈的指示）和各種標誌標牌的指示。

3、Motion Planning的最佳化目標

瞭解Motion Planning的約束條件之後，需要構造目標最佳化函式，然後最小化目標函式，從而獲得在當前環境下的最優運動軌跡。目標函式的種類有很多，下面列舉一些常用的目標函式。

1）關注路徑長度（Path Length），尋求到達目的地的最短路徑。

$s_f = \int^{s_f}_{s_i}{\sqrt{1+ (\frac{dy}{dx})^2}dx}$

2）關注通行時間（Travel Time），尋求到達目的地的最短時間。

$T_f = \int^{s_f}_{0} {\frac{1}{v(s)}ds}$

3）懲罰偏離參考軌跡和參考速度的行為。

$\int^{s_f}_{0} {||x(s) - x_{ref}(s)||ds}$

$\int^{s_f}_{0} {||v(s) - v_{ref}(s)||ds}$

4）考慮軌跡平滑性（Smoothness）

$\int^{s_f}_{0} {||\dddot{x}(s)||^2ds}$

5）考慮曲率約束（Curvature）

$\int^{s_f}_{0} {||k(s)||^2ds}$

透過組合設計自己的目標最佳化函式，從而獲得較好的Planning效果。

4、分級運動規劃器(Hierarchical Motion Planning)

Motion Planning是一個異常複雜的問題，所以通常我們把它切分為一系列的子問題（Sub Problem）。比如Mission Planner、Behavior Planner、Local Planner、Vehicle Control等。

4.1 Mission Planner

Mission Planner關注High-Level的地圖級別的規劃；透過Graph Based的圖搜尋演算法實現自動駕駛路徑的規劃。

4.2 Behavior Planner

Behavior Planner主要關注交通規則、其它道路交通參與者（腳踏車、行人、社會車輛）等等，決定在在當前場景下應該採取何種操作（如停車讓行、加速透過、避讓行人等等）。

Behavior Planner的實現方式比較常見的有幾種：

有限狀態機(Finite State Machines)、規則匹配系統(Rule Based System)、強化學習系統(Reinforcement Learning)。

有限狀態機中的State是各個行為決策，根據對外界環境的感知和交通規則的約束在各個狀態之間轉換。比如在路口紅綠燈的場景，當路口交通燈為紅色不可通行時，車輛會首先切換到Decelerate to Stop狀態，然後在路口停止線完全停下來，進入Stop狀態，並持續在Stop狀態等待，直至交通燈變為綠色允許車輛通行，車輛進入Track Speed狀態，繼續前行。