技術標籤：地圖距離演算法

無人車尋徑模組和普通的二維度導航不同,無人車Routing所考慮的不僅僅侷限於路徑的長短和擁塞情況等,還需要考慮到無人車執行某些特定行駛動作的難易程度。例如,無人車路由尋徑可能會盡量避免在短距離內進行換道,因為無人車的規劃控制演算法出於安全考慮,需要的換道空間可能比正常的駕駛員駕駛所需要的換道空間更大。從安全第一的原則出發,無人車Routing模組可能會給"換道"路徑賦予更高的權重（Cost）。

我們可以把無人車在高精地圖的Lane級別尋徑問題,抽象成一個在有向帶權圖上的最短路徑搜尋問題。路由尋徑模組首先會基於Lane級別的高精度地圖,在一定範圍內所有可能經過的Lane 上進行分散"撒點",我們稱這些點為"Lane Point"。這些點代表了對無人車可能經過的Lane上的位置的抽樣。這些點與點之間,由有向帶權的邊進行連線, 如圖7-3和圖7-4所示。一般來說,在不考慮倒車這一特殊情況下,Lane Point之間是沿著Lane行進方向單向可達的關係。連線Lane Point之間邊的權重,代表了無人車從一個

Lane Point行駛到另一個點的潛在代價。Lane Point的取樣頻率需要保證即使是地圖上被分割比較短的Lane,也能得到充分的取樣點。Lane Point之間的連線具有區域性性Locality）。自然同一條 Lane上面的點是前後連線的,但值得注意的是,不同Lane之間的Lane Point 也有相互連線的關係。一個明顯的例子是,在轉彎時,轉彎Lane的第一個Lane Point和其前驅Lane的最後一個Lane Point自然連線在一起。另外兩條相鄰的平行Lane,在可以合法進行換道的位置（比如白色虛線位置）,其對應位置的Lane Point 也可能互相連線。圖7-3給出了幾個典型Lane連線場景的Lane Point之間的權重設定:在任何一個Lane的內部取樣點Lane Point之間,我們把cost設定為1;考慮到右轉的代價低於左轉,我們把直行接右轉的cost設定為5,直行接左轉的cost設定為8,右轉Lane內部Lane Poin連線cost設定為2,左轉Lane 內部Lane Point連線cost設定為3。在圖7-3所示的換道場景中,兩條平行可以換道的Lane,每條Lane 內部的連線cost 依然為1,但為了突出換道的代價,我們把相鄰Lane之間的連線權重設定為10。

在一個路網（Road Graph）下,我們來對比從A到B兩個可能不同的路由路徑Route1和Route2。其中Route 1對應從Lane1出發, 在左下角的路口處直行接Lane4,之後右轉Lane5）,再繼續直行經過Lane 10和Lanell, 最後直行經過Lane 12到達目的地;Route2對應同樣從起點A所在的Lane1出發,但在左下角的第一個路口處右轉接Lane2,然後直行並且從Lane3換道至Lane6,在右下角路口處經過Lane7左轉接直行（Lane 8）,最後在右上角的路口處右轉（Lane 9）進入最後目的地B所在的Lane 12。即使Route2的實際物理長度小於Route 1,按照圖7-3設定的cost,無人車Routing也會偏向於選擇總cost較小的Route2假設屬於不同Lane的Lane Point 之間的連線cost除了所述外均為1,讀者可以驗證Route 1的總cost為22, Route2的總 cost為44）。

Ⅰ無人車典型的路由尋徑演算法

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

（1）從高精地圖的路網資料介面中讀取一定範圍的地圖 Lane 連線資料,按照7.2.1 節所述進行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,Lane PointX到Lane Point Y的距離為5,那麼可能的距離為3+5=8。比較該 tentative distance和Y的當前標記距離。如果Y的當前標記距離較小,那麼儲存Y的當前標記距離不變,否則更新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序列重構。

