自動(dòng)駕駛中的決策規(guī)劃模塊是衡量和評(píng)價(jià)自動(dòng)駕駛能力最核心的指標(biāo)之一，它的主要任務(wù)是在接收到傳感器的各種感知信息之后，對(duì)當(dāng)前環(huán)境作出分析，然后對(duì)底層控制模塊下達(dá)指令。典型的決策規(guī)劃模塊可以分為三個(gè)層次：全局路徑規(guī)劃、行為決策、運(yùn)動(dòng)規(guī)劃。

引言

在一套完整的自動(dòng)駕駛系統(tǒng)中，如果將感知模塊比作人的眼睛和耳朵，那么決策規(guī)劃就是自動(dòng)駕駛的大腦。大腦在接收到傳感器的各種感知信息之后，對(duì)當(dāng)前環(huán)境作出分析，然后對(duì)底層控制模塊下達(dá)指令，這一過(guò)程就是決策規(guī)劃模塊的主要任務(wù)。同時(shí)，決策規(guī)劃模塊可以處理多么復(fù)雜的場(chǎng)景，也是衡量和評(píng)價(jià)自動(dòng)駕駛能力最核心的指標(biāo)之一[1]。

圖1. ?自動(dòng)駕駛系統(tǒng)中的決策規(guī)劃模塊分層結(jié)構(gòu)，引用自[2]

如圖1所示，典型的決策規(guī)劃模塊可以分為三個(gè)層次。

其中，全局路徑規(guī)劃（Route Planning）在接收到一個(gè)給定的行駛目的地之后，結(jié)合地圖信息，生成一條全局的路徑，作為為后續(xù)具體路徑規(guī)劃的參考；

行為決策層（Behavioral Layer）在接收到全局路徑后，結(jié)合從感知模塊得到的環(huán)境信息（包括其他車輛與行人，障礙物，以及道路上的交通規(guī)則信息），作出具體的行為決策（例如選擇變道超車還是跟隨）；

最后，運(yùn)動(dòng)規(guī)劃（Motion Planning）層根據(jù)具體的行為決策，規(guī)劃生成一條滿足特定約束條件（例如車輛本身的動(dòng)力學(xué)約束、避免碰撞、乘客舒適性等）的軌跡，該軌跡作為控制模塊的輸入決定車輛最終行駛路徑。

本文將分別介紹各層的主要作用與常見算法，并且比較各種算法的優(yōu)劣性及適用情景。

全局路徑規(guī)劃（Route Planning）?

全局路徑規(guī)劃是指在給定車輛當(dāng)前位置與終點(diǎn)目標(biāo)后，通過(guò)搜索選擇一條最優(yōu)的路徑，這里的“最優(yōu)”包括路徑最短，或者到達(dá)時(shí)間最快等條件。這一過(guò)程類似于我們生活中經(jīng)常用到的“導(dǎo)航”功能，區(qū)別在于自動(dòng)駕駛中使用的高精地圖與我們常見的地圖不太一樣，在高精地圖中包含了每條車道在內(nèi)的更多信息。常見的全局路徑規(guī)劃算法包括Dijkstra和A算法，以及在這兩種算法基礎(chǔ)上的多種改進(jìn)。Dijkstra算法[3]和A*算法[4]也是在許多規(guī)劃問(wèn)題中應(yīng)用最為廣泛的兩種搜索算法。

Dijkstra算法是由計(jì)算機(jī)科學(xué)家Edsger W. Dijkstra在1956年提出，用來(lái)尋找圖形中節(jié)點(diǎn)之間的最短路徑。在Dijkstra算法中，需要計(jì)算每一個(gè)節(jié)點(diǎn)距離起點(diǎn)的總移動(dòng)代價(jià)。同時(shí)，還需要一個(gè)優(yōu)先隊(duì)列結(jié)構(gòu)。對(duì)于所有待遍歷的節(jié)點(diǎn)，放入優(yōu)先隊(duì)列中會(huì)按照代價(jià)進(jìn)行排序。在算法運(yùn)行的過(guò)程中，每次都從優(yōu)先隊(duì)列中選出代價(jià)最小的作為下一個(gè)遍歷的節(jié)點(diǎn)。直到到達(dá)終點(diǎn)為止。

Dijkstra算法的優(yōu)點(diǎn)是：給出的路徑是最優(yōu)的；缺點(diǎn)是計(jì)算時(shí)間復(fù)雜度比較高（O(N2)）,因?yàn)槭窍蛑車M(jìn)行探索，沒(méi)有明確的方向。

2. A*算法

為了解決Dijkstra算法的搜索效率問(wèn)題，1968年，A算法由Stanford研究院的Peter Hart, Nils Nilsson以及Bertram Raphael發(fā)表，其主要改進(jìn)是借助一個(gè)啟發(fā)函數(shù)來(lái)引導(dǎo)搜索的過(guò)程。具體來(lái)說(shuō)，A算法通過(guò)下面這個(gè)函數(shù)來(lái)計(jì)算每個(gè)節(jié)點(diǎn)的優(yōu)先級(jí):

