深度 | 無人駕駛的決策規劃控制技術

雷鋒網按：本文由李力耘與劉少山聯合創作，前者為百度美國研發中心高級架構師，後者系PerceptIn聯合創始人。文章首發於《程序員》，雷鋒網已獲授權發布，如欲轉載，請聯繫Heyc@csdn.net。

無人車作為一個複雜軟硬體結合的系統，其安全可靠運行需要車載硬體、感測器集成、感知預測以及控制規劃等多個模塊的協同配合。本文作者認為最關鍵的部分是感知預測和決策控制規劃的緊密配合。

狹義上的決策規劃控制部分，包含了無人車行為決策（Behavior Decision）、動作規劃（Motion Planning）以及反饋控制（Feedback Control）這三個模塊。而從更寬泛的概念來說，無人車的決策規劃控制模塊，緊密依賴於上游的路由尋徑（Routing）以及交通預測（Prediction）的計算結果，所以本文也對路由尋徑和交通預測模塊進行介紹。

圖1展示了一種無人車軟體系統的典型功能模塊劃分。

圖1 無人車軟體系統模塊

其中感知（Perception）模塊負責從感測器數據中探測計算出周邊環境的物體及其屬性。這些物體信息經過預測模塊的計算，生成預測軌跡傳遞給決策規劃控制系統中的行為決策模塊。

決策規劃控制系統的另一個上游模塊是路由尋徑模塊，其作用在簡單意義上可以理解為無人車軟體系統內部的導航，即在宏觀層面上指導無人車軟體系統的控制規劃模塊按照什麼樣的道路行駛從而實現從起始點到目的地點。值得注意的是這裡的路由尋徑雖然在一定程度上類似傳統的導航，但其細節上緊密依賴於專門為無人車導航繪製的高精度地圖，所以和傳統的導航還是有本質的不同。

一般來說，路由尋徑會作為單獨的模塊來進行實現，而交通預測部分，則既可以做為感知模塊的業務延伸，也可以看成是決策規劃控制模塊的外圍模塊而單獨進行實現。

決策規劃控制（Decision，Planning&Control）系統的任務，就是在對感知到的周邊物體的預測軌跡的基礎上，結合無人車的路由意圖和當前位置，對車輛做出最合理的決策和控制。整個決策規劃控制軟體系統，可以按照解決問題的不同層面，如圖1所示自上而下劃分為行為決策（Behavioral Decision）、動作規劃（Motion Planning）以及反饋控制（Feedback Control）這三個模塊。

其中行為決策模塊（Decision），可以直觀理解成無人車的「副駕駛」。

行為決策接受路由尋徑的結果，同時也接收感知預測和地圖信息。綜合這些輸入信息，行為決策模塊在宏觀上決定了無人車如何行駛。宏觀層面的決策包括在道路上的正常跟車，在遇到交通燈和行人時的等待避讓，以及在路口和其他車輛的交互通過等。

例如，在路由尋徑要求無人車保持當前車道（Lane）行駛，感知發現前方有一輛正常行駛的車輛，行為決策的決定便很可能是跟車行為。

動作規劃模塊，在圖1的劃分中，解決的是具體的無人車動作（Motion）的規劃問題。其功能可以理解為，在一個較小的時空區域內，具體解決無人車從A點到B點如何行駛的問題。動作規劃模塊在這裡解決的問題，相對行為決策，又更加具體了一步。動作規劃需要具體把一個短暫時間t內從A到B的中間路徑點做出規劃，包括選擇途經哪些具體的路徑點，以及到達每個路徑點時，無人車的速度、朝向、加速度等。不僅如此，動作規劃還需要保證兩點：一是在後續時間內，生成從A到B的時空路徑需要保持一定的一致性；二是，這些生成的A到B之間的路徑點，包括到達每個點的速度朝向加速度等，都在下游的反饋控制的實際可操作的物理範圍之內。