（6）當找到從A到B的最短路徑後,根據prev_map進行Lane序列重構。基於Djkstra演算法的Lane Point有向帶權圖上的Routing演算法偽碼如下所示。其中第2到16行是典型的Dijkstra演算法構建每個源Lane Point 到其他每個Lane Point的最小距離表。之後從第17至22行,根據得到的每個節點標記的最小距離對映,通過不斷查詢前驅的prev_map對映重建最短路徑。注意這裡的最短路徑是一個Lane Point的序列,在第 23 行我們對 Lane Point按照lane 進行聚類合併最終生成如{(lane,strt position, end_posittion)}格式的Routing輸出。

funtion Dikstra_Routing(LanePoinGraphVE,src,dst)
create vertex set Q
create map dis,prev
for each lane point v in V:
dis[v]=inf
prev[]=nullptr
add v toQ
dis[src]=0
whileQ is not empty:
u=vertex in Qst.dis[4]is the minimum
remove u from Q
for each connected lane point v of u:
candidate=dis[u]+cos(u,v)
If candidate<disr[v]:
dis[]=candidate
prev[]=u;
ret=empty sequence
u=dst
while prev[a]!=nullptr:
insert u at the beginning of ret
u=prev[u]
insert u at the beginning of ret
merge lane point in rer with same lane id and retum the mergedsequence

ⅡA"演算法在路徑規劃上的應用

另一種在無人車路由尋徑中常用的演算法是 A*演算法。A*算是一種啟發式的搜尋演算法。A*演算法在某種程度上和廣度優先搜尋（BFS）、深度優先搜尋（DFS）類似,都是按照一定的原則確定如何展開需要搜尋的節點樹狀結構。A*可以認為是一種基於"優點"（best first/merit based）的搜尋演算法。

A*演算法首先會維護一個當前可能需要搜尋展開的節點集合（openSet）。每次迴圈,A*會從這個 openSet 中選取 cost 最小的節點進行展開來繼續深入搜尋,這個 cost由f(1v)=g(v)+h(v)兩部分組成。在A*演算法的搜尋樹結構中,每個節點v都有一個由源點到該節點的最小cost,記為g(v）;同時每個節點v還對應一個啟發式的cost稱之為heuristic）,記為(）;其中(v)作為一個heuristic,用來估計當前節點v到目標節點的最小cost。當該heuristic h(）滿足一定的屬性時,A*能夠保證找到源節點到目的節點的最短路徑。A*演算法的搜尋樹在每次迴圈中都會展開f(v）=g(v）+(v）最小的節點,直到到達目的節點。

A*的詳細演算法如圖所示。其中演算法第11行的reconstructroute部分類似於Djksra演算法最後的路由重構部分。reconstruct_route 從最後的目的節點出發,通過前驅節點的對映prev_map向前重構出最終的路由尋徑輸出。

A*演算法作為一種啟發式（heuristic based）的搜尋演算法,當h(）的定義滿足admisible 屬性,即(v,dst)不會超過實際的h(y,dst)之間的最小 cost時,總是能找到最短的路徑。當heuristic不滿足這一條件時,A*並不能保證找到最短路徑。有向帶權圖場景下,對於任意兩個Lane PointA和B,一種heuristic啟發函式的定義為:

　　其中dist代表兩個Lane Point之間在地球經緯度或者墨卡託座標系下的距離。

function AStar Routing(LanePoinGraphr(VE), src, dst)
create vertex set closedSet// set of already visited nodes create vertex set openSer//set of nodes o be expanded insert src into openSer
create map gScore,/Score with default value inf
create prev_map with default value nullptr
Score[src]=h(src,dst)
while openSet is not empty:
current=the node v in openSet s.t.Score[] is minimum in openSet if current=dst
return reconstruction_route(prev_map,current)
remove current from openSer
insert current into closedSet
for each neighbor u of current:
if u is in closedSer:
continue;// ignore the neighbor who has already been evaluated candidate_score= gScore[current]+h(current,a)
tu not in openSer:// discovered a
insert u into openSet
else if candidate_score>=gScore(u]:// this is not a better path continue;
prev[u]=current
gScore[u]=candidate_score
fScore[u4]=gsScore[u]+h(u,dsr)