改進A*算法在無人船路徑規(guī)劃中的應用

趙春宇,姜 皓,徐茂竹,滿偉俊,楊偉明,陳范模

(1.上海交通大學 電子信息與電氣工程學院,上海 201100;2.上海十方生態(tài)園林股份有限公司,上海 201100)

無人船作為一種新型機器人,在城市水域的水質監(jiān)測以及水體管養(yǎng)等領域發(fā)揮著重要的作用[1-3]。路徑規(guī)劃作為機器人的核心能力,近年已有諸多學者在該方面提出了多種傳統(tǒng)算法,如人工勢場法[4]、蟻群算法[5]、RRT算法[6]和A*算法等。其中,A*算法因在靜態(tài)環(huán)境規(guī)劃的應用中簡單有效、對不同地圖的適應性強以及在機器人路徑規(guī)劃中應用廣泛等特點,很多學者依據A*算法作出改進,提出更加有效的算法[7-10]。目前,針對無人船的路徑規(guī)劃聚焦在海洋領域。余必秀等[11]針對傳統(tǒng)A*算法規(guī)劃的路徑在繞過障礙物后繼續(xù)前進的路徑與原規(guī)劃路徑相差過大的問題,提出在原代價函數基礎上增加一項和當前點到原繪畫航線垂直距離相關的代價值,以達到在避障后快速回到原路徑的效果。呂霞付等[12]使用改進的A*算法解決無人船在復雜環(huán)境下遍歷所有區(qū)域時陷入死區(qū)的問題,并降低了全覆蓋規(guī)劃的重復率和路徑步數。Lee等[13]在改進路徑規(guī)劃算法時考慮了無人船本身的轉彎能力,在滿足航向角約束的同時為實際行駛生成更可靠的航路點。Singh等[14]考慮了無人船在行進過程中周圍環(huán)境的作用,包括移動障礙物以及水流干擾作用,并對生成軌跡的安全性和平滑性進行了討論。

綜上,目前的研究工作主要集中在優(yōu)化路徑總長度以及在外界干擾下路徑的魯棒性等方面,而針對用于河道監(jiān)測的無人船研究起步較晚,針對其在城市水域中的路徑規(guī)劃研究則更匱乏。基于此,首先分析了傳統(tǒng)A*算法的不足;然后針對無人船在城市水域中航行時的特點對A*算法進行優(yōu)化;最后選取典型的城市河道環(huán)境模型,通過Python進行可視化仿真對比,使無人船航行時不僅能夠盡量遠離障礙物,而且路徑更加平滑。

1 傳統(tǒng)A*算法

A*算法是一種經典的路徑搜索和圖形遍歷算法,常用于二維柵格地圖的路徑規(guī)劃。通過將啟發(fā)式搜索和常規(guī)的圖搜索法結合在一起,在減少了搜索范圍的同時又能保證規(guī)劃出來的路徑為最短路徑。A*算法通過一個估值函數來引導路徑的搜索,估值函數為

F(N)=G(N)+H(N)

(1)

式中:F(N)為節(jié)點N的綜合優(yōu)先級,選擇下一個路徑子節(jié)點的方法是從當前節(jié)點的相鄰節(jié)點中選出優(yōu)先級最高(F(N)最低)的點;G(N)為從起點移動到節(jié)點N的實際距離代價,即節(jié)點N的G(N)等于其父節(jié)點的G(N)加上其父節(jié)點到節(jié)點N的距離代價;H(N)為從節(jié)點N移動到終點的預估距離代價,通常取節(jié)點N到終點的歐幾里得距離作為預計代價,設終點為P,即

(2)

2 改進的A*算法

水質監(jiān)測無人船及工作環(huán)境如圖1所示。無人船需要裝載水質檢測傳感器進入這些狹窄河道作業(yè)。傳統(tǒng)A*算法規(guī)劃出來的路徑有兩個明顯的缺點:1) 由于傳統(tǒng)A*算法的估值函數僅考慮了路徑在距離上的最優(yōu)解,其規(guī)劃出來的路徑通常和障礙物邊緣貼得很近。由于在建立地圖過程中坐標轉換的誤差以及障礙物形狀測量上的誤差,路徑在移動端地圖上繪制時,甚至會向障礙物內部一定程度地偏移。無人船如按照這樣的路徑行駛,則很容易和障礙物相撞。2) 傳統(tǒng)A*算法由于只能遍歷柵格中的某一個固定點,其轉折方向只能是45°的整數倍,致使其規(guī)劃的路徑會產生一些不必要的轉折。這些轉折不利于無人船的運動控制,會讓無人船消耗更多資源。筆者針對這兩方面缺陷,提出了以下兩種改進方法。