決策規劃控制系統最下層的模塊是反饋控制模塊。這是一個直接和無人車底層控制介面CAN-BUS對接的模塊。其核心任務是消化上層動作規劃模塊的輸出軌跡點，通過一系列結合車身屬性和外界物理因素的動力學計算，轉換成對車輛Drive-By-Wire控制的油門、剎車以及方向盤信號，從而儘可能地控制車去實際執行這些軌跡點。反饋控制模塊主要涉及對車輛自身控制，以及和外界物理環境交互的建模。

上述模塊的劃分方法，非常有效地將無人車決策控制規劃這樣一個複雜問題，按照計算邏輯從抽象到具體的做出了非常合理的切分。這樣的劃分使得每個模塊可以各司其職專註解決本層次的問題，從而提升了整個複雜軟體系統的開發效率。

作為決策規劃控制模塊的直接數據上游之一，預測模塊的作用是對感知所探測到的物體進行行為預測，並且將預測的結果具體化為時間空間維度的軌跡傳遞給下游模塊。

一般而言，感知模塊所輸出的物體信息包括位置、速度、朝向以及物體分類（如車輛，行人，自行車）等物理屬性。這些感知所計算輸出的物體屬性偏向於客觀的物理屬性。利用這些輸出的屬性，結合客觀的物理規律，可以對物體做出一個在非常短時間內的「瞬時預測」。

預測模塊所需要解決的問題，不僅僅局限於結合物理規律對物體做出預測，往往更重要的是結合物體和周邊環境，以及積累的歷史數據知識，對感知到的物體做出更為宏觀的行為預測。例如在圖2中，行為預測需要在宏觀層面預測圖中的車輛是否會保持直行還是右轉通過路口。

圖2 無人車周邊物體行為預測

行為預測的軌跡，既包括了障礙物在將來一段時間內運動的方向，還體現了它們在運動中的速度變化。譬如行人過馬路的時候會預測他們使用較為恆定的步行速度，車輛轉彎的時候會先減速後加速，而加減速的快慢也取決於彎道的弧度和長短。實際的無人車系統中，往往將宏觀層面的行為預測和軌跡生成抽象成兩個問題來解決。

宏觀層面的行為預測問題，往往可以抽象成經典的機器學習問題，並且利用基於大數據的深度學習技術來解決。例如， 在假設車輛按照高精地圖劃分的道路（Lane）行駛的前提下，我們可以認為在任何一個時刻，車輛可行駛的每一個Lane序列都是一個需要進行Binary Classification（二元分類）的樣本。在這個假設下，我們不需要對直行、並道、路口拐彎等場景進行區分處理，因為無論是直行、並道，和路口拐彎，都可以統一看成是車輛在不同Lane序列上的行駛。車輛的宏觀行為預測問題，便簡化為對於Lane序列的Binary Classification問題。

圖3 無人車行為預測中的Lane序列Binary Classification抽象

如圖3所示，在t時刻，無人車主車位於Lane 1，此時按照Lane序列的可能展開途徑，我們考慮三條軌跡：

• Trajectory 1： Lane 1、Lane 2、Lane 3對應路口右轉；

• Trajectory 2：Lane 1、Lane 6、Lane 8對應路口直行；

• Trajectory 3：Lane 1、Lane 4、Lane 5、Lane 7對應換道後直行通過路口。

假設在t+w時刻，無人車經過Lane 6行駛到Lane 8的位置，那麼軌跡Trajectory 2便成為該Binary Classification的正樣本，其餘兩條軌跡便成為負樣本。在這種基於Lane序列的問題抽象下，所有的正負樣本可以從歷史數據的回放中獲得，並成為模型訓練的樣本數據。另一方面，模型的特徵抽取可以結合一定時間內的如下信息來設計：

• 車輛本身的物理信息：速度、朝向等；

• 車輛相對於道路的信息：在Lane上的橫向/縱向位移和速度，相對Lane邊界的距離等；

• 車輛周邊的其他物體的信息：車輛周圍例如左右相鄰Lane是否有障礙物等。

在上述的樣本標籤定義和特徵抽取下，無人車Prediction部分的宏觀行為預測可以很好的抽象成典型的機器學習問題來解決。在預測得出的宏觀行為基礎上，相關的軌跡生成和速度預測可以通過特定的規則或者物理模型來實現。

