來自矽谷的無人駕駛一線技術

• 2019 年 10 月 8 日
• 筆記

我們已經拉開了全自動無人駕駛的序幕，本文選自《第一本無人駕駛技術書（第2版）》，本書來自矽谷無人駕駛一線技術團隊的實踐經驗，為無數讀者揭開無人駕駛技術的神秘面紗 。

作為一個複雜的軟、硬體結合系統，無人車的安全可靠運行需要車載硬體、感測器集成、感知預測，以及控制規劃等多個模組的協同配合工作。

本文介紹的路由尋徑模組（也稱為尋徑模組），其作用可以簡單理解為實現無人駕駛軟體系統內部的導航功能，即在宏觀層面上指導無人駕駛軟體系統的控制規劃模組按照什麼樣的道路行駛，從而實現從起始點到目的地點的目的。

（值得注意的是，這裡的路由尋徑雖然在一定程度上與傳統的導航類似，但其在細節上緊密依賴於專門為無人車導航繪製的高精地圖，所以和傳統的導航有本質不同。）

普通的Google或者百度導航解決的是從A點到B 點的道路層面的路由尋徑問題。普通導航的底層導航元素最小可以具體到某一條路的某一個車道。這些道路和車道都是符合自然的道路劃分和標識的。無人車路徑規劃的尋徑問題，雖然也是要解決從A 點到B 點的路由問題，但由於其輸出結果並不以為實際的駕駛員所使用為目的，而是給下游的行為決策和動作規劃等模組作為輸入的，其路徑規劃的層次要更加深入到無人車使用的高精地圖的車道級別。

無人車路由尋徑模組的高精地圖道路級別路由尋徑

上圖的箭頭線段代表高精地圖級別的道路劃分和方向。Lane1,Lane2,…,Lane8 構成了一條路由導航輸出的路由片段序列。可以看到，無人車地圖級別的Lane 劃分並非和實際的自然道路劃分對應。

例如Lane2、Lane5、Lane7 都代表了由地圖定義繪製的「虛擬」轉向Lane。類似地，一條較長的自然道路也可能被劃分為若干個Lane（例如Lane3 和Lane4）。

路由尋徑模組的輸出嚴格依賴無人車高精地圖的繪製。在高精地圖定義的路網（Road Graph）劃分的基礎上，以及在一定的最優策略定義下，路由尋徑模組需要解決的問題是計算出一個從起點到終點的最佳道路行駛序列：

{(lane,start_position,end_position)}

我們將 (lane,start_position,end_position)i 稱作一個路由片段 （Routing Segment），所在的道路由Lane 標識，start_position 和end_position 分別代表在這條路由上的起始縱向距離和結束縱向距離。

和普通的Google或者百度導航不同，無人車路由尋徑所考慮的不僅是路徑的長短、擁塞情況等，還需要考慮無人車執行某些特定行駛動作的難易程度。

例如，無人車路由尋徑可能會盡量避免在短距離內進行換道，出於安全考慮，短距離內需要的換道空間可能比正常的駕駛距離所需要的換道空間更大。從安全第一的原則出發，無人車路由尋徑模組可能會給「換道」路徑賦予更高的權重（cost）。

我們可以把無人車在高精地圖的Lane 級別尋徑問題，抽象成一個在有向帶權圖上的最短路徑搜索問題。路由尋徑模組首先會基於Lane 級別的高精地圖，在一定範圍內所有可能經過的Lane 上進行分散「撒點」，我們稱這些點為「Lane Point」。這些點代表了對無人車可能經過的Lane 上的位置的抽樣。這些點與點之間，由有向帶權的邊進行連接。

①換道場景中Lane Point 間cost 的設置

②無人車尋徑基於Lane Point 的有向帶權圖上的

最短路徑問題抽象

