復(fù)雜約束下自動駕駛車輛的運動規(guī)劃解析

01什么是MotionPlanning

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

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

將這些簡單的行為(behavior)組合起來，就可以完成復(fù)雜的駕駛行為。

02Motion Planning的約束條件（constraints）

Motion Planning是一個復(fù)雜的問題，它的執(zhí)行過程需要滿足很多約束條件：

2.1 車輛運動學(xué)約束

車輛運動受到運動學(xué)約束，比如它不能實現(xiàn)瞬時側(cè)向移動，前驅(qū)的車輛必須依賴前輪的轉(zhuǎn)向才能實現(xiàn)變道、轉(zhuǎn)向等操作，在彎道上不能速度過快等等。通常我們采用單車模型(Bicycle Model)對車輛運動進行建模。

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

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

1）將靜態(tài)障礙物(Static Obstacle)在網(wǎng)格占位圖中表示出來，然后檢測規(guī)劃路線是否與靜態(tài)障礙物區(qū)域相交。

2）將車輛的輪廓擴大，比如擴展成一個圓形，然后檢測障礙物是否與Circle發(fā)生碰撞。

2.3 動態(tài)障礙物約束

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

2.4 道路交通規(guī)則約束

車輛在道路上行駛必須要遵守車道線約束規(guī)則(比如左轉(zhuǎn)專用道只能左轉(zhuǎn)、實線不能變道、路口必須遵守紅綠燈的指示)和各種標志標牌的指示。

03Motion Planning的優(yōu)化目標

了解Motion Planning的約束條件之后，需要構(gòu)造目標優(yōu)化函數(shù)，然后最小化目標函數(shù)，從而獲得在當前環(huán)境下的最優(yōu)運動軌跡。目標函數(shù)的種類有很多，下面枚舉一些常用的目標函數(shù)。

1）關(guān)注路徑長度(Path Length)，尋求到達目的地的最短路徑。

2）關(guān)注通行時間(Travel Time)，尋求到達目的地的最短時間。

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

4）考慮軌跡平滑性(Smoothness)

5）考慮曲率約束(Curvature)

通過組合設(shè)計自己的目標優(yōu)化函數(shù)，從而獲得較好的Planning效果。

04分級運動規(guī)劃器

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

4.1 Mission Planner

Mission Planner關(guān)注High-Level的地圖級別的規(guī)劃；通過Graph Based的圖搜索算法實現(xiàn)自動駕駛路徑的規(guī)劃。

4.2 Behavior Planner

Behavior Planner主要關(guān)注交通規(guī)則、其它道路交通參與者(自行車、行人、社會車輛)等等，決定在在當前場景下應(yīng)該采取何種操作(如停車讓行、加速通過、避讓行人等等)。

Behavior Planner的實現(xiàn)方式比較常見的有幾種：有限狀態(tài)機(Finite State Machines)、規(guī)則匹配系統(tǒng)(Rule Based System)、強化學(xué)習(xí)系統(tǒng)(Reinforcement Learning)。

有限狀態(tài)機中的State是各個行為決策，根據(jù)對外界環(huán)境的感知和交通規(guī)則的約束在各個狀態(tài)之間轉(zhuǎn)換。比如在路口紅綠燈的場景，當路口交通燈為紅色不可通行時，車輛會首先切換到Decelerate to Stop狀態(tài)，然后在路口停止線完全停下來，進入Stop狀態(tài)，并持續(xù)在Stop狀態(tài)等待，直至交通燈變?yōu)榫G色允許車輛通行，車輛進入Track Speed狀態(tài)，繼續(xù)前行。

Rule-Based System是通過一系列的分級的規(guī)則匹配來決定下一步的決策行為。比如交通燈綠色->通行；交通燈紅色->停車等待。

基于強化學(xué)習(xí)的Behavior Planner系統(tǒng)如下：

4.3 Local Planner

Local Planner關(guān)注如何生成舒適的、碰撞避免的行駛路徑和舒適的運動速度，所以Local Planner又可以拆分為兩個子問題：Path Planner和Velocity Profile Generation。Path Planner又分為Sampling-Based Planner、Variational Planner和Lattice Planner。

最經(jīng)典的Sampling-Based Planner算法是Rapidly Exploring Random Tree，RRT算法。

Variational Planner根據(jù)Cost Function進行優(yōu)化調(diào)整，從而避開障礙物，生成安全的軌跡。

Lattice Planner將空間搜索限制在對車輛可行的Action Space。

Velocity Profile Generation要考慮到限速、速度的平滑性等。

Vehicle Control將Planner的規(guī)劃結(jié)果轉(zhuǎn)化為車輛的運動行為。

審核編輯 ：李倩