D star路徑搜索算法

一、簡介

“D*算法”的名稱源自 Dynamic A Star,最初由Anthony Stentz於“Optimal and Efficient Path Planning for Partially-Known Environments”中介紹。它是一種啓發式的路徑搜索算法,適合面對周圍環境未知或者周圍環境存在動態變化的場景。

二、算法介紹

同A*算法類似,D-star通過一個維護一個優先隊列(OpenList)來對場景中的路徑節點進行搜索,所不同的是,D*不是由起始點開始搜索,而是以目標點爲起始,通過將目標點置於Openlist中來開始搜索,直到機器人當前位置節點由隊列中出隊爲止(當然如果中間某節點狀態有動態改變,需要重新尋路,所以纔是一個動態尋路算法)。

2.1 符號表示

本部分主要介紹一下論文中用到的一些符號及其含義。

論文中將地圖中的路徑點用State表示,每一個State包含如下信息:

Backpointer: 指向前一個state的指針,指向的state爲當前狀態的父輩,當前state稱爲指針指向state的後代,目標state無Backpointer。(路徑搜索完畢後,通過機器人所在的state,通過backpointer即可一步步地移動到目標Goal state,GoalState以後用 G表示),b(X)=Y表示X的父輩爲Y。

Tag:表示當前state的狀態,有 New、Open、Closed三種狀態,New表示該State從未被置於Openlist中,Open表示該State正位於OpenList中,Closed表示已不再位於Openlist中。

H(X):代價函數估計,表示當前State到G的開銷估計。

K(X):Key Function,該值是優先隊列Openlist中的排序依據,K值最小的State位於隊列頭 ,對於處於OpenList上的State X,K(X)表示從X被置於Openlist後,X到G的最小代價H(X),可以簡單理解爲。K(X)將位於Openlist的State X劃分爲兩種不同的狀態,一種狀態爲Raise(如果K(X)<H(X)),用來傳遞路徑開銷的增加(例如某兩點之間開銷的增加,會導致與之相關的節點到目標的路徑開銷隨之增加);另一種狀態爲 Lower(如果K(X)<H(X)),用來傳遞路徑開銷的減少(例如某兩點之間開銷的減少,或者某一新的節點被加入到Openlist中,可能導致與之相關的節點到目標的路徑開銷隨之減少)。

kmin:表示所有位於Openlist上的state的最小K值。

C(X,Y) :表示X與Y之間的路徑開銷。

Openlist 是依據K值由小到大進行排序的優先隊列。

2.2 算法描述

搜索的關鍵是state的傳遞過程,即由G向機器人所在位置進行搜索的過程,這種傳遞過程是通過不斷地從當前OpenList中取出K值最小的State來實現的,每當一個State由Openlist中移出時,它會將開銷傳遞給它的鄰居state,這些鄰居state會被置於Openlist中,持續進行該循環,直到機器人所在State的狀態爲 Closed ,或者Openlist爲空(表示不存在到G的路徑)。

算法最主要的是兩個函數, Process-StateModify-Cost ,前者用於計算到目標G的最優路徑,後者用於改變兩個state之間的開銷C(X,Y)並將受影響的state置於Openlist中。

算法的主要流程,在初始時,所有state的t(Tag)被設置爲 New ,H(G)被設置爲0,G被放置於OpenList,然後Process-State函數被不斷執行,直到機器人所處state X由openlist中出隊,然後可以通過機器人的當前state按backpointer指向目標G。當移動過程中發現新探測到的障礙時,Modify-Cost函數立刻被調用,來更正C(°)中的路徑開銷並將受影響的state重新置於openlist中。令Y表示robot發現障礙時所在的state,通過不斷調用Process-State直到kmin≥H(Y),這時表示路徑開銷的更改已經傳播到了Y,此時,新的路徑構建完成。

在這裏插入圖片描述

上圖中L1-L3表示擁有最低K值的X由openlist中移出,如果X爲Lower,那麼它的路徑代價爲最優的。

在L8-L13,X的所有鄰接state都被檢測是否其路徑代價可以更低,狀態爲New的鄰接state被賦予初始路徑開銷值,並且開銷的變動被傳播給每一個backpointer指向X的鄰接state Y(不管這個新的開銷比原開銷大或者小),也就是說只要你指向了X,那麼X的路徑開銷變動時,你的路徑代價必須隨之改變。這裏可能存在由於X路徑開銷變動過大,Y可以通過非X的其他state到達目標且路徑開銷更小的情況,這點在L8-13中並沒有處理,而是放在後續針對Y的process-state函數中,在對Y進行處理時,會將其backpointer指向周圍路徑開銷最小的state。如果X的鄰接State狀態爲New時,應將其鄰接state的backpointer指向X。所有路徑開銷有所變動的state都被置於Openlist中進行處理,從而將變動傳播給鄰接的state。
上述討論的時X爲Lower狀態,接下來討論X爲Raise狀態。

如果X爲Raise,它的路徑開銷H可能不是最優的,在L4-L7中,通過其鄰居state中已經處於最優開銷(即h(Y)≤kold)的節點來優化X的路徑開銷,如果存在更短的路徑,則將X的backpointer指向其neighbor。在L15-L18中,開銷變動傳播到狀態爲New的鄰居state。如果X可以使一個backpointer並不指向X的鄰居state的路徑開銷最小,即Y通過X到目標G的距離更短,但是此時Y的backpointer並不指向X,針對這種情況,可以將X重新置於Openlist中進而優化Y。在L23-25中,如果X可以通過一個狀態爲closed的並不是最理想的鄰居stateY來減小路徑開銷,那麼將Y重新置於Openlist中。

在這裏插入圖片描述
在modify-cost中,更新C(X,Y)並將X重新置於Openlist中,當X通過process-state進行傳播時,會對Y進行開銷計算,h(Y)=h(X)+c(X,Y)

三、算法總結


相比A-star算法,D-star的主要特點就是由目標位置開始向起始位置進行路徑搜索,當物體由起始位置向目標位置運行過程中,發現路徑中存在新的障礙時,對於目標位置到新障礙之間的範圍內的路徑節點,新的障礙是不會影響到其到目標的路徑的。新障礙只會影響的是物體所在位置到障礙之間範圍的節點的路徑。在這時通過將新的障礙周圍的節點加入到Openlist中進行處理然後向物體所在位置進行傳播,能最小程度的減少計算開銷。 路徑搜索的過程我個人感覺其實和Dijkstra算法比較像,A-star算法中f(n)=g(n)+h(n),h(n)在D-star中並沒有體現,路徑的搜索並沒有A-star所具有的方向感,即朝着目標搜索的感覺,這種搜索更多的是一種由目標位置向四周發散搜索,直到把起始位置納入搜索範圍爲止,更像是Dijkstra算法。

水平有限,對原文中的許多地方理解的還不透徹,描述能力也有限,本文更多的像是對論文第二部分進行了簡要的翻譯。不懂的地方建議去原文中查詢。

在網上找到了一個用java寫的D-star的源碼,分享給大家,幫助大家理解。
鏈接:https://pan.baidu.com/s/1G1UtCe_1H8UhNe4hRdFl6A 密碼:yta2

發表評論
所有評論
還沒有人評論,想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章