聊聊無人駕駛技術中的路由尋徑

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

---

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

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

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

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

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

例如Lane二、Lane五、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 分別表明在這條路由上的起始縱向距離和結束縱向距離。

和普通的谷歌或者百度導航不一樣，無人車路由尋徑所考慮的不只是路徑的長短、擁塞狀況等，還須要考慮無人車執行某些特定行駛動做的難易程度。

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

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

通常來講，在不考慮倒車狀況時，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」，當出現須要緊急避讓，或者周圍交通狀況致使無人車沒法執行當前的路由尋徑結果時，無人車會按照安全第一的原則繼續行駛，而且發起從新路由尋徑的請求。

無人駕駛是一個複雜的系統，涉及的技術點種類多且跨度大，入門者經常不知從何入手。本書首先宏觀地呈現了無人駕駛的總體技術架構，概述了無人駕駛涉及的各個技術點。在讀者對無人駕駛技術有了宏觀認識後，本書深刻淺出地講解了無人駕駛定位導航、感知、決策與控制等算法，以及深度學習在無人駕駛中的應用等多個主要技術點。本書的做者都是無人駕駛行業的從業者與研究人員，有着多年無人駕駛及人工智能技術的實戰經驗。

瞭解本書詳情：京東、噹噹