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

01什么是MotionPlanning

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

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

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

02Motion Planning的約束條件（constraints）

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

2.1 車輛運(yùn)動(dòng)學(xué)約束

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

5）考慮曲率約束(Curvature)

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

04分級(jí)運(yùn)動(dòng)規(guī)劃器

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

4.1 Mission Planner

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

4.2 Behavior Planner

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

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

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

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

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

4.3 Local Planner

Local Planner關(guān)注如何生成舒適的、碰撞避免的行駛路徑和舒適的運(yùn)動(dòng)速度，所以Local Planner又可以拆分為兩個(gè)子問題：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進(jìn)行優(yōu)化調(diào)整，從而避開障礙物，生成安全的軌跡。

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

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

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

審核編輯 ：李倩