自動駕駛中的決策規劃模塊是衡量和評價自動駕駛能力最核心的指標之一，它的主要任務是在接收到傳感器的各種感知信息之后，對當前環境作出分析，然后對底層控制模塊下達指令。典型的決策規劃模塊可以分為三個層次：全局路徑規劃、行為決策、運動規劃。

引言

在一套完整的自動駕駛系統中，如果將感知模塊比作人的眼睛和耳朵，那么決策規劃就是自動駕駛的大腦。大腦在接收到傳感器的各種感知信息之后，對當前環境作出分析，然后對底層控制模塊下達指令，這一過程就是決策規劃模塊的主要任務。同時，決策規劃模塊可以處理多么復雜的場景，也是衡量和評價自動駕駛能力最核心的指標之一[1]。

圖1. ?自動駕駛系統中的決策規劃模塊分層結構，引用自[2]

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

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

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

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

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

全局路徑規劃（Route Planning）?

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

Dijkstra算法是由計算機科學家Edsger W. Dijkstra在1956年提出，用來尋找圖形中節點之間的最短路徑。在Dijkstra算法中，需要計算每一個節點距離起點的總移動代價。同時，還需要一個優先隊列結構。對于所有待遍歷的節點，放入優先隊列中會按照代價進行排序。在算法運行的過程中，每次都從優先隊列中選出代價最小的作為下一個遍歷的節點。直到到達終點為止。

Dijkstra算法的優點是：給出的路徑是最優的；缺點是計算時間復雜度比較高（O(N2)）,因為是向周圍進行探索，沒有明確的方向。

2. A*算法

為了解決Dijkstra算法的搜索效率問題，1968年，A算法由Stanford研究院的Peter Hart, Nils Nilsson以及Bertram Raphael發表，其主要改進是借助一個啟發函數來引導搜索的過程。具體來說，A算法通過下面這個函數來計算每個節點的優先級:

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

其中：

f(n) 是節點n的綜合優先級。當我們選擇下一個要遍歷的節點時，我們總會選取綜合優先級最高（值最?。┑墓濣c。

g(n)是節點n距離起點的代價。

h(n) 是節點n距離終點的預計代價，這也就是A*算法的啟發函數。

行為決策（Behavioral Layer）

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

這一過程面臨三個主要問題：

首先，真實的駕駛場景千變萬化，如何覆蓋？

其次，真實的駕駛場景是一個多智能體決策環境，包括主車在內的每一個參與者所做出的行為，都會對環境中的其他參與者帶來影響，因此我們需要對環境中其他參與者的行為進行預測；

最后，自動駕駛車輛對于環境信息不可能做到100%的感知，例如存在許多被障礙物遮擋的可能危險情形。

綜合以上幾點，在自動駕駛行為決策層，我們需要解決的是在多智能體決策的復雜環境中，存在感知不確定性情況的規劃問題。可以說這一難題是真正實現L4、L5級別自動駕駛技術的核心瓶頸之一，近年來隨著深度強化學習等領域的快速發展，為解決這一問題帶來了新的思路和曙光。

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

1. 有限狀態機模型

自動駕駛車輛最開始的決策模型為有限狀態機模型[6]，車輛根據當前環境選擇合適的駕駛行為，如停車、換道、超車、避讓、緩慢行駛等模式，狀態機模型通過構建有限的有向連通圖來描述不同的駕駛狀態以及狀態之間的轉移關系，從而根據駕駛狀態的遷移反應式地生成駕駛動作。

有限狀態機模型因為簡單、易行，是無人駕駛領域目前最廣泛的行為決策模型，但該類模型忽略了環境的動態性和不確定性，此外，當駕駛場景特征較多時，狀態的劃分和管理比較繁瑣，多適用于簡單場景下，很難勝任具有豐富結構化特征的城區道路環境下的行為決策任務。

2. 決策樹模型