無人車路徑規劃的Routing尋徑問題，雖然也是要解決從A點到B點的路由問題，但由於其輸出結果並不為實際的駕駛員所使用，而是給下游的行為決策（Decision）和動作規劃（Planning）等模塊作為輸入，其路徑規劃的層次要更加深入到無人車所使用的高精地圖的車道（Lane）級別。

如圖4所示，其中的箭頭線段代表高精地圖級別的道路劃分和方向。

圖4 無人車路由尋徑模塊（Routing）的高精地圖道路（Lane）級別尋徑路由

Lane1、Lane2………Lane8構成了一條Routing輸出的路由片段序列。可以看到，無人車地圖級別的Lane劃分並非和實際的自然道路劃分對應。比如Lane2、Lane5、Lane7都代表了由地圖定義繪製的「虛擬」轉向Lane。類似的，一條較長的自然道路，也可能被劃分為若干個Lane（例如Lane3、Lane4）。

作為整體無人車決策控制規劃（Decision，Planning&Control）系統的最上游模塊，路由尋徑模塊的輸出嚴格依賴於無人車高精地圖（HD-Map）的繪製。在高精地圖定義繪製的路網（Road Graph）的道路（Lane）劃分的基礎上，以及在一定的最優策略定義下，路由尋徑模塊需要解決的問題是計算出一個從起點到終點的最佳道路（Lane）行駛序列：{(lane,start_posotion,end_position)i}，其中，(lane,start_posotion,end_position)i我們稱作一個Routing Segment（路由片段），所在的道路由lane來標識，start_posotion,end_position分別代表在這條道路上的起始縱向距離和結束縱向距離。

圖5 無人車尋徑（Routing）基於Lane Point的有向帶權圖上的最短路徑問題抽象

我們可以把無人車在高精地圖的Lane級別尋徑問題，抽象成一個在有向帶權圖上的最短路徑搜索問題（如圖5所示）。

路由尋徑（Routing）模塊首先會基於Lane級別的高精度地圖，在一定範圍內所有可能經過的Lane上進行分散「撒點」，我們稱這些點為「Lane Point」。這些點代表了對無人車可能經過的Lane上的位置的抽樣。這些點與點之間，由有向帶權的邊進行連接。Lane Point之間連接的權，代表了無人車從一個點行駛到另一個點的潛在代價（Cost）。在這樣的有向帶權圖的問題抽象下，路由尋徑問題可以利用常見的A*演算法或者Dijkstra演算法來進行實現。

行為決策（Behavior Decision）層在整個無人車決策規劃控制軟體系統中扮演著「副駕駛」的角色。這個層面彙集了所有重要的車輛周邊信息，不僅包括了無人車本身的當前位置、速度、朝向以及所處車道，還收集了無人車一定距離以內所有重要的感知相關的障礙物信息以及預測軌跡。行為決策層需要解決的問題，就是在知曉這些信息的基礎上，決定無人車的行駛策略。這些信息具體包括：

• 所有的路由尋徑結果：比如無人車為了達到目的地，需要進入的車道是什麼（Target Lane）。

• 無人車的當前自身狀態：車的位置速度朝向，以及當前主車所在的車道。

• 無人車的歷史信息：在上一個行為決策（Behavioral Decision）周期，無人車所做出的決策是什麼？是跟車，停車，轉彎或者是換道？

• 無人車周邊的障礙物信息：無人車周邊一定距離範圍內的所有障礙物信息。例如周邊的車輛所在的車道，鄰近的路口有哪些車輛，它們的速度位置如何？以及在一個較短的時間內它們的意圖和預測的軌跡。周邊是否有自行車或者行人，以及他們的位置速度軌跡等。

• 無人車周邊的交通標識信息：一定範圍內的Lane的變化情況。比如路由尋徑的結果是在Lane1的縱向位移10m處換道進入對應的相鄰Lane2的縱向位移20m處，那麼Lane1的合法的縱向位移換道空間是多大？比如從一個直行Lane行駛結束，需要進入下一個左轉Lane，兩條Lane的交界處是否有紅綠燈或者人行道？

• 當地的交通規則：例如道路限速，是否可以紅燈右拐等等。