f(n)=g(n)+h(n)

其中：

f(n) 是節(jié)點(diǎn)n的綜合優(yōu)先級(jí)。當(dāng)我們選擇下一個(gè)要遍歷的節(jié)點(diǎn)時(shí)，我們總會(huì)選取綜合優(yōu)先級(jí)最高（值最小）的節(jié)點(diǎn)。

g(n)是節(jié)點(diǎn)n距離起點(diǎn)的代價(jià)。

h(n) 是節(jié)點(diǎn)n距離終點(diǎn)的預(yù)計(jì)代價(jià)，這也就是A*算法的啟發(fā)函數(shù)。

行為決策（Behavioral Layer）

在確定全局路徑之后，自動(dòng)駕駛車輛需要根據(jù)具體的道路狀況、交通規(guī)則、其他車輛與行人等情況作出合適的行為決策。

這一過(guò)程面臨三個(gè)主要問(wèn)題：

首先，真實(shí)的駕駛場(chǎng)景千變?nèi)f化，如何覆蓋？

其次，真實(shí)的駕駛場(chǎng)景是一個(gè)多智能體決策環(huán)境，包括主車在內(nèi)的每一個(gè)參與者所做出的行為，都會(huì)對(duì)環(huán)境中的其他參與者帶來(lái)影響，因此我們需要對(duì)環(huán)境中其他參與者的行為進(jìn)行預(yù)測(cè)；

最后，自動(dòng)駕駛車輛對(duì)于環(huán)境信息不可能做到100%的感知，例如存在許多被障礙物遮擋的可能危險(xiǎn)情形。

綜合以上幾點(diǎn)，在自動(dòng)駕駛行為決策層，我們需要解決的是在多智能體決策的復(fù)雜環(huán)境中，存在感知不確定性情況的規(guī)劃問(wèn)題。可以說(shuō)這一難題是真正實(shí)現(xiàn)L4、L5級(jí)別自動(dòng)駕駛技術(shù)的核心瓶頸之一，近年來(lái)隨著深度強(qiáng)化學(xué)習(xí)等領(lǐng)域的快速發(fā)展，為解決這一問(wèn)題帶來(lái)了新的思路和曙光。

以下將行為決策層的模型分為四類分別介紹[5]：

1. 有限狀態(tài)機(jī)模型

自動(dòng)駕駛車輛最開始的決策模型為有限狀態(tài)機(jī)模型[6]，車輛根據(jù)當(dāng)前環(huán)境選擇合適的駕駛行為，如停車、換道、超車、避讓、緩慢行駛等模式，狀態(tài)機(jī)模型通過(guò)構(gòu)建有限的有向連通圖來(lái)描述不同的駕駛狀態(tài)以及狀態(tài)之間的轉(zhuǎn)移關(guān)系，從而根據(jù)駕駛狀態(tài)的遷移反應(yīng)式地生成駕駛動(dòng)作。

有限狀態(tài)機(jī)模型因?yàn)楹?jiǎn)單、易行，是無(wú)人駕駛領(lǐng)域目前最廣泛的行為決策模型，但該類模型忽略了環(huán)境的動(dòng)態(tài)性和不確定性，此外，當(dāng)駕駛場(chǎng)景特征較多時(shí)，狀態(tài)的劃分和管理比較繁瑣，多適用于簡(jiǎn)單場(chǎng)景下，很難勝任具有豐富結(jié)構(gòu)化特征的城區(qū)道路環(huán)境下的行為決策任務(wù)。

2. 決策樹模型

決策/行為樹模型[7]和狀態(tài)機(jī)模型類似，也是通過(guò)當(dāng)前駕駛狀態(tài)的屬性值反應(yīng)式地選擇不同的駕駛動(dòng)作，但不同的是該類模型將駕駛狀態(tài)和控制邏輯固化到了樹形結(jié)構(gòu)中，通過(guò)自頂向下的“輪詢”機(jī)制進(jìn)行駕駛策略搜索。這類決策模型具備可視化的控制邏輯，并且控制節(jié)點(diǎn)可復(fù)用，但需要針對(duì)每個(gè)駕駛場(chǎng)景離線定義決策網(wǎng)路，當(dāng)狀態(tài)空間、行為空間較大時(shí)，控制邏輯將比較復(fù)雜。另外，該類模型同樣無(wú)法考慮交通環(huán)境中存在的不確定性因素。

3. 基于知識(shí)的推理決策模型