決策/行為樹模型[7]和狀態機模型類似，也是通過當前駕駛狀態的屬性值反應式地選擇不同的駕駛動作，但不同的是該類模型將駕駛狀態和控制邏輯固化到了樹形結構中，通過自頂向下的“輪詢”機制進行駕駛策略搜索。這類決策模型具備可視化的控制邏輯，并且控制節點可復用，但需要針對每個駕駛場景離線定義決策網路，當狀態空間、行為空間較大時，控制邏輯將比較復雜。另外，該類模型同樣無法考慮交通環境中存在的不確定性因素。

3. 基于知識的推理決策模型

基于知識的推理決策模型由“場景特征－駕駛動作”的映射關系來模仿人類駕駛員的行為決策過程，該類模型將駕駛知識存儲在知識庫或者神經網絡中，這里的駕駛知識主要表現為規則、案例或場景特征到駕駛動作的映射關系。進而，通過“查詢”機制從知識庫或者訓練過的網絡結構中推理出駕駛動作。

該類模型主要包括：基于規則的推理系統[8]、基于案例的推理系統[9]和基于神經網絡的映射模型[10]。

該類模型對先驗駕駛知識、訓練數據的依賴性較大，需要對駕駛知識進行精心整理、管理和更新，雖然基于神經網絡的映射模型可以省去數據標注和知識整合的過程，但是仍然存在以下缺點：

其“數據”驅動機制使得其對訓練數據的依賴性較大，訓練數據需要足夠充分[11]；

將映射關系固化到網絡結構中，其解釋性較差；

存在“黑箱”問題，透明性差，對于實際系統中出現的問題可追溯性較差，很難發現問題的根本原因。

4. 基于價值的決策模型

根據最大效用理論，基于效用／價值的決策模型的基本思想是依據選擇準則在多個備選方案中選擇出最優的駕駛策略／動作[12]。

為了評估每個駕駛動作的好壞程度，該類模型定義了效用（utility）或價值（value）函數，根據某些準則屬性定量地評估駕駛策略符合駕駛任務目標的程度，對于無人駕駛任務而言，這些準則屬性可以是安全性、舒適度、行車效率等，效用和價值可以是由其中單個屬性決定也可以是由多個屬性決定。

澳大利亞格里菲斯大學的Furda和Vlacic提出了多準則決策方法從候選動作集中選擇最優的駕駛動作[13]；新加坡國立大學的Bandyopadhyay等人提出了基于POMDP的行為決策模型[14]，用以解決存在感知不確定性的情況；卡內基梅隆大學的Wei J等人提出基于PCB（Prediction and-Cost-function Based）的行為決策模型[15]，其側重點在于如何構建恰當的代價函數來指導對環境的預測；為了解決在多智能體參與的復雜環境中的決策問題，許多基于博弈論的模型也被研究者用來推理車輛之間的交互行為[16]，[17]；此外，因為在特征提取方面的優勢，深度強化學習技術也開始被廣泛應用，以完成最優駕駛動作的生成[18]。

運動規劃（Motion Planning）

在確定具體的駕駛行為之后，我們需要做的是將“行為”轉化成一條更加具體的行駛“軌跡”，從而能夠最終生成對車輛的一系列具體控制信號，實現車輛按照規劃目標的行駛。這一過程稱為運動規劃（Motion Planning），運動規劃的概念在機器人領域已經有較長時間的研究歷史，我們可以從數學的角度將它看做如下的一個優化問題：

路徑規劃（Path Planning）

圖3. 路徑規劃的定義

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

確定的起始狀態以及目標點所在的區域

避免碰撞

對路徑的微分約束（例如在實際問題中路徑曲率不能太小，對應于其二階導數的約束）

該優化問題的目標泛函定義為J(σ)，其具體意義可以表示為路徑長度、控制復雜度等衡量標準。