一般來說，在不考慮倒車情況時，Lane Point 之間是沿著Lane 行進方向單向可達的關係。連接Lane Point 之間邊的權重，代表了無人車從一個Lane Point 行駛到另一個點的潛在代價。Lane Point 的取樣頻率需要保證即使是地圖上被分割比較短的Lane，也能得到充分的取樣點。Lane Point 之間的連接具有局部性。同一條Lane 上面的點是前後連接的，值得注意的是，不同Lane 之間的Lane Point 也有相互連接的關係。一個明顯的例子是，在轉彎時，轉彎Lane 的第一個Lane Point 和其前驅Lane 的最後一個Lane Point 自然連接在一起。另外，兩條相鄰的平行Lane，在可以合法進行換道的位置（比如虛線位置），其對應位置的Lane Point 也可能互相連接。

圖① 給出了換道場景中Lane Point 間cost 的設置：在任何一個Lane 的內部取樣點Lane Point 之間，我們把cost設置為1；考慮到右轉的代價低於左轉，我們把直行接右轉的cost 設置為5，直行接左轉的cost 設置為8，右轉Lane 內部Lane Point 連接cost 設置為2，左轉Lane 內部LanePoint 連接cost 設置為3。在圖①的換道場景中，兩條平行可以換道的Lane，每條Lane 內部的連接cost 依然為1，但為了突出換道的代價，我們把相鄰Lane 之間的連接權重設置為10。

按照圖①設置的cost，在圖②的一個路網（Road Graph）下，對比從A 到B兩個可能不同的路由路徑Route 1 和Route 2。其中Route 1 對應從L1 出發，在左下角的路口處直行接L4，之後右轉（L5），再繼續直行經過L10 和L11，最後直行經過L12 到達目的地；Route 2 對應同樣從A 出發的L1，但在左下角的第一個路口處右轉接L2，然後直行並且從L3 換道至L6，在右下角路口處經過L7 左轉接直行（L8），最後在右上角的路口處右轉（L9）進入最後目的地B 所在的L12。即使Route 2 的實際物理長度小於Route 1，按照圖①設置的cost，無人車路由尋徑也會偏向於選擇總cost 較小的Route 2

（假設屬於不同Lane 的Lane Point 之間的連接cost 除了圖①所示外均為1，讀者可以驗證Route 1 的總cost 為22，Route 2 的總cost 為44）。

針對上文的無人車路由尋徑有向帶權圖的最短路徑問題，我們這裡介紹一種常見的無人車路由尋徑演算法：Dijkstra 演算法。

Dijkstra 演算法是一種常見的圖論中的最短路徑演算法，由Edsger W. Dijkstra 在1959 年發表。給定一個圖中的源節點（Source Node），Dijkstra 演算法會尋找該源節點到所有其他節點的最短路徑。結合無人車路由的Lane Point 場景，演算法的描述如下。

（1）從高精地圖的路網數據介面中讀取一定範圍的地圖Lane 連接數據，按照上文中所述進行Lane Point 抽樣並構建Lane Point Graph。無人車主車（也稱作Master Vehicle）所在Lane 上最接近無人車主車的Lane Point 為源節點，目的地所在Lane 上最接近目的地的Lane Point 為目的節點。設置源節點到其他節點（包括目的節點）的距離為無窮大（inf），源節點到自身的距離為0。

（2）當前節點設置為源Lane Point，設置其他所有Lane Point 為unvisited（未訪問）並將它們放到一個集合中（Unvisited Set），同時維護一個前驅節點的映射prev_map，保存每一個visited 的Lane Point 到其前驅Lane Point 的映射。

（3）從當前Lane Point 節點出發，考慮相鄰能夠到達的所有未訪問的Lane Point，計算可能的距離（Tentative Distance）。例如，假設當前Lane Point X 被標記的距離為3，LanePoint X 到Lane Point Y 的距離為5，那麼可能的距離為3+5=8。比較該Tentative Distance和Lane Point Y 的當前標記距離。如果Lane Point Y 的當前標記距離較小，那麼保存Lane Point Y 的當前標記距離不變，否則更新Lane Point Y 的當前標記距離為這個新的Tentative Distance 並且更新prev_map。

（4）對當前Lane Point 的所有連接的unvisited Lane Point 重複步驟（3）的操作，當所有相連接的Lane Point 均被操作過之後，標記當前的Lane Point 為visited，從unvisited的集合中去除。被visited 的Lane Point 的標記距離將不再被更新。