基于知識(shí)的推理決策模型由“場(chǎng)景特征－駕駛動(dòng)作”的映射關(guān)系來(lái)模仿人類駕駛員的行為決策過(guò)程，該類模型將駕駛知識(shí)存儲(chǔ)在知識(shí)庫(kù)或者神經(jīng)網(wǎng)絡(luò)中，這里的駕駛知識(shí)主要表現(xiàn)為規(guī)則、案例或場(chǎng)景特征到駕駛動(dòng)作的映射關(guān)系。進(jìn)而，通過(guò)“查詢”機(jī)制從知識(shí)庫(kù)或者訓(xùn)練過(guò)的網(wǎng)絡(luò)結(jié)構(gòu)中推理出駕駛動(dòng)作。

該類模型主要包括：基于規(guī)則的推理系統(tǒng)[8]、基于案例的推理系統(tǒng)[9]和基于神經(jīng)網(wǎng)絡(luò)的映射模型[10]。

該類模型對(duì)先驗(yàn)駕駛知識(shí)、訓(xùn)練數(shù)據(jù)的依賴性較大，需要對(duì)駕駛知識(shí)進(jìn)行精心整理、管理和更新，雖然基于神經(jīng)網(wǎng)絡(luò)的映射模型可以省去數(shù)據(jù)標(biāo)注和知識(shí)整合的過(guò)程，但是仍然存在以下缺點(diǎn)：

其“數(shù)據(jù)”驅(qū)動(dòng)機(jī)制使得其對(duì)訓(xùn)練數(shù)據(jù)的依賴性較大，訓(xùn)練數(shù)據(jù)需要足夠充分[11]；

將映射關(guān)系固化到網(wǎng)絡(luò)結(jié)構(gòu)中，其解釋性較差；

存在“黑箱”問(wèn)題，透明性差，對(duì)于實(shí)際系統(tǒng)中出現(xiàn)的問(wèn)題可追溯性較差，很難發(fā)現(xiàn)問(wèn)題的根本原因。

4. 基于價(jià)值的決策模型

根據(jù)最大效用理論，基于效用／價(jià)值的決策模型的基本思想是依據(jù)選擇準(zhǔn)則在多個(gè)備選方案中選擇出最優(yōu)的駕駛策略／動(dòng)作[12]。

為了評(píng)估每個(gè)駕駛動(dòng)作的好壞程度，該類模型定義了效用（utility）或價(jià)值（value）函數(shù)，根據(jù)某些準(zhǔn)則屬性定量地評(píng)估駕駛策略符合駕駛?cè)蝿?wù)目標(biāo)的程度，對(duì)于無(wú)人駕駛?cè)蝿?wù)而言，這些準(zhǔn)則屬性可以是安全性、舒適度、行車效率等，效用和價(jià)值可以是由其中單個(gè)屬性決定也可以是由多個(gè)屬性決定。

澳大利亞格里菲斯大學(xué)的Furda和Vlacic提出了多準(zhǔn)則決策方法從候選動(dòng)作集中選擇最優(yōu)的駕駛動(dòng)作[13]；新加坡國(guó)立大學(xué)的Bandyopadhyay等人提出了基于POMDP的行為決策模型[14]，用以解決存在感知不確定性的情況；卡內(nèi)基梅隆大學(xué)的Wei J等人提出基于PCB（Prediction and-Cost-function Based）的行為決策模型[15]，其側(cè)重點(diǎn)在于如何構(gòu)建恰當(dāng)?shù)拇鷥r(jià)函數(shù)來(lái)指導(dǎo)對(duì)環(huán)境的預(yù)測(cè)；為了解決在多智能體參與的復(fù)雜環(huán)境中的決策問(wèn)題，許多基于博弈論的模型也被研究者用來(lái)推理車輛之間的交互行為[16]，[17]；此外，因?yàn)樵谔卣魈崛》矫娴膬?yōu)勢(shì)，深度強(qiáng)化學(xué)習(xí)技術(shù)也開始被廣泛應(yīng)用，以完成最優(yōu)駕駛動(dòng)作的生成[18]。

運(yùn)動(dòng)規(guī)劃（Motion Planning）

在確定具體的駕駛行為之后，我們需要做的是將“行為”轉(zhuǎn)化成一條更加具體的行駛“軌跡”，從而能夠最終生成對(duì)車輛的一系列具體控制信號(hào)，實(shí)現(xiàn)車輛按照規(guī)劃目標(biāo)的行駛。這一過(guò)程稱為運(yùn)動(dòng)規(guī)劃（Motion Planning），運(yùn)動(dòng)規(guī)劃的概念在機(jī)器人領(lǐng)域已經(jīng)有較長(zhǎng)時(shí)間的研究歷史，我們可以從數(shù)學(xué)的角度將它看做如下的一個(gè)優(yōu)化問(wèn)題：

路徑規(guī)劃（Path Planning）

圖3. 路徑規(guī)劃的定義