然而在自動駕駛問題中，車輛周圍的環境是持續動態變化的，因此單純的路徑規劃不能給出在行駛過程中一直有效的解，因此我們需要增加一個維度——時間T，相應的規劃問題通常被稱為軌跡規劃。

軌跡規劃（Trajectory Planning）

圖4. 軌跡規劃的定義

時間維度的增加為規劃問題帶來了巨大的挑戰。例如，對于一個在2D環境中移動一個抽象為單點的機器人，環境中的障礙物近似為多邊形的問題。路徑規劃問題可以在多項式時間內求解，而加入時間維度的軌跡規劃問題已經被證明是NP-hard問題。

在自動駕駛的實際場景中，無論是對車輛本身還是對周圍環境，建立更為精確的模型意味著對優化問題更為復雜的約束，同時也意味著求解的更加困難。因此實際采用的算法都是建立在對實際場景的近似前提下，在模型精確性和求解效率二者之間尋求一個最佳的平衡點。

下文對自動駕駛領域目前常見的幾類運動規劃算法分別進行介紹，在實際中，往往是其中幾類思想的結合才能最終達到比較好的規劃結果，并滿足更多的不同場景。

1. 基于搜索的規劃算法

通過搜索來解決運動規劃問題是最樸素的思路之一,其基本思想是將狀態空間通過確定的方式離散成一個圖,然后利用各種啟發式搜索算法搜索可行解甚至是最優解。? ? ??

在將狀態空間離散化的過程中，需要注意的是確保最終形成的柵格具有最大的覆蓋面積，同時不會重復。如圖5所示，左邊的柵格是由直行、左轉90°、右轉90°這三種行為生成；而如果選擇直行、左轉89°、右轉89°三種行為，最后就無法生成一個覆蓋全部區域的柵格結構。

圖5. 構建柵格圖，引用自[2]

在將狀態空間柵格化之后，我們就可以使用前文已經介紹的Dijkstra、A*搜索算法，完成最終的規劃。然而在實際復雜環境中，柵格數目眾多，并且環境隨時間動態變化，會導致搜索結點過多，因此發展出了多種改進算法，用以處理不同的具體場景：

1） Hybrid A* 算法，在A*算法的基礎上考慮了車的最大轉向問題，例如限定計算的路徑上車最大轉向不超過5°。該算法目前的應用場景有車掉頭（Stanford 參加DARPA 挑戰賽使用的Junior車采用了該算法進行uturn），泊車等等對方向盤控制要求較高的場景。

2）D*、D*Lite算法，事先由終點向起點進行搜索，使用Dijkstra算法，存儲路網中目標點到每個點的最短路徑長度k, 和該節點到目標點的實際長度值h，開始情況下 k==h， 并且存儲每個節點的上一個節點，保證能夠沿著鏈接走下去。

計算結束后，獲取了一條當時最優路徑。當車行駛到某個節點時，通過傳感器發現該節點已經無法通行（有障礙物），則對已存儲的路網信息一些相關點的h值進行修改（變大），選擇一個鄰居點滿足仍然h==k的，即仍然是最優路徑上的點，作為下一個點。

然后走到終點。該類算法適用于在未知環境中的導航以及路徑規劃，廣泛用于目前各種移動機器人和自主車輛載具，例如“機遇號”和“勇氣號”火星車。

2. 基于采樣的規劃算法

通過對連續的狀態空間進行采樣，從而將原問題近似成一個離散序列的優化問題，這一思路也是在計算機科學中應用最為廣泛的算法。在運動規劃問題中，基于采樣的基本算法包括概率路線圖(PRM)和快速搜索隨機樹(RRT)算法。

圖6. 利用RRT算法實現u形彎的軌跡規劃，引用自[19]

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

預處理階段：對狀態空間內的安全區域均勻隨機采樣n個點，每個采樣點分別與一定距離內的鄰近采樣點連接，并丟棄掉與障礙物發生碰撞的軌跡,最終得到一個連通圖。