無人車的行為決策模塊， 就是要在上述所有信息的基礎上，做出如何行駛的決策。

可以看出，無人車的行為決策模塊是一個信息匯聚的地方。由於需要考慮如此多種不同類型的信息以及受到非常本地化的交規限制，行為決策問題往往很難用一個單純的數學模型來進解決。往往更適合行為決策模塊的解決方法，是利用一些軟體工程的先進觀念來設計一些規則引擎系統。

例如在DARPA無人車競賽中，Stanford的無人車系統「Junior」利用一系列cost設計和有限狀態機（Finite State Machine）來設計無人車的軌跡和操控指令。在近來的無人車規劃控制相關工作中，基於馬爾可夫決策過程（Markov Decision Process）的模型也開始被越來越多得應用到無人車行為層面的決策演算法實現當中。

簡而言之，行為決策層面需要結合路由尋徑的意圖，周邊物體和交通規則，輸出宏觀的行為層面決策指令供下游的動作規劃模塊去更具體地執行。其具體的指令集合設計則需要和下游的動作規劃模塊達成一致。

動作規劃（Motion Planning）

在行為決策層下游的模塊是動作規劃（Motion Planning）。

其任務是具體將行為決策的宏觀指令解釋成一條帶有時間信息的軌跡曲線，來給最底層的反饋控制來進行實際對車的操作。更具體而言，動作規劃模塊試圖解決在一定的約束條件下優化某個範圍內的時空路徑問題。這裡的「時空路徑」指車輛在一定時間段行駛的軌跡。該軌跡不僅包括位置信息，還包括了整條軌跡的時間信息和車輛姿態：即到達每個位置的時間，速度，以及相關的運動變數如加速度，曲率，曲率的高階導數等。

動作規劃可以拆分成為兩個問題：軌跡規劃（Trajectory Planning）和速度規劃（Speed Planning）來解決。其中軌跡規劃只解決在二維平面上，根據行為決策和綜合地圖信息定義的某種Cost函數下，優化軌跡的問題；而速度規劃問題則是在選定了一個或者若干個軌跡（Trajectory）之後，解決用什麼樣的速度來行駛的問題。

，其中（x，y）表示車輛在二維平面的位置，θ表示車輛的朝向，k表示曲率（也即朝向θ的變化率），v表示車輛的速度（即軌跡任意點的切線速度）。車輛的這些姿態變數的標量大小滿足如下關係：

其中曲率的k大小往往由系統的輸入限制條件決定。在此基礎上，考慮一條由車輛運動產生的連續軌跡（Path）。我們稱沿著軌跡的方向的位移為S方向。軌跡相對於車輛姿態的系統關係由下列偏微分方程式給出：

我們的軌跡規劃（Trajectory Planning）演算法非常依賴於地圖對於道路的定義。這裡我們定義道路由其道路中心線（Center Line）所定義，且定義道路的採樣函數為：

，其中s代表道路的中心線切向方向的位移（也稱為縱向位移s）。與此對應的是道路的中心線垂直方向位移l，也稱之為橫向位移。如果考慮一個車輛的姿態點p點在道路上（s，l）坐標，那麼其實際的姿態和（s，l）的關係

滿足：

其中曲率K_r定義為在道路轉彎的內側曲率加大（隨縱向位移l加大），外側曲率則減小。我們使用右手坐標系，所以如圖6所示在靠近原點處朝x軸的正方向，縱向位移l朝著y軸正方向加大。假設對於某條道路Lane(k)，其縱向寬度lk保持不變。那麼該條道路變可以表示成為一個隨著中心線橫向位移s的點集{p(s,lk):s∈R+} 。我們稱這樣的一個坐標系統為坐標系統。

圖6 XY平面下的SL坐標系統及其網格劃分

在上述的車輛模型和道路模型下，我們討論軌跡規劃所產生的軌跡曲線。

首先我們定義車輛的軌跡（Trajectory）為一個從[0,1]區間到車輛姿態向量集合C={x? }的連續映射：ρ:[0,1]→C 。其中，車輛的初始姿態向量為x? =(x,y,θ,k)。每條軌跡終點處如圖7所示，軌跡1的終點姿態為ρ₁(1)=q_end1軌跡2的終點姿態向量為ρ₂(1)=q_end2 ，初始姿態為ρ₁(0)=ρ₂(0)=q_init。