在以機(jī)器人為代表的許多場(chǎng)景中，我們可以認(rèn)為周圍的環(huán)境是確定的。在這種情況下，所謂的路徑規(guī)劃，是指在給定的一個(gè)狀態(tài)空間Χ，尋找一個(gè)滿足一定約束條件的映射σ:[0,1]?Χ，這些約束包括：

確定的起始狀態(tài)以及目標(biāo)點(diǎn)所在的區(qū)域

避免碰撞

對(duì)路徑的微分約束（例如在實(shí)際問(wèn)題中路徑曲率不能太小，對(duì)應(yīng)于其二階導(dǎo)數(shù)的約束）

該優(yōu)化問(wèn)題的目標(biāo)泛函定義為J(σ)，其具體意義可以表示為路徑長(zhǎng)度、控制復(fù)雜度等衡量標(biāo)準(zhǔn)。

然而在自動(dòng)駕駛問(wèn)題中，車輛周圍的環(huán)境是持續(xù)動(dòng)態(tài)變化的，因此單純的路徑規(guī)劃不能給出在行駛過(guò)程中一直有效的解，因此我們需要增加一個(gè)維度——時(shí)間T，相應(yīng)的規(guī)劃問(wèn)題通常被稱為軌跡規(guī)劃。

軌跡規(guī)劃（Trajectory Planning）

圖4. 軌跡規(guī)劃的定義

時(shí)間維度的增加為規(guī)劃問(wèn)題帶來(lái)了巨大的挑戰(zhàn)。例如，對(duì)于一個(gè)在2D環(huán)境中移動(dòng)一個(gè)抽象為單點(diǎn)的機(jī)器人，環(huán)境中的障礙物近似為多邊形的問(wèn)題。路徑規(guī)劃問(wèn)題可以在多項(xiàng)式時(shí)間內(nèi)求解，而加入時(shí)間維度的軌跡規(guī)劃問(wèn)題已經(jīng)被證明是NP-hard問(wèn)題。

在自動(dòng)駕駛的實(shí)際場(chǎng)景中，無(wú)論是對(duì)車輛本身還是對(duì)周圍環(huán)境，建立更為精確的模型意味著對(duì)優(yōu)化問(wèn)題更為復(fù)雜的約束，同時(shí)也意味著求解的更加困難。因此實(shí)際采用的算法都是建立在對(duì)實(shí)際場(chǎng)景的近似前提下，在模型精確性和求解效率二者之間尋求一個(gè)最佳的平衡點(diǎn)。

下文對(duì)自動(dòng)駕駛領(lǐng)域目前常見的幾類運(yùn)動(dòng)規(guī)劃算法分別進(jìn)行介紹，在實(shí)際中，往往是其中幾類思想的結(jié)合才能最終達(dá)到比較好的規(guī)劃結(jié)果，并滿足更多的不同場(chǎng)景。

1. 基于搜索的規(guī)劃算法

通過(guò)搜索來(lái)解決運(yùn)動(dòng)規(guī)劃問(wèn)題是最樸素的思路之一,其基本思想是將狀態(tài)空間通過(guò)確定的方式離散成一個(gè)圖,然后利用各種啟發(fā)式搜索算法搜索可行解甚至是最優(yōu)解。? ? ??

在將狀態(tài)空間離散化的過(guò)程中，需要注意的是確保最終形成的柵格具有最大的覆蓋面積，同時(shí)不會(huì)重復(fù)。如圖5所示，左邊的柵格是由直行、左轉(zhuǎn)90°、右轉(zhuǎn)90°這三種行為生成；而如果選擇直行、左轉(zhuǎn)89°、右轉(zhuǎn)89°三種行為，最后就無(wú)法生成一個(gè)覆蓋全部區(qū)域的柵格結(jié)構(gòu)。

圖5. 構(gòu)建柵格圖，引用自[2]

在將狀態(tài)空間柵格化之后，我們就可以使用前文已經(jīng)介紹的Dijkstra、A*搜索算法，完成最終的規(guī)劃。然而在實(shí)際復(fù)雜環(huán)境中，柵格數(shù)目眾多，并且環(huán)境隨時(shí)間動(dòng)態(tài)變化，會(huì)導(dǎo)致搜索結(jié)點(diǎn)過(guò)多，因此發(fā)展出了多種改進(jìn)算法，用以處理不同的具體場(chǎng)景：

1） Hybrid A* 算法，在A*算法的基礎(chǔ)上考慮了車的最大轉(zhuǎn)向問(wèn)題，例如限定計(jì)算的路徑上車最大轉(zhuǎn)向不超過(guò)5°。該算法目前的應(yīng)用場(chǎng)景有車掉頭（Stanford 參加DARPA 挑戰(zhàn)賽使用的Junior車采用了該算法進(jìn)行uturn），泊車等等對(duì)方向盤控制要求較高的場(chǎng)景。

