是的，這一篇的文章主題是自動駕駛車輛規劃。全文共7144字。
/ 導讀 /
一輛自動駕駛車輛的典型架構如圖1所示。從圖中可以看到，自動駕駛車輛從系統功能上劃分，可以分為環境感知模塊、決策規劃模塊和車輛控制模塊。如果把感知模塊比作人的眼睛和耳朵，那麼決策規劃就是感知信息匯聚的模塊，扮演著車輛「大腦」的角色。大腦在接收到傳感器的各類感知信息後，對當前周邊環境態進行分析，然後對車輛控制模塊下達指令。決策規劃模塊決定了自動駕駛車輛可以適應多麼複雜的場景，能否高效安全的行駛，是評級自動駕駛能力的最核心指標之一，具有十分重要的學術研究價值。
當前自動駕駛汽車存在三大類典型的感知決策控制方法，如圖2所示。
由上至下依次為[2]：
Sequential Planning、
Behavior-awarePlanning和End-to-end Planning。
Sequential Planning是層次最簡明的一種方法，將先進的感知和決策方法生成運動規劃和控制的輸入。其作用是實時地求解出一序列連續的控制輸入或可行運動狀態使得汽車能夠安全地由初始狀態到達終止狀態，並且求解出結果不僅要滿足汽車的運動學及動力學約束，同時還要滿足結構化道路帶來的幾何約束以及交通法規約束。
整體架構如圖3所示[3]，可以分為全局路徑規劃層（Route Planning）、行為決策層（Behavioral Layer）和運動規劃層（Motion planning）。
圖3：Sequential Planning 整體架構
其中：
全局路徑規劃在接收到行駛目的地之後，結合地圖信息，生成起點到終點的路徑尋找，也可稱為任務規劃（Mission planning）；
行為決策層也可稱之為行為決策（Decision），在接收到全局路徑信息後，結合當前交通信息和障礙物等情況，給運動規劃模塊輸入一系列的決策信息，如在十字路口通行優先級、交通堵塞處理、車輛匯流合理判斷問題等；
運動規劃層根據具體的行為決策信息，規劃生成一條滿足特定約束條件的運動軌跡，以達到某種目的，如安全避障、準確進入停車位等。
Behavior-aware Planning引入駕駛員與自動駕駛汽車交互、自動駕駛汽車與道路場景交互、自動駕駛汽車與周邊參與者交互，在決策規劃的過程中考慮外部環境的不確定性，從而增加安全性。
目前在研究中，經常用到的五類方法是[2]：Cooperation and Interaction (協同與交流)、Games-Theoretic Approaches（博弈論）、Probabilistic Approaches（概率方法）、Partially Observable Markov Decision Processes（隱馬爾可夫）、Learning-Based Approaches（基於學習的方法）。
圖4：Behavior-aware Planning常用研究方法
End-to-end Planning通過深度學習等系統驅動，從離線收集的真實數據中學習駕駛員駕駛行為，直接通過輸入的圖像或者視頻信息得到汽車駕駛行為的指令，包括方向盤轉角、油門大小、踩剎車的程度等。英偉達[4]使用卷積神經網絡（CNN）將從前向攝像機得到的原始圖像映射成自動駕駛汽車的駕駛命令，如圖5所示。三架攝像機安裝在數據採集汽車的擋風玻璃後面，而來自攝像機的時間戳視頻是與人類駕駛員的轉向角度同時被捕獲的。圖像被送入一個卷積神經網絡，然後計算一個被推薦的轉向命令。這個被推薦的轉向命令會與該圖像的期望命令相比較，卷積神經網絡的權重就會被調整以使其實際輸出更接近期望輸出。
圖5：End-to-end Planning系統框圖
本文將主要介紹Sequential Planning的分層架構和常見算法。
全局路徑規劃就是指在給定車輛當前位置與終點目標後，通過搜索選擇一條最優的路徑。這裡的「最優」通常包括時間最短或者路程最短等。通俗意義上講，這一過程和我們生活中用到的「導航」功能類似，區別在於自動駕駛的全局路徑規劃使用高精度地圖，裡面包括更多的路段和車道信息。
最基礎和普適的全局路徑規划算法是Dijkstra算法和A*算法。
Dijkstra算法是由計算機科學家Edsger W. Dijkstra在1956年提出的。Dijkstra算法用來尋找圖形中節點之間的最短路徑。在Dijkstra算法中，需要計算每一個節點距離起點的總移動代價。同時，還需要一個優先隊列結構。對於所有待遍歷的節點，放入優先隊列中會按照代價進行排序。在算法運行的過程中，每次都從優先隊列中選出代價最小的作為下一個遍歷的節點，直到到達終點為止。
簡單說，就是從起點開始向外擴展，直至達到目標，從中找到一條最短路徑。如下圖中[5]，粉紅色的方塊是起點，藍色的方塊是目標，而藍綠色區域顯示了Dijkstra算法掃描的區域。圖a表示在地圖沒有障礙物；圖b表示地圖存在凹形障礙物。
在理解A*算法之前，除了Dijkstra算法，還需要理解最佳優先搜索算法。貪婪的最佳優先搜索算法以與Dijkstra類似的方式工作，但是使用的是以每個節點到達終點的距離作為優先級。最佳優先搜索算法比Dijkstra算法運行更快。圖7中，黃色表示具有較高啟發式值（達到目標的成本較高）的那些節點，而黑色表示具有較低啟發式值（達到目標的成本較低）的那些節點。在地圖中存在障礙物時，最佳優先搜索是「貪婪的」，即使不是正確的道路，它也會嘗試朝目標邁進。由於它只考慮達到目標的成本，而忽略了到目前為止的成本，因此即使它所走的路徑已經很長，它也會繼續前進，如圖7中b所示。
結合最佳優先搜索算法和Dijkstra算法的優點不好嗎？A*最初發表於1968年，由Stanford研究院的Peter Hart、Nils Nilsson以及Bertram Raphael發表。它結合了Dijkstra算法的特點（靠近起點的最佳點）和貪婪的「最佳優先搜索」使用的特點（靠近目標的最佳點），A*算法通過下面這個函數來計算每個節點的優先級。
f(n)=g(n)+h(n)
其中：
f(n)是節點n的綜合優先級。當我們選擇下一個要遍歷的節點時，我們總會選取綜合優先級最高（值最小）的節點。
g(n)是節點n距離起點的代價。
h(n)是節點n距離終點的預計代價，同時也是A*算法的啟發函數。
有研究者在研究人類駕駛行為時，將人類駕駛任務分為了3個不同層次：導航任務、引導任務與穩定任務。與之對應的是在順序規劃中，自動駕駛車輛根據任務進行分層，分別執行全局路徑規劃、行為決策、軌跡生成任務。行為決策層在道路行駛過程中根據周邊交通環境，做出車道保持、換道、超車、駛過路口等決定。
在這個過程中，面臨幾個主要問題：
真實的駕駛環境中，交通場景和道路是複雜的，如何能夠在任意場景下做出正確決策。
自動駕駛車輛在環境中面臨無法預測的突發情況時，如何能夠做出正確的決策。
自動駕駛車輛在無法完全感知周圍交通環境的情況下，如何進行正確決定。
行為決策是自動駕駛車輛技術的一項關鍵技術，提高自動駕駛車輛在城區複雜、不確定環境下的行為決策規划水平，可以提高自動駕駛車輛的智能化水平。
當前，主要存在三種主要的行為決策模型。
有限狀態機（Finite State Machine, FSM）表示有限個狀態及在這些狀態之間的轉移和動作等行為的數學模型。在自動駕駛中，車輛根據交通場景和交通規則選擇合適的駕駛行為，如車道保持、前車跟隨、停車等待等模式，狀態機模型通過構建有限的有向連通圖來描述不同的駕駛狀態以及狀態之間的轉移關係，從而生成駕駛動作。
經典的有限狀態機：「Junior」是史丹福大學在2007年參加DARPA城市挑戰賽時的無人車，它是第二名完成該比賽的無人車，Junior的行為規劃系統就是通有限狀態機實現的，如圖8。
其中：
LOCATE_VEHICLE: 表示Junior的初始狀態，確定無人車在地圖中的位置。
FORWARD_DRIVE: 表示在普通道路上的行駛狀態，包含車輛直行、車道保持和障礙物避障等。
PARKING_NAVIGATE：表示停車場內的普通駕駛模式。
UTURN_STOP: 表示U型掉頭前的停止狀態。
CROSS_DIVIDER: 表示跨車道行駛。
STOP_SIGN_WAIT: 表示當無人車在十字路口等待時，進入此狀態。
CROSS_INTERSECTION：表示在這個狀態下無人車處理十字路口通過這一場景，無人車會等待直到確認能夠安全通過。
UTURN_DRIVE: 表明在U型掉頭時調用的狀態。
TRAFFIC_JAM和ESCAPE：處理交通阻塞時的兩個狀態。
BAD_RNDF: 表示如果當前道路和預先做的路網圖不同的時候，即進入該狀態，在這個狀態下，無人車會採用混合A*算法完成車輛的路徑規劃。
MISSION_COMPLETE: 當挑戰賽（DARPA）結束，無人車進入該狀態，即整個狀態機的結束狀態。
有限狀態機結構簡單，控制邏輯明確，是無人駕駛領域使用比較廣泛的行為決策模型，但是當駕駛狀態和模式過多的時候，有限狀態機的修改和更新變得非常繁瑣。
決策樹模型[7]和有限狀態機模型類似，被廣泛應用到自動駕駛車輛行為規劃中。決策樹（Decision Tree）由一個決策圖和可能的結果組成，用來創建到達目標的規劃。在自動駕駛行為規划算法中將駕駛狀態和控制邏輯轉化為樹形結構，通過自頂向下的「輪詢」機制進行駕駛策略的搜索和選擇。決策樹由非葉節點和葉節點組成，非葉節點對應駕駛狀態，葉節點對應駕駛動作。這類決策模型具備可視化的控制邏輯，並且控制節點可復用，但需要針對每個駕駛場景離線定義決策網路，當狀態空間、行為空間較大時，控制邏輯將比較複雜。另外，該類模型同樣無法考慮交通環境中存在的不確定性因素。
基於知識的推理決策模型由「場景特徵-駕駛動作」的映射關係來模仿人類駕駛員的行為決策過程，該類模型通過識別當前場景，查詢存儲在知識庫或者神經網絡中駕駛知識，然後推理出對應的駕駛動作。採集了人類駕駛員的行為數據，按照場景的不同分類存儲，形成行為庫。通過基於資訊理論、粒子計算的知識獲取表達，形成相應的知識庫。這裡的知識既包括數據統計分析得到的數學模型，也包括通過機器學習得到「黑箱」。根據感知系統實時信息，利用知識庫進行行為決策。通過不同的仿真場景和實車實驗對知識庫進行反覆驗證，迭代更新，逐漸完善。
運動規劃是無人駕駛順序規劃中的最後一步，輸入是車輛當前的狀態（車輛當前位置、感知障礙物位置速度、地圖信息等）和具體的駕駛行為，目標輸出是一系列運動軌跡和控制信息（速度等）。運動規劃要做的就是生成當前狀態和目標的一系列映射，實現車輛按照規劃軌跡進行安全駕駛。
從機器人領域的角度看，運動規劃的本質是一個搜索問題，將交通環境以確定的方法離散成一個圖，然後利用各種搜索算法尋找最短路徑或者安全路徑。
從數學角度看，運動規劃的本質是一個優化問題。在確定的狀態空間中，尋找一個滿足一定約束條件的映射。這些約束包括：
局部約束問題：比如要考慮進行障礙物的避障。
微分約束問題：要考慮曲線的曲率大小是否合適，曲率是否連續。
全局約束問題：要考慮兩點之間的最短路徑問題。
車輛的動力學約束問題：要考慮是否滿足車輛橫縱向動力學。
在自動駕駛場景中，駕駛環境是動態變化的，因此在運動規劃中需要加入時間約束，為動態的行駛過程提供有效解。時間約束的增加給自動駕駛的運動規劃帶來了新的挑戰。目前，常用的自動駕駛算法有基於圖搜索的方法、基於隨機採樣的方法、基於曲線插值的方法、基於直接優化的方法等。在實際應用中，運動規劃往往需要上述幾種方法進行結合，才能達到比較好的規劃效果。
解決運動規劃的一大類算法是啟發性搜索算法，其基本思想是將狀態空間通過確定的方式離散成一個圖，然後利用各種啟發式搜索算法搜索可行解甚至是最優解。其中包含兩個過程，狀態空間建模和啟發式搜索兩個過程。
在離散搜索思路下，狀態空間通過被建模為佔據柵格(Occupancy Grid)或狀態晶格(State Lattice)。佔據柵格法將空間建模為離散柵格，根據障礙物的分布確定柵格是否被佔據。與佔據柵格不同，狀態晶格法將規劃區域劃分為表徵狀態的柵格，兩個柵格的連接則表徵了它們之間的狀態轉移。
在狀態空間建模後，利用圖搜索算法得到從初始狀態到終止狀態的軌跡。圖搜索算法包括Dijkstra算法和A*算法等，以及A*算法的變種。與佔據柵格法相比，狀態晶格法搜索得到的任意兩柵格之間的路徑可以用預先計算的晶格表示。
State Lattice圖9所示[8]。將網格稱為狀態晶格，在其上應用了運動規劃搜索，路徑搜索基於包含所有可行特徵的一組格子或圖元的局部查詢，從而使車輛從初始狀態行駛到其他狀態。在一個環境下先計算出來車輛所有可能的路徑曲線，然後根據碰撞檢測、交通規則等判斷計算可行的路徑，利用成本函數決定之間的最佳路徑。成本函數可以是：任務到達cost、橫向偏移cost、碰撞cost、縱向衝擊度 cost、橫向加速度cost、向心加速度cost等。
圖9：State Lattice
通過對連續的狀態空間進行採樣，從而將原問題近似成一個離散序列的優化問題，從中篩選出目標樣本點序列作為路徑計算結果。最為常見的是概率路線圖方法（PRM，Probabilistic Roadmap Method）以及快速擴展隨機樹方法（RRT，Rapidly exploring Random Tree）。
概率路線圖(PRM):
PRM 算法首先在可行使空間中進行離線採樣，構造出路線網絡圖（Roadmap），隨後根據起始點與目標點在路線網絡圖中進行查詢，得到可行的行駛路徑。整個過程分為預處理階段和查詢階段。
預處理階段：對狀態空間內的安全區域均勻隨機採樣ｎ個點，每個採樣點分別與一定距離內的鄰近採樣點連接，並丟棄掉與障礙物發生碰撞的軌跡，最終得到一個連通圖。
查詢階段：對於給定的一對初始和目標狀態，分別將其連接到已經構建的圖中，再使用搜索算法尋找滿足要求的軌跡。
快速擴展隨機樹方法（RRT）:
RRT採樣過程中並不關心整個完整的空間，而是通過一個稠密的採樣序列的引導，在每個擴張周期中對空間進行一次採樣從而逐步遞增的向整個空間擴張。隨著採樣次數的增加，搜索樹逐步的覆蓋整個搜索空間，如圖10所示。
初始狀態為根節點構建快速搜索樹。每一個擴張周期算法通過隨機採樣序列的引導在狀態空間中得到一個採樣節點；隨後，遍歷搜索樹，通過一個度量函數找到搜索樹上距離採樣節點最近的節點；連接兩節點並選擇合適的控制輸入使得規劃對象由搜索樹節點向採樣點運動擴張，得到擴張節點；最後將擴張節點添加到搜索樹上完成該周期的搜索。重複上述周期性擴張，直到目標節點添加到搜索樹上或到達最大循環迭代次數為止。
曲線插值的軌跡規劃方法的主要思想是基於預設的目標軌跡點（waypoint）
序列，重構出滿足連續性、舒適性以及車輛動力學約束的路徑。常用的曲線插值
算法包括 Clothoid 曲線擬合、多項式曲線擬合、樣條曲線擬合、貝塞爾曲線擬合等。
Clothoid曲線[9]：
Clothoid 曲線是指曲率連續線性變化的一類曲線，因此 Clothoid 曲線生成的路徑曲率是連續的，經常被用來連接直線路徑與圓弧路徑。實際上，許多行車道路的設計均是按照 Clothoid 曲線設計的。
Polynomial Curves（多項式擬合）[10]：
基於多項式軌跡規劃方法的設計思想為根據車輛的初始狀態和目標狀態對變道軌跡進行規劃，使車輛在指定的時間到達相鄰車道。採樣函數是函數f(x,y,t)，從描述的函數族類中尋找一條軌跡，能充分描述車輛從起始位置過渡到目標位置過程的動態特性。
隨著多項式次數的變大，曲線的擬合效果越好，但次數的增也會導致參數求解的運算量指數增長，通常選用五次多項式進行變道軌跡的規劃。在 x 向、y 向分別選用五次多項式構造變道軌跡的曲線簇。
樣條曲線擬合：
樣條是一種分段的多項式參數曲線，它可以被定義為多項式曲線、B樣條曲線等。樣條插值是一種工業設計中常用的、得到平滑曲線的一種插值方法。
三次樣條也是其中用的較為廣泛的一種。說白了，就是通過一系列值點，根據插值連續性、微分連續性和邊界條件等構建一條光滑曲線的過程。
貝塞爾曲線擬合：
貝塞爾曲線有著很多特殊的性質,在圖形設計和路徑規劃中應用都非常廣泛。貝塞爾曲線完全由其控制點決定其形狀,n個控制點對應著n-1階的貝塞爾曲線，並且可以通過遞歸的方式來繪製。
基於優化的軌跡規劃方法根據一個需要最大或最小化的優化指標及一些約束決策出一條最優軌跡。經典的框架是KIT[11]在Frenet lattice將橫向軌跡和縱向軌跡進行分別優化。坐標系下的運動規劃的輸出式一個軌跡，軌跡是一個時間到位置的函數，即t->(x，y)。當求解這個函數時，其複雜度較高且計算量大，所以將規劃分成橫向規劃和縱向規劃，會有顯著的特點。
Frenet坐標系
由於真實世界中的道路都是彎曲的，為了簡化求解優化問題的參數表達，在自動駕駛中通常採用Frenet坐標系。笛卡爾坐標系下的主車位置以及障礙物位置被投影到Serret-Frenet坐標系下，以車輛前進方向為Station坐標軸，車輛前進方向的法線方向為Lateral坐標軸，車輛路徑規劃將在Station-Lateral坐標系（S-L）下進行。
因為在公路行駛中，我們總是能夠簡單的找到道路的參考線（即道路的中心線），那麼基於參考線的位置的表示就可以簡單的使用縱向距離S（即沿著道路方向的距離）和橫向距離L（即偏離參考線的距離）來描述。
橫縱向解耦優化
橫向規劃就是行車方向上的規劃，可以看成是如何打方向盤的規劃，它決定了軌跡的形狀。
這個問題通常的解法分兩種：
一種是無車道的場景，比如在Free Space（自由空間）中規劃或者泊車之類的場景，車輛一般處在低速行駛狀態，缺乏車道線等先驗信息，業界大多都用搜索等路徑生成的方式去處理無車道場景。
另一種是有車道的情況。雖然可以參考車道線信息，但是規劃上想輸出s→(x,y)函數，難度並不小。常見的做法是離線生成參考線，隨後就可以將求解s→(x,y)的問題變為求解s→L的問題，L是指車輛在這個參考線上的橫向偏移量。確定參考線後，通過把參考線離散化，採一些點出來，那麼橫向規劃問題就轉化為求解一個離參考線偏移距離的一個問題，即轉化成s→L的問題。其計算約束是車輛行駛不跨越邊界，避免碰撞，而優化的目標是要離參考線近，要離障礙物遠，曲率不大，曲率變化率不大等。
縱向規劃本質是對車輛在設定好的路徑上的速度規劃，決定了車輛在整個軌跡上的運動過程。求解這類優化問題，第一個約束是遵守交規（信號燈、限速、停車讓行等），第二個約束是避免碰撞，第三個約束是乘坐舒適，也意味著車輛的速度變化率不大，加速度變化率不大，行駛速度也要儘量快一點（限速內）等。
[1] Bacha A, Bauman C, Faruque R, et al. Odin: Team victortango's entry in the darpa urban challenge[J]. Journal of field Robotics, 2008, 25(8): 467-492.
[2] Schwarting W, Alonso-Mora J, Rus D. Planning and Decision-Making for Autonomous Vehicles[J]. Annual Review of Control Robotics & Autonomous Systems, 2018, 1(1): annurev-control-060117-105157.
[3] Paden B, Cap M, Yong S Z, et al. A Survey of Motion Planning and Control Techniques for Self-driving Urban Vehicles[J]. IEEE Transactions on Intelligent Vehicles, 2016, 1(1):33-55.
[4] Mariusz Bojarski, Davide Del Testa, Daniel Dworakowski, et al. End to End Learning for Self-Driving Cars. https://arxiv.org/abs/1604.07316.
[5] Heuristics [EB/OL]. [2016-07-22].
http://theory.stanford.edu/~amitp/GameProgramming/AStarComparison.html.
[6] Montemerlo M, Becker J, Bhat S, et al. Junior: The stanford entry in the urban challenge[J]. Journal of field Robotics, 2008, 25(9): 569-597.
[7] Olsson M. Behavior Trees for decision-making in Autonomous Driving[M]. 2016.
[8] J. Ziegler and C. Stiller, 「Spatiotemporal state lattices for fast trajectory planning in dynamic on-road driving scenarios,」 in Intelligent Robots and Systems, 2009. IROS 2009. IEEE/RSJ International Conference on. IEEE, 2009, pp. 1879–1884.
[9] J. Funke, P. Theodosis, R. Hindiyeh, G. Stanek, K. Kritatakirana, C. Gerdes, D. Langer, M. Hernandez, B. Muller-Bessler, and B. Huhnke, 「Up to the limits: Autonomous audi tts,」 in Intelligent Vehicles Symposium (IV), 2012 IEEE, June 2012, pp. 541–547.
[10] W. Xu, J. Wei, J. Dolan, H. Zhao, and H. Zha, 「A real-time motion planner with trajectory optimization for autonomous vehicles,」 in Robotics and Automation (ICRA), 2012 IEEE International Conference on, May 2012, pp. 2061–2067.
[11] Werling M, Ziegler J, Kammel S, et al. Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame[C]. //Robotics and Automation (ICRA), 2010 IEEE International Conference on. IEEE, 2010.
總結自動駕駛規劃的一些相關知識，加深自己對知識的鞏固，也希望能和大家一起探索。
如果覺得有用，各位路過的大佬點個關注、在看，茫茫人海相遇不易~
如果存在疑問或者覺得汽車人寫點欠妥，後臺加微信交流哇。
獲取方式很簡單，直接在我公眾號回復關鍵字【規劃】或者添加微信，即可獲取！
獲取方式很簡單，直接在我公眾號回復關鍵字【規劃】或者添加微信，即可獲取！
獲取方式很簡單，直接在我公眾號回復關鍵字【規劃】添加微信，即可獲取！
掃碼關注我
微信號 : Automan_Notes
一個汽車人的成長和實踐歷程