軌跡優化的目標便是在所有可能的軌跡曲線中，篩選出滿足邊界條件的軌跡曲線，再尋找一條/若干條平滑且Cost函數最低的曲線。其中軌跡的候選曲線我們用類似在路由尋徑（Routing）模塊中介紹的「撒點」的採樣方式來生成。

參考圖7，在某條Lane的SL坐標系下，我們按照均勻切分的S和L方向的方格內，在固定S和L間隔下，考慮每個（s_i，l_j）區域的中心點（如圖7所示，又稱為軌跡點Trajectory Point）。一條候選的軌跡（Trajectory）便可以看做是沿著Lane的中心線縱向位移s方向連接不同Trajectory Point的平滑曲線。在圖7所示的道路SL分割和採樣下，可能的Trajectory Point有16個（4個s位置，4個l位置），從車輛的初始位置出發，我們只考慮在s方向單調增大的可能，不考慮城市綜合道路行駛中的倒車情況，那麼總的候選曲線的總條數為4₄=256條。軌跡優化便是要在這256條候選的曲線中找出Cost最優的軌跡。

圖7 SL坐標系下道路的分割採樣以及可能的軌跡

我們採用多項式螺旋線來連接軌跡點Trajectory Point，從而生成候選的曲線。多項式螺旋線，如圖8所示，代表了一類曲率可以用弧長（對應我們軌跡中的s方向）的多項式函數來表示的曲線簇。我們使用三階（Cubic）或者五階（Quintic）的多項式螺旋線，其曲率K和軌跡弧長S的關係K(S)為：

或者

。

圖8 多項式螺旋線以及車輛姿態的螺旋線示意圖

基於這種使用三階（五階）螺旋線連接的軌跡（Trajectory），其參數可以快速有效的通過梯度下降（Gradient Descent）的方法來搜索。以三階多項式為例，我們考慮從車輛初始姿態q_init=(x_I,y_I,θ_I,K_I)到目標姿態q_goal=(x_G,y_G,θ_G,K_G)，且具有連續曲率的三階螺旋線：

在初始狀態 時，考慮曲率的一階導數和二階導數均需要滿足初始狀態的限制，我們可以得到：

這樣使得實際未知參數減少到2個（K₃,S_G）,利用梯度向量我們可以快速尋找到非常接近初始狀態限制的三階螺旋線的參數。在上述的所有候選曲線中，我們可以根據業務的不同需要來設置Cost函數，然後選擇出在任何時間點Cost最小且滿足邊界條件限制的曲線。由於候選曲線隨著我們採樣間隔隨指數增長，往往將Trajectory Point建立成某種有向帶權圖，然後利用圖論中的搜索方法結合Cost設置來選取最優曲線。具體的搜索方法可以參考中的動態編程方法。

在軌跡規劃選定了一條或者若干條曲線後，速度規劃部分將決定車輛以什麼樣的速度來通過這條曲線。軌跡規劃在選取曲線時的Cost設置偏重於靜態障礙物，而速度規劃在選取曲線時的Cost設置應該注意以下幾點：

• 動態的障礙物信息：如Prediction模塊預測的軌跡信息；

• 上游Decision輸出的宏觀層面指令：如對某個障礙物需要避讓（Yield）；

• 狀態量的連續性限制：如速度，加速度等均不能跳變。

在此基礎之上，速度規劃可以將曲線的縱向位移S和時間變數T建立一個二維平面，將上述的限制投影在該平面上求解這個問題。這種考慮曲線縱向位移s和時間t的速度規劃求解方式稱之為S-T求解。類似得，如果考慮橫向位移，也可以在三維的S-L-T空間內進行求解。