2）D*、D*Lite算法，事先由終點(diǎn)向起點(diǎn)進(jìn)行搜索，使用Dijkstra算法，存儲(chǔ)路網(wǎng)中目標(biāo)點(diǎn)到每個(gè)點(diǎn)的最短路徑長(zhǎng)度k, 和該節(jié)點(diǎn)到目標(biāo)點(diǎn)的實(shí)際長(zhǎng)度值h，開始情況下 k==h， 并且存儲(chǔ)每個(gè)節(jié)點(diǎn)的上一個(gè)節(jié)點(diǎn)，保證能夠沿著鏈接走下去。

計(jì)算結(jié)束后，獲取了一條當(dāng)時(shí)最優(yōu)路徑。當(dāng)車行駛到某個(gè)節(jié)點(diǎn)時(shí)，通過(guò)傳感器發(fā)現(xiàn)該節(jié)點(diǎn)已經(jīng)無(wú)法通行（有障礙物），則對(duì)已存儲(chǔ)的路網(wǎng)信息一些相關(guān)點(diǎn)的h值進(jìn)行修改（變大），選擇一個(gè)鄰居點(diǎn)滿足仍然h==k的，即仍然是最優(yōu)路徑上的點(diǎn)，作為下一個(gè)點(diǎn)。

然后走到終點(diǎn)。該類算法適用于在未知環(huán)境中的導(dǎo)航以及路徑規(guī)劃，廣泛用于目前各種移動(dòng)機(jī)器人和自主車輛載具，例如“機(jī)遇號(hào)”和“勇氣號(hào)”火星車。

2. 基于采樣的規(guī)劃算法

通過(guò)對(duì)連續(xù)的狀態(tài)空間進(jìn)行采樣，從而將原問(wèn)題近似成一個(gè)離散序列的優(yōu)化問(wèn)題，這一思路也是在計(jì)算機(jī)科學(xué)中應(yīng)用最為廣泛的算法。在運(yùn)動(dòng)規(guī)劃問(wèn)題中，基于采樣的基本算法包括概率路線圖(PRM)和快速搜索隨機(jī)樹(RRT)算法。

圖6. 利用RRT算法實(shí)現(xiàn)u形彎的軌跡規(guī)劃，引用自[19]

1）基本算法：概率路線圖(PRM)

預(yù)處理階段：對(duì)狀態(tài)空間內(nèi)的安全區(qū)域均勻隨機(jī)采樣n個(gè)點(diǎn)，每個(gè)采樣點(diǎn)分別與一定距離內(nèi)的鄰近采樣點(diǎn)連接，并丟棄掉與障礙物發(fā)生碰撞的軌跡,最終得到一個(gè)連通圖。

査詢階段：對(duì)于給定的一對(duì)初始和目標(biāo)狀態(tài),分別將其連接到已經(jīng)構(gòu)建的圖中,再使用搜索算法尋找滿足要求的軌跡。

容易看出,一旦構(gòu)建一個(gè)PRM之后,可以用于解決不同初始、目標(biāo)狀態(tài)的運(yùn)動(dòng)規(guī)劃問(wèn)題,但是這個(gè)特性對(duì)于自動(dòng)駕駛運(yùn)動(dòng)規(guī)劃而言是不必要的。另外PRM要求對(duì)狀態(tài)之間作精確連接，這對(duì)于存在復(fù)雜微分約束的運(yùn)動(dòng)規(guī)劃問(wèn)題是十分困難的。

2）?基本算法：快速搜索隨機(jī)樹(RRT)

樹的初始化：初始化樹的結(jié)點(diǎn)集和邊集,結(jié)點(diǎn)集只包含初始狀態(tài),邊集為空。

樹的生長(zhǎng)：對(duì)狀態(tài)空間隨機(jī)采樣，當(dāng)采樣點(diǎn)落在狀態(tài)空間安全區(qū)域時(shí)，選擇當(dāng)前樹中離采樣點(diǎn)最近的結(jié)點(diǎn)，將其向采樣點(diǎn)擴(kuò)展(或連接)。若生成的軌跡不與障礙物發(fā)生碰撞，則將該軌跡加入樹的邊集，該軌跡的終點(diǎn)加入到樹的結(jié)點(diǎn)集。

RRT是一種增量式采樣的搜索方法，無(wú)須設(shè)置任何分辨率參數(shù)。在極限情況，該搜索樹將稠密的布滿整個(gè)空間，此時(shí)搜索樹由很多較短曲線或路經(jīng)構(gòu)成，以實(shí)現(xiàn)充滿整個(gè)空間的目的。

3）多種改進(jìn)算法