査詢階段：對于給定的一對初始和目標狀態,分別將其連接到已經構建的圖中,再使用搜索算法尋找滿足要求的軌跡。

容易看出,一旦構建一個PRM之后,可以用于解決不同初始、目標狀態的運動規劃問題,但是這個特性對于自動駕駛運動規劃而言是不必要的。另外PRM要求對狀態之間作精確連接，這對于存在復雜微分約束的運動規劃問題是十分困難的。

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

樹的初始化：初始化樹的結點集和邊集,結點集只包含初始狀態,邊集為空。

樹的生長：對狀態空間隨機采樣，當采樣點落在狀態空間安全區域時，選擇當前樹中離采樣點最近的結點，將其向采樣點擴展(或連接)。若生成的軌跡不與障礙物發生碰撞，則將該軌跡加入樹的邊集，該軌跡的終點加入到樹的結點集。

RRT是一種增量式采樣的搜索方法，無須設置任何分辨率參數。在極限情況，該搜索樹將稠密的布滿整個空間，此時搜索樹由很多較短曲線或路經構成，以實現充滿整個空間的目的。

3）多種改進算法

從以上基礎算法的描述我們可以了解到，對狀態空間進行采樣，可以保證得到連接起始點與終點的可行解，但由于采樣過程是對整個空間進行均勻采樣，因此效率很低；在復雜場景下無法實現實時求解；此外，最終規劃結果無法保證得到的可行解是最優解。針對這些劣勢，多種改進算法被提出并應用于自動駕駛問題：

效率改進--不均勻采樣

- RRT-Connect：同時構建兩棵分別起始于初始狀態和目標狀態的樹,當兩棵樹生長到一起時則找到可行解。

-? 啟發式（hRRT）：使用啟發式函數增加擴展代價低的結點被采樣的概率。

-? 結合駕駛員模型：結合駕駛員視覺注意力模型進行偏向性采樣，利用視覺特征信息引導運動規劃，使規劃出的軌跡更符合人類駕駛行為。

-? 構造新度量RG-RRT(reachability guided RT)：常規類歐式距離的度量并不能真實反映構形或狀態之間的遠近，RG-RRT計算樹中結點的能達集，當采樣點到結點的距離大于采樣點到該結點能達集的距離時, 該節點才有可能被選中進行擴展。

-? 加入障礙物懲罰（RC-RRT、EG-RRT、ADD-RRT等）：降低靠近障礙物的結點獲得擴展的概率。

實時性改進

- anytime RRT先快速構建一個RRT,獲得一個可行解并記錄其代價.之后算法會繼續采樣,但僅將有利于降低可行解代價的結點插入樹中,從而逐漸獲得較優的可行解.。

-? Replanning將整個規劃任務分解為若干等時間的子任務序列，在執行當前任務的同時規劃下一個任務。

最優性改進

- PRM*、RRG、RRT*：根據隨機幾何圖理論（在狀態空間中隨機采樣m個點,并將距離小于r(n)的點連起來,就構成了隨機幾何圖）對標準PRM 和RRT做出改進,得到了具有漸近最優性質的PRM*、RRG和RRT*算法

3. 直接優化方法

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

1）Frenet坐標系

圖7. Frenet坐標系

由于真實世界中的道路都是彎曲的，為了簡化求解優化問題的參數表達，在自動駕駛中通常采用Frenet坐標系。

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

因為在公路行駛中，我們總是能夠簡單的找到道路的參考線（即道路的中心線），那么基于參考線的位置的表示就可以簡單的使用縱向距離S（即沿著道路方向的距離）和橫向距離L（即偏離參考線的距離）來描述。

2）路徑-速度解耦法