無人車反饋控制模塊中常用的車輛控制模型為自行車模型。在該模型中，車輛姿態（Pose）是處於一個二維的平面坐標系內，並且可以由車輛所處的位置（position）以及車身和坐標平面的夾角（heading）來完全描述。同時我們假設車輛前後輪由一個剛性（rigid）不變的軸連接，其中車輛的前輪可以在一定的角度範圍內自由轉動，而車輛的後輪保持和車身的平行關係不能轉動。前輪的轉動對應實際車輛控制中方向盤的轉動。

車輛的自行車模型所代表的車輛姿態如圖9所示。

圖9 車輛控制的自行車模型

這裡我們使用一個基於x-y的二維平面，其中

和

分別代表其x和y方向的單元向量。向量p_r和向量p_f分別代表車輛後輪和前輪與地面的接觸點。車輛的朝向角θ代表車輛和x軸的夾角（即向量p_r和單元向量

的夾角）。方向盤轉角δ定義為前輪朝向和車輛朝向角的夾角。其中前後輪與地面接觸點的向量p_f和p_r之間滿足：

其中

和

分別代表車輛前後輪在和地面接觸點處的瞬時速度向量。考慮車輛的後輪速度在x-y軸的投影標量

和

以及後輪的切向速度

，那麼上述的向量p_f和p_r之間的關係限制在後輪相關分量上的表現形式為：

其中 l 代表車輛前後軸中心間距。類似地，用車輛前輪相關分量的表現形式為：

這裡前後輪的切向速度標量大小滿足：

。

在上述的車輛模型下，反饋控制（Feedback Control）需要解決的問題便是找到滿足車輛動態姿態限制的方向盤轉角δ∈[δ_min,δ_max]的以及前向速度v_r∈[δ_min,δ_max]。而對這些狀態量的控制可以是一個典型的PID反饋控制系統（如圖10所示）。其中e(t)代表當前的跟蹤誤差，而這個跟蹤的變數誤差可以是軌跡的縱向/橫向誤差，角度/曲率誤差或者是若干車輛姿態狀態變數的綜合誤差。其中P控制器代表對當前誤差的反饋，其增益由K_p控制；I和D控制器分別代表積分項和微分項，其增益分別有K_I和K_D來控制。

圖10 基於PID的反饋控制系統

具體到無人車的反饋控制（Feedback Control）模塊，我們需要解決的問題是控制車輛儘可能遵循上遊動作規劃（Motion Planning）所輸出的時空軌跡。可以使用兩個基於PID反饋控制的控制器來分別控制方向盤轉角δ以及前進速度vs。

筆者認為整個無人車廣義決策規劃控制（Decision，Planning&Control）範疇下的路由尋徑（Routing），行為決策（Behavior Decision），動作規劃（Motion Planning）以及反饋控制（Feedback Control）等幾大模塊，在當前的學術和工業界都有一些較為成熟的解決方案可以借鑒。

這些解決方案有些有著牢固的理論基礎和數學推導，還有的在實際的無人車相關比賽中有著的出色發揮。事實上，我們認為單獨看每個層面需要解決的問題，都不是非常困難。如何將整個無人車決策規劃控制的問題有效清晰地劃分到不同的模塊，並且將各個上下游模塊的解決方案配合起來達到整體的協調效果，才是無人車廣義決策規劃控制的難點和挑戰所在。

因此我們並不著力於以調研的形式介紹所有的各個模塊層面的現存解決方案，而是著眼於清晰地提供一套有效劃分無人車決策控制規劃這一複雜問題到不同層面子問題的方法。我們試圖向讀者展示，如何有效地將無人車決策控制規劃這樣一個複雜問題自上而下地進行分割，並且明確每個層面需要解決的具體問題的範圍和限制。我們希望通過展示這樣的「分而治之」的決策控制規劃解決思路，能有益於讀者對整個無人車軟體系統的運作的了解。

雷鋒網註：

李力耘，百度美國研發中心高級架構師。在百度無人車部門負責無人車行為預測方向的系統架構及演算法優化，擁有多項國際專利，其中已遞交三十餘項無人車決策預測相關專利申請。

劉少山，PerceptIn聯合創始人。加州大學歐文分校計算機博士，研究方向：智能感知計算、系統軟體、體系結構與異構計算。現在PerceptIn主要專註於SLAM技術及其在智能硬體上的實現與優化。