從以上基礎(chǔ)算法的描述我們可以了解到，對(duì)狀態(tài)空間進(jìn)行采樣，可以保證得到連接起始點(diǎn)與終點(diǎn)的可行解，但由于采樣過(guò)程是對(duì)整個(gè)空間進(jìn)行均勻采樣，因此效率很低；在復(fù)雜場(chǎng)景下無(wú)法實(shí)現(xiàn)實(shí)時(shí)求解；此外，最終規(guī)劃結(jié)果無(wú)法保證得到的可行解是最優(yōu)解。針對(duì)這些劣勢(shì)，多種改進(jìn)算法被提出并應(yīng)用于自動(dòng)駕駛問(wèn)題：

效率改進(jìn)--不均勻采樣

- RRT-Connect：同時(shí)構(gòu)建兩棵分別起始于初始狀態(tài)和目標(biāo)狀態(tài)的樹,當(dāng)兩棵樹生長(zhǎng)到一起時(shí)則找到可行解。

-? 啟發(fā)式（hRRT）：使用啟發(fā)式函數(shù)增加擴(kuò)展代價(jià)低的結(jié)點(diǎn)被采樣的概率。

-? 結(jié)合駕駛員模型：結(jié)合駕駛員視覺注意力模型進(jìn)行偏向性采樣，利用視覺特征信息引導(dǎo)運(yùn)動(dòng)規(guī)劃，使規(guī)劃出的軌跡更符合人類駕駛行為。

-? 構(gòu)造新度量RG-RRT(reachability guided RT)：常規(guī)類歐式距離的度量并不能真實(shí)反映構(gòu)形或狀態(tài)之間的遠(yuǎn)近，RG-RRT計(jì)算樹中結(jié)點(diǎn)的能達(dá)集，當(dāng)采樣點(diǎn)到結(jié)點(diǎn)的距離大于采樣點(diǎn)到該結(jié)點(diǎn)能達(dá)集的距離時(shí), 該節(jié)點(diǎn)才有可能被選中進(jìn)行擴(kuò)展。

-? 加入障礙物懲罰（RC-RRT、EG-RRT、ADD-RRT等）：降低靠近障礙物的結(jié)點(diǎn)獲得擴(kuò)展的概率。

實(shí)時(shí)性改進(jìn)

- anytime RRT先快速構(gòu)建一個(gè)RRT,獲得一個(gè)可行解并記錄其代價(jià).之后算法會(huì)繼續(xù)采樣,但僅將有利于降低可行解代價(jià)的結(jié)點(diǎn)插入樹中,從而逐漸獲得較優(yōu)的可行解.。

-? Replanning將整個(gè)規(guī)劃任務(wù)分解為若干等時(shí)間的子任務(wù)序列，在執(zhí)行當(dāng)前任務(wù)的同時(shí)規(guī)劃下一個(gè)任務(wù)。

最優(yōu)性改進(jìn)

- PRM*、RRG、RRT*：根據(jù)隨機(jī)幾何圖理論（在狀態(tài)空間中隨機(jī)采樣m個(gè)點(diǎn),并將距離小于r(n)的點(diǎn)連起來(lái),就構(gòu)成了隨機(jī)幾何圖）對(duì)標(biāo)準(zhǔn)PRM 和RRT做出改進(jìn),得到了具有漸近最優(yōu)性質(zhì)的PRM*、RRG和RRT*算法

3. 直接優(yōu)化方法

在絕大多數(shù)情況下，不考慮高度的變化，自動(dòng)駕駛的軌跡規(guī)劃問(wèn)題是一個(gè)三維約束優(yōu)化問(wèn)題（2D空間+時(shí)間T），因此，我們可以采用解耦的策略，將原始問(wèn)題分解為幾個(gè)低維問(wèn)題，從而大大降低求解難度。

1）Frenet坐標(biāo)系

圖7. Frenet坐標(biāo)系

由于真實(shí)世界中的道路都是彎曲的，為了簡(jiǎn)化求解優(yōu)化問(wèn)題的參數(shù)表達(dá)，在自動(dòng)駕駛中通常采用Frenet坐標(biāo)系。

在Frenet坐標(biāo)系中，我們使用道路的中心線作為參考線，使用參考線的切線向量t和法線向量n建立一個(gè)坐標(biāo)系，如右圖所示，它以車輛自身為原點(diǎn)，坐標(biāo)軸相互垂直，分為s方向（即沿著參考線的方向，通常被稱為縱向，Longitudinal）和d方向（或L方向，即參考線當(dāng)前的法向，被稱為橫向，Lateral），相比于笛卡爾坐標(biāo)系（左圖），F(xiàn)renet坐標(biāo)系明顯地簡(jiǎn)化了問(wèn)題。

因?yàn)樵诠沸旭傊?，我們總是能夠?jiǎn)單的找到道路的參考線（即道路的中心線），那么基于參考線的位置的表示就可以簡(jiǎn)單的使用縱向距離S（即沿著道路方向的距離）和橫向距離L（即偏離參考線的距離）來(lái)描述。

2）路徑-速度解耦法