圖1 水質監(jiān)測無人船Fig.1 Water quality monitoring unmanned surface vehicle

2.1 估值函數改進

針對規(guī)劃的路徑貼近障礙物的問題,對傳統(tǒng)A*算法的估值函數作出改進。在原有估值函數的基礎上,引入新的估值函數D(N)。將D(N)定義為附近的障礙物對節(jié)點N的影響程度,D(N)越大,代表節(jié)點N間隔障礙物越遠。新的估值函數的計算式為

F(N)=G(N)+H(N)-D(N)k

(3)

式中k為遍歷節(jié)點的安全性對路徑搜索的影響權重。k值越大,規(guī)劃的路徑距障礙物邊界越遠,路徑安全性越高。k值大小根據多次實驗的結果確定,不斷調整k值使得規(guī)劃的路徑既與障礙物之間留有一定的安全空間,又在距離上接近最短路徑。對于不同的地圖,數據k的選取不同,將k選取為2。

柵格搜索方式如圖2所示。D(N)的計算方法是以節(jié)點N為中心,逐層向外遍歷周圍的節(jié)點,設定搜索的節(jié)點最遠與節(jié)點N相距m個柵格。

圖2 柵格搜索方式Fig.2 Raster search mode

設當前訪問的節(jié)點與節(jié)點N相距d個柵格,d從1開始取值,即從第1層柵格開始搜索,若出現含有障礙物的禁航柵格,則返回1,并跳出當前循環(huán);若未出現禁航柵格,則d加1,繼續(xù)搜索第2層柵格,重復前面步驟;若m層柵格仍未出現禁航柵格,則返回m。對下一個節(jié)點重復上述步驟。

若在路徑搜索的過程中計算D(N),則會延長路徑規(guī)劃完成的時間。故在程序運行開始前,對存儲柵格地圖的二維矩陣進行預處理,即遍歷柵格地圖中所有可航行的柵格點,計算它們的D(N)并存儲到二維矩陣中。

2.2 遍歷方式改進

傳統(tǒng)A*算法通常把柵格的一個端點作為節(jié)點來訪問,遍歷節(jié)點N的周圍節(jié)點,只訪問8個離散的柵格端點,具體方式如圖3所示。

圖3 傳統(tǒng)A*算法探索鄰域方式Fig.3 Traditional A* algorithm exploring neighborhood mode

當起點和終點的連線與柵格的邊線形成的夾角不是45°的整數倍時,會產生多余的轉折,此時規(guī)劃的路徑和最短路徑的對比如圖4所示。

圖4 傳統(tǒng)A*算法規(guī)劃路徑與最短路徑比較Fig.4 Comparison between traditional A* algorithm and shortest path

為了在遍歷當前節(jié)點的子節(jié)點時不僅只訪問其周圍8個柵格的端點,Ferguson等[15]提出了Field D*算法,對柵格進行近似線性插值,提高路徑規(guī)劃規(guī)劃結果的平滑性。在這種線性插值的思想上,根據無人船柵格地圖實際情況來進行改進。重新定義節(jié)點位置的方法如圖5所示。將柵格邊線上的任意點,而不是柵格的端點,作為節(jié)點來訪問。

圖5 重新定義節(jié)點位置Fig.5 Redefines node positions

設當前節(jié)點為N(x0,y0),當節(jié)點N與某柵格邊線之間的柵格為可通行柵格時,將柵格邊線上估值函數值最小的點作為節(jié)點N的子節(jié)點。設邊線端點為X1和X2,該邊線上有一任意點X距X1距離為d,設點X到起點的實際距離代價為G(X),點X到終點的預計距離代價為H(X),點X的G(X)和H(X)的計算式為

(4)

在實際的路徑規(guī)劃算法中,分成兩類情況來討論柵格邊線上估值函數最小節(jié)點的位置:節(jié)點N是柵格的端點和節(jié)點N位于柵格邊線上。

第一類情況如圖6所示,節(jié)點N的橫縱坐標均為整數。設X1為邊線距離節(jié)點N較近的端點(橫坐標或者縱坐標與節(jié)點N相同),X2為另一個端點;X為邊線X1X2上的一個任意點;d為點X到X1的距離;柵格為邊長是1的正方形;G(X)為從起點移動到點X的實際距離代價;H(X)為從點X移動到終點的預計代價。

圖6 節(jié)點N位于柵格邊線交點Fig.6 Node N located at the intersection of grid edges

根據不同情況討論X的位置,具體情況如下：

(5)