（5）不斷從unvisited 的Lane Point 集合中選取Lane Point 作為當前節點並重複步驟（4），直到我們的目標Lane Point 從unvisited 集合中被去除；或者在一定範圍內的Lane Point 均無法到達（unvisited 集合中最小的Tentative Distance 為無窮大，代表從源Lane Point 無法到達剩下的所有unvisited Lane Point）。此時，需要返回給下游模組沒有可達路徑（尋徑失敗），或者重新讀入更大範圍的地圖路網數據，重新開始尋徑的過程。

（6）當找到從A 到B 的最短路徑後，根據prev_map 進行Lane 序列重構。

基於Dijkstra 演算法的Lane Point 有向帶權圖上的路由尋徑演算法的偽碼如下。

其中第2~16 行是典型的用Dijkstra 演算法構建每個源Lane Point 到其他Lane Point 的最小距離表。從第 17~22 行，根據得到的每個節點標記的最小距離映射，通過不斷查找前驅的prev_map 映射重建最短路徑。注意這裡的最短路徑是一個Lane Point 的序列，在第23 行，我們對Lane Point 按照Lane 進行聚類合併最終生成如{(lane,start_position, end_position)i}格式的路由尋徑輸出。

假設根據上文的Lane Point 有向帶權圖生成方法的圖有V 個節點和E 條邊。在使用最小優先隊列（minimum priority queue）來優化第10 行的最小距離查找的情況下，Dijkstra 的路由尋徑演算法複雜度可以達到O(丨E丨+丨V丨log丨V丨)。

其他：A*演算法

另外，還有一種在無人車路由尋徑中常用的演算法是 A*演算法。A*演算法是一種啟發式的搜索演算法。A*演算法在某種程度上和廣度優先搜索（BFS）、深度優先搜索（DFS）類似，都是按照一定的原則確定如何展開需要搜索的節點樹狀結構。A*可以認為是一種基於「優點」（best first/merit based）的搜索演算法。在《第一本無人駕駛技術書（第2版）》中會對此進行詳細介紹。

在實際的無人車路由尋徑計算問題中，更重要的往往不是演算法的選擇，而是cost 的設置策略。

上文描述的cost 調整是整個路由尋徑策略的精髓，而具體的演算法實現（Dijkstra 或者A*）並不是最重要的。例如，從地圖資訊我們得知某一條道路的某一條Lane非常擁堵，就可以把進入這條Lane 上的Lane Point 之間的連接權重cost 提高；類似地，如果某條Lane 被交通管制不能通行，我們可以相應地把這條Lane 上的Lane Point 設置為互相不可達，從而使得演算法不會去選擇某條特定的Lane。路由尋徑的Lane Point 之間的cost 可以根據不同策略實時靈活調整，為無人車路由尋徑提供支援。考慮到實際的路網數據往往較大，基於Lane Point 有向帶權圖的最短路徑往往是在提前預先載入（preload）的部分地圖路網數據上進行的。如果出現在較小範圍內不可達的情況，則可能需要重新讀入更大的路網和地圖數據重新進行路由尋徑。

對路由尋徑模組產生路由計算的請求，有兩種情況：一種情況是當無人車開始行駛時，由用戶來設置起點和終點，從而觸發路由尋徑請求；另一種情況是，請求是由下游模組發起的。這裡我們討論「強Routing」和「弱Routing」兩種系統設計思想。「強Routing」指的是下游模組（如行為決策及動作規劃）嚴格遵守路由尋徑模組的輸出。例如，路由尋徑模組要求按照某條Lane X 行駛，但感知發現Lane X 上有一輛行駛非常慢的障礙車，在強路由的設計下，無人車會嚴格執行在Lane X 上行駛；但在「弱Routing」的設計下，無人車可能會短暫跨越到相鄰的Lane，超過障礙車輛，再回到Lane X 繼續行駛。無論是「強Routing」還是「弱Routing」，當出現需要緊急避讓，或者周圍交通情況導致無人車無法執行當前的路由尋徑結果時，無人車會按照安全第一的原則繼續行駛，並且發起重新路由尋徑的請求。