在Frenet坐標(biāo)系中，路徑-速度解耦法分別優(yōu)化路徑和速度，路徑優(yōu)化主要考慮靜態(tài)障礙物，通過(guò)動(dòng)態(tài)規(guī)劃生成一條靜態(tài)的參考路徑（SL維度），接著基于生成的路徑，考慮對(duì)速度的規(guī)劃（ST維度）。這一過(guò)程可以不斷迭代，從而實(shí)現(xiàn)對(duì)軌跡的實(shí)時(shí)更新。在百度的開源自動(dòng)駕駛平臺(tái)Apollo中采用的EM planner就是基于類似的解決思路。這一方案具有較強(qiáng)的靈活性，可以普適性的適用于許多場(chǎng)景。

除此之外，也可以選擇不同的解耦方式，例如分別對(duì)縱向軌跡（ST維度）和橫向軌跡（LT維度）進(jìn)行規(guī)劃。但需要指出的是，通過(guò)解耦的方法得到的解可能不是最優(yōu)的，并且這種算法不具備完備性，在一些復(fù)雜環(huán)境可能找不到可行解。

4. 參數(shù)化曲線構(gòu)造法

圖8. 常見的參數(shù)化曲線構(gòu)造法，引用自[19]

參數(shù)化曲線構(gòu)造法的出發(fā)點(diǎn)是車輛本身的約束，包括運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)的約束，因此一般規(guī)劃的路徑需要是曲率連續(xù)的。這類方法根據(jù)起始點(diǎn)和目標(biāo)點(diǎn)，考慮障礙物，通過(guò)構(gòu)造一族符合車輛約束的曲線給出一條平滑路徑。

如圖8所示，常見的曲線有Dubins曲線（由直線和圓弧構(gòu)成，是一種簡(jiǎn)單車輛模型Dubin模型在二維空間中的最短曲線族），回旋曲線，多項(xiàng)式曲線，貝塞爾曲線，樣條曲線等。單純應(yīng)用參數(shù)化曲線構(gòu)造法很難滿足實(shí)際復(fù)雜場(chǎng)景，因此現(xiàn)在越來(lái)越多的自動(dòng)駕駛系統(tǒng)將其與其他方法結(jié)合，用來(lái)對(duì)已經(jīng)規(guī)劃生成的軌跡做平滑處理，以滿足車輛運(yùn)動(dòng)學(xué)與動(dòng)力學(xué)約束。

5. 人工勢(shì)場(chǎng)法

人工勢(shì)場(chǎng)法是受物理學(xué)中電磁場(chǎng)的啟發(fā)，假設(shè)障礙物和目標(biāo)位置分別產(chǎn)生斥力和引力，從而可以沿著勢(shì)場(chǎng)的最速梯度下降去規(guī)劃路徑。這類方法的一個(gè)關(guān)鍵問(wèn)題是如何選擇合適的勢(shì)場(chǎng)函數(shù)，例如：Stephen Waydo使用流函數(shù)進(jìn)行平滑路徑的規(guī)劃[20]，Robert Daily在高速車輛上提出諧波勢(shì)場(chǎng)路徑規(guī)劃方法[21]。在簡(jiǎn)單場(chǎng)景下，人工勢(shì)場(chǎng)法具有較高的求解效率，但其存在的最大問(wèn)題是可能陷入局部最小值，在這種情況下，所獲得的路徑不是最優(yōu)，甚至可能找不到路徑。

算法復(fù)雜度（Complexity）

在規(guī)劃問(wèn)題中，對(duì)于一個(gè)算法的評(píng)價(jià)除了要考慮其時(shí)間和空間復(fù)雜度之外，還要考慮其是否具有完備性和最優(yōu)性，退一步的情況下考慮其是否具有概率完備性和漸進(jìn)最優(yōu)性。只要在了解這些性質(zhì)的基礎(chǔ)上，我們才能針對(duì)不同的實(shí)際場(chǎng)景設(shè)計(jì)和應(yīng)用不同的算法，從而達(dá)到模型復(fù)雜和效率最優(yōu)的最佳衡點(diǎn)。

1）完備性（Completeness）：如果在起始點(diǎn)和目標(biāo)點(diǎn)間有路徑解存在，那么一定可以得到解，如果得不到解那么一定說(shuō)明沒(méi)有解存在；

2）概率完備性（Probabilistically Completeness）：如果在起始點(diǎn)和目標(biāo)點(diǎn)間有路徑解存在，只要規(guī)劃或搜索的時(shí)間足夠長(zhǎng)，就一定能確保找到一條路徑解；

3）最優(yōu)性（Optimality）：規(guī)劃得到的路徑在某個(gè)評(píng)價(jià)指標(biāo)上是最優(yōu)的（評(píng)價(jià)指標(biāo)一般為路徑的長(zhǎng)度）