3) 當H(X1)和H(X2)的值上述兩種情況均不滿足時,點X取點X1的位置。

第二類情況如圖7所示,節(jié)點N的橫坐標含有小數,縱坐標為整數(或者橫坐標為整數,縱坐標含有小數),與之相鄰的6條柵格邊線有兩種類型：第一種柵格邊線有一個端點的橫坐標或縱坐標與節(jié)點N的橫坐標或縱坐標相同;第二種柵格邊線兩個端點的橫縱坐標均與節(jié)點N的橫縱坐標不相同。

圖7 節(jié)點N位于柵格邊線上Fig.7 Node N on the grid edge

當柵格邊線如圖7(a)所示時,設X1為柵格邊線X1X2上距離節(jié)點N較近的端點,X2為另一個端點;d為邊線X1X2上有一任意點X距離點X1的距離;t為節(jié)點N與點X1的距離;G(X)為從起點移動到點X的實際距離代價;H(X)為從點X移動到終點的預計代價。根據不同情況,具體討論為

(6)

3) 當H(X1)和H(X2)的值不滿足上述兩種情況時,點X取點X1。

當柵格邊線如圖7(b)所示時,設m為邊線X1X2上有一任意點X與點X2的距離;t為節(jié)點N的縱坐標與點X2的縱坐標差值的絕對值;G(X)為從起點移動到點X的實際距離代價;H(X)為從點X移動到終點的預計代價。通過推導得出的結果為

3) 當H(X1)和H(X2)的值不滿足上述兩種情況時,點X取在柵格邊線X1X2上,距點X2的距離m計算式為

(7)

2.3 時間復雜度分析

改進后的A*算法流程圖如圖8所示。其中計算障礙估值函數為離線任務,在構建地圖后進行初始化。假設柵格大小為M×M,該過程的時間復雜度為O(M2),屬于地圖的一部分信息,不影響A*算法在線規(guī)劃時的速度。傳統(tǒng)遍歷節(jié)點的方式改進為遍歷節(jié)點之間邊線的方式,遍歷最大數量由M2變?yōu)?M2,平均時間復雜度為O(2M·log(2M)),相較于傳統(tǒng)A*算法的平均時間復雜度O(M·logM)屬于同一數量級,不會引入大量的時間開銷。

圖8 改進A*算法流程Fig.8 Flow chart of improved A* algorithm

3 仿真及實驗結果

通過Python進行可視化仿真,選取典型的城市河道環(huán)境模型,路徑規(guī)劃結果如圖9所示。傳統(tǒng)A*算法規(guī)劃的路徑上多個節(jié)點緊貼障礙物邊緣。由于無人船對電機的控制存在誤差,且柵格地圖和實際地圖存在一定的偏差,無人船按照這樣的路徑行駛極易撞上障礙物。

圖9 不同算法的路徑規(guī)劃結果Fig.9 Path planning results of different algorithms

兩種路徑規(guī)劃算法的對比如表1所示。改進A*算法規(guī)劃的路徑不僅始終與障礙物保持安全距離,在河道的中央處行駛,保證了無人船航行的安全性,而且無效轉折次數明顯減少,提高了路徑搜索的質量。

表1 仿真結果對比

為了驗證路徑規(guī)劃算法的可行性,將路徑規(guī)劃算法實際部署到船載主機樹莓派上,并在移動端的電子地圖上實時繪制無人船的位置,生成路徑,以此來評估規(guī)劃路徑的長度、安全性和平滑程度。

無人船實際航行路線如圖10所示。其中,圖10(a)為傳統(tǒng)A*算法的路徑規(guī)劃結果;圖10(b)為改進A*算法的路徑規(guī)劃結果。在移動端地圖中,設置相同的起始點,分別用傳統(tǒng)A*算法和改進A*算法進行路徑規(guī)劃。由圖10(a,b)可知:改進以后的規(guī)劃路線距離河岸更遠,安全性更高,路徑更為平滑。

圖10 無人船實際航行路線Fig.10 Actual route of unmanned surface vehicle

4 結 論

根據無人船工作環(huán)境的特點,分析了傳統(tǒng)A*算法在規(guī)劃路徑時存在的缺陷,通過引入新的障礙距離評價函數D(N)和遍歷柵格邊線的方式對A*算法進行了改進。仿真和實驗結果表明:筆者方法相較于傳統(tǒng)A*算法規(guī)劃的路徑能夠和障礙物保持一定安全距離,同時路線更加平滑,轉折次數明顯減少。改進的A*算法提高了路徑的安全性,為無人船在城市河道作業(yè)時的路徑規(guī)劃問題提供了新思路。