在Frenet坐標系中，路徑-速度解耦法分別優化路徑和速度，路徑優化主要考慮靜態障礙物，通過動態規劃生成一條靜態的參考路徑（SL維度），接著基于生成的路徑，考慮對速度的規劃（ST維度）。這一過程可以不斷迭代，從而實現對軌跡的實時更新。在百度的開源自動駕駛平臺Apollo中采用的EM planner就是基于類似的解決思路。這一方案具有較強的靈活性，可以普適性的適用于許多場景。

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

4. 參數化曲線構造法

圖8. 常見的參數化曲線構造法，引用自[19]

參數化曲線構造法的出發點是車輛本身的約束，包括運動學與動力學的約束，因此一般規劃的路徑需要是曲率連續的。這類方法根據起始點和目標點，考慮障礙物，通過構造一族符合車輛約束的曲線給出一條平滑路徑。

如圖8所示，常見的曲線有Dubins曲線（由直線和圓弧構成，是一種簡單車輛模型Dubin模型在二維空間中的最短曲線族），回旋曲線，多項式曲線，貝塞爾曲線，樣條曲線等。單純應用參數化曲線構造法很難滿足實際復雜場景，因此現在越來越多的自動駕駛系統將其與其他方法結合，用來對已經規劃生成的軌跡做平滑處理，以滿足車輛運動學與動力學約束。

5. 人工勢場法

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

算法復雜度（Complexity）

在規劃問題中，對于一個算法的評價除了要考慮其時間和空間復雜度之外，還要考慮其是否具有完備性和最優性，退一步的情況下考慮其是否具有概率完備性和漸進最優性。只要在了解這些性質的基礎上，我們才能針對不同的實際場景設計和應用不同的算法，從而達到模型復雜和效率最優的最佳衡點。

1）完備性（Completeness）：如果在起始點和目標點間有路徑解存在，那么一定可以得到解，如果得不到解那么一定說明沒有解存在；

2）概率完備性（Probabilistically Completeness）：如果在起始點和目標點間有路徑解存在，只要規劃或搜索的時間足夠長，就一定能確保找到一條路徑解；

3）最優性（Optimality）：規劃得到的路徑在某個評價指標上是最優的（評價指標一般為路徑的長度）

4）漸進最優性（Asymptotically optimality）：經過有限次規劃迭代后得到的路徑是接近最優的次優路徑，且每次迭代后都與最優路徑更加接近，是一個逐漸收斂的過程

表1 常見算法比較

未來發展趨勢

圖9. 自動駕駛發展時間線及過程中重要的運動規劃算法，引用自[19]

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

1）與車輛動力學結合：將動力學參數評價指標和最優規劃等結合，從最優控制角度進行規劃是近年采用較多的方法，在這個過程中可以充分考慮車輛動力學因素，規劃出的軌跡更加合理。例如采用模型預測控制理論（Model Predictive Control）。其不足在于：對車輛的約束越多，優化其軌跡的難度越大，較難實現在線的實時計算。

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

3）與機器學習結合：隨著以神經網絡為代表的人工智能的快速發展，許多傳統的規劃問題也帶來了新的解決思路。在自動駕駛領域，其發展趨勢包括：

自動駕駛車輛在復雜環境中作出最優決策，這一問題與強化學習的定義非常吻合，因此如前文所述，隨著深度強化學習技術的快速發展，越來越多的研究團隊開始將其應用于自動駕駛決策規劃中，將行為決策與運動規劃模塊相融合，直接學習得到行駛軌跡。為了解決環境獎勵函數不易獲得的問題，人們還提出了首先利用逆強化學習（IRL）根據人類專家演示學習，然后使用強化學習來學習最優策略。

端到端模型：使用一個深度神經網絡，直接根據車輛狀態和外部環境信息得出車輛的控制信號。盡管目前的端到端模型存在類似“黑箱”的不可解釋性，但相信隨著人類對深度神經網絡理解的不斷加深，這一方法因其突出的簡潔高效優勢而具有很強的發展潛力。

決策與運動規劃模塊融合

編輯：黃飛

?