4）漸進(jìn)最優(yōu)性（Asymptotically optimality）：經(jīng)過(guò)有限次規(guī)劃迭代后得到的路徑是接近最優(yōu)的次優(yōu)路徑，且每次迭代后都與最優(yōu)路徑更加接近，是一個(gè)逐漸收斂的過(guò)程

表1 常見算法比較

未來(lái)發(fā)展趨勢(shì)

圖9. 自動(dòng)駕駛發(fā)展時(shí)間線及過(guò)程中重要的運(yùn)動(dòng)規(guī)劃算法，引用自[19]

人類對(duì)自動(dòng)駕駛的興趣最早可以追溯到1925年，近年來(lái)對(duì)自動(dòng)駕駛的研究熱潮始于美國(guó)國(guó)防先進(jìn)研究項(xiàng)目局（Defense Advanced Research Projects Agency, DARPA ）在2004-2007年舉辦的3屆自動(dòng)駕駛挑戰(zhàn)賽[22]，如圖9所示。在此之后，上文提到的各類決策規(guī)劃方法的有效性都被實(shí)際驗(yàn)證。同時(shí)，將運(yùn)動(dòng)規(guī)劃方法與控制理論、狀態(tài)參數(shù)估計(jì)、機(jī)器學(xué)習(xí)等多領(lǐng)域方法相結(jié)合的解決方案也不斷出現(xiàn)，成為未來(lái)的發(fā)展趨勢(shì)：

1）與車輛動(dòng)力學(xué)結(jié)合：將動(dòng)力學(xué)參數(shù)評(píng)價(jià)指標(biāo)和最優(yōu)規(guī)劃等結(jié)合，從最優(yōu)控制角度進(jìn)行規(guī)劃是近年采用較多的方法，在這個(gè)過(guò)程中可以充分考慮車輛動(dòng)力學(xué)因素，規(guī)劃出的軌跡更加合理。例如采用模型預(yù)測(cè)控制理論（Model Predictive Control）。其不足在于：對(duì)車輛的約束越多，優(yōu)化其軌跡的難度越大，較難實(shí)現(xiàn)在線的實(shí)時(shí)計(jì)算。

2）與狀態(tài)參數(shù)估計(jì)結(jié)合：狀態(tài)參數(shù)估計(jì)可以更加準(zhǔn)確獲得車輛參數(shù)，因此可以將狀態(tài)估計(jì)器加入規(guī)劃模塊中，通過(guò)在線估計(jì)車輛狀態(tài)并將其反饋給規(guī)劃器，提高軌跡質(zhì)量。例如：不同地面類型會(huì)引起車輛滑移特性的變化，進(jìn)而影響車輛狀態(tài)，通過(guò)結(jié)合估計(jì)參數(shù)實(shí)時(shí)重新規(guī)劃軌跡，閉環(huán)規(guī)劃從而提高軌跡安全性。

3）與機(jī)器學(xué)習(xí)結(jié)合：隨著以神經(jīng)網(wǎng)絡(luò)為代表的人工智能的快速發(fā)展，許多傳統(tǒng)的規(guī)劃問(wèn)題也帶來(lái)了新的解決思路。在自動(dòng)駕駛領(lǐng)域，其發(fā)展趨勢(shì)包括：

自動(dòng)駕駛車輛在復(fù)雜環(huán)境中作出最優(yōu)決策，這一問(wèn)題與強(qiáng)化學(xué)習(xí)的定義非常吻合，因此如前文所述，隨著深度強(qiáng)化學(xué)習(xí)技術(shù)的快速發(fā)展，越來(lái)越多的研究團(tuán)隊(duì)開始將其應(yīng)用于自動(dòng)駕駛決策規(guī)劃中，將行為決策與運(yùn)動(dòng)規(guī)劃模塊相融合，直接學(xué)習(xí)得到行駛軌跡。為了解決環(huán)境獎(jiǎng)勵(lì)函數(shù)不易獲得的問(wèn)題，人們還提出了首先利用逆強(qiáng)化學(xué)習(xí)（IRL）根據(jù)人類專家演示學(xué)習(xí)，然后使用強(qiáng)化學(xué)習(xí)來(lái)學(xué)習(xí)最優(yōu)策略。

端到端模型：使用一個(gè)深度神經(jīng)網(wǎng)絡(luò)，直接根據(jù)車輛狀態(tài)和外部環(huán)境信息得出車輛的控制信號(hào)。盡管目前的端到端模型存在類似“黑箱”的不可解釋性，但相信隨著人類對(duì)深度神經(jīng)網(wǎng)絡(luò)理解的不斷加深，這一方法因其突出的簡(jiǎn)潔高效優(yōu)勢(shì)而具有很強(qiáng)的發(fā)展?jié)摿Α?/p>

決策與運(yùn)動(dòng)規(guī)劃模塊融合

編輯：黃飛

?