隨博文，黃志堅(jiān)

(上海海事大學(xué) 商船學(xué)院，上海 201306)

0 引 言

無人艇是一種無人操作的水面艦艇，常用于危險(xiǎn)水域搜救或人員無法到達(dá)的水域執(zhí)行探測(cè)，偵察等任務(wù)。無人艇的應(yīng)用極大節(jié)省了人力物力成本，并且可以高效快速執(zhí)行任務(wù)。為使無人艇執(zhí)行任務(wù)時(shí)可以達(dá)到較高的避碰性能以及選擇最優(yōu)的路徑規(guī)劃，本文對(duì)無人艇的航跡規(guī)劃、自主避障做進(jìn)一步的研究。

當(dāng)前智能航行體的路徑規(guī)劃算法主要有遺傳算法、Dijkstra 算法、A*算法、人工勢(shì)場(chǎng)法、蟻群算法等。遺傳算法最早是由美國J.Holland 教授提出的，遺傳算法是模擬自然界中按“優(yōu)勝劣汰”法則進(jìn)行進(jìn)化過程而設(shè)計(jì)的算法，在動(dòng)態(tài)路徑規(guī)劃中得到廣泛應(yīng)用。謝玉龍等[1]針對(duì)傳統(tǒng)遺傳算法解決船舶路徑規(guī)劃問題的不足，提出了一種改進(jìn)的遺傳算法。改進(jìn)算法改變了種群的編碼方式，由二維編碼變?yōu)榛谧鴺?biāo)軸的一維編碼；在傳統(tǒng)遺傳算法的基礎(chǔ)上增加3 個(gè)新的遺傳操作：復(fù)原操作、重構(gòu)操作和錄優(yōu)操作，使算法盡早收斂于全局最優(yōu)解，錄優(yōu)操作保證種群朝著最優(yōu)解方向進(jìn)化。Dijkstra 算法是一種常見的最短路徑算法，由Edsger W.Dijkstra 在1959 年提出。傳統(tǒng)的Dijkstra 算法直接搜索全局空間而不考慮目標(biāo)信息，導(dǎo)致路徑求解時(shí)花費(fèi)的時(shí)間較長，難以滿足快速路徑規(guī)劃的需求。陳家[2]提出一種將蟻群算法和Dijkstra 算法相結(jié)合的復(fù)合算法，應(yīng)用于無人駕駛救助船路徑規(guī)劃中。該復(fù)合算法首先通過Dijkstra 算法尋找出最短路徑，然后利用改進(jìn)的蟻群算法對(duì)所得到的最短路徑進(jìn)行優(yōu)化，使最后得到的路徑更符合實(shí)際情況。陳超等[3]提出一種改進(jìn)的人工勢(shì)場(chǎng)法，通過建立新的引力和斥力場(chǎng)函數(shù)來避免局部最小點(diǎn)問題，并應(yīng)用于水面無人艇的路徑規(guī)劃中。余必秀等[4]提出一種改進(jìn)的A*算法，在原始代價(jià)函數(shù)的基礎(chǔ)上，新增一個(gè)與當(dāng)前點(diǎn)到預(yù)設(shè)航線的垂直距離相關(guān)的代價(jià)值，并將改進(jìn)后的算法應(yīng)用于無人航道測(cè)量船的路徑規(guī)劃中，使無人航道測(cè)量船在避開障礙物之后更快地回到預(yù)設(shè)航線。陳立家等[5]將改進(jìn)的蟻群算法應(yīng)用于船舶航行路徑搜索中，提出一種多約束條件下航行綜合成本最低的最優(yōu)航線生成算法，可以在多約束條件下規(guī)劃出最優(yōu)航線。王紅衛(wèi)等[6]提出一種平滑A*算法，能處理不同柵格規(guī)模下、障礙物隨機(jī)分布的復(fù)雜環(huán)境下移動(dòng)機(jī)器人的路徑規(guī)劃問題。

A*算法是一種啟發(fā)式的搜索算法，是求解最短路徑最有效的直接搜索方法，同時(shí)也適用于路徑的二次規(guī)劃。由于A*算法中為了處理復(fù)雜流程需要大量數(shù)學(xué)計(jì)算和理論推導(dǎo)，從而導(dǎo)致A*算法規(guī)劃的路徑存在折線、轉(zhuǎn)折多的問題，這使得在仿真實(shí)驗(yàn)中，規(guī)劃的路徑與障礙物距離過近，極易引發(fā)碰撞，存在極大的安全隱患。針對(duì)此問題，本文以柵格法環(huán)境建模為基礎(chǔ)，當(dāng)航行體行至障礙物附近時(shí)，對(duì)障礙物尖角檢測(cè)，然后進(jìn)行路徑平滑處理，使得航行體與障礙物始終處于安全距離的范圍內(nèi)。對(duì)比結(jié)果表明，改進(jìn)后的A*算法規(guī)劃路徑優(yōu)于傳統(tǒng)的A*算法。

1 基于A*算法進(jìn)行路徑規(guī)劃的基本原理

1.1 A*算法原理

A*算法是對(duì)估價(jià)函數(shù)加上一些限制后得到的一種啟發(fā)式搜索算法[7-8]，啟發(fā)式搜索可以有效地避免無效的搜索路徑，提高搜索效率。A*算法路徑搜索步驟如下：

新建Closed 表和Open 表，并進(jìn)行初始化。將初始節(jié)點(diǎn)s 添加到新建的Open 表。如果Open 表為空則表示失敗并且退出，否則取最小節(jié)點(diǎn)F 作為當(dāng)前考察點(diǎn)x。從Open 表中將x 移入Closed 表。如果x 是目標(biāo)節(jié)點(diǎn)，那么確定找到最優(yōu)解并且退出，否則擴(kuò)展x 并生成繼節(jié)點(diǎn)n?？疾靫 的所有的繼節(jié)點(diǎn)n。

1）所有的繼節(jié)點(diǎn)n 都有g(shù)(n) = g(x) + g(x, n)；

2）創(chuàng)建一個(gè)新的指針，將n 返回s；

3）如果n 是Open 表的舊節(jié)點(diǎn)，則把舊節(jié)點(diǎn)標(biāo)記成o，并且把n 加入到x 的字節(jié)點(diǎn)表里。若f(n) ＜ f(o)，則f(o) = f(n)。如果n 沒在Open 表，那么將判斷它是否在Closed 表。

4）如果n 是Closed 表的舊節(jié)點(diǎn)，則把它標(biāo)記為o，把節(jié)點(diǎn)n 加入到x 的子節(jié)點(diǎn)表中。若f(n) ＜ f(o)，則f(o) = f(n)。否則將其加入到Open 表和x 的后繼節(jié)點(diǎn)；第5 步：算出F 值，并返回到第3 步，繼續(xù)執(zhí)行。

5）算出F 值，并返回到第3 步，繼續(xù)執(zhí)行。

采用A*算法無人艇的路徑規(guī)劃，首先需要建立A*優(yōu)化函數(shù)[9]： f( n)=g(n)+h(n)，在A*算法中給出如下定義：

O 為存放等待擴(kuò)展節(jié)點(diǎn)集合；C 為存放已擴(kuò)展過節(jié)點(diǎn)集合；Ss為起點(diǎn)；Te為目標(biāo)點(diǎn)；Oi為障礙物柵格；Oobs為障礙柵格的集合；g(n)為初始節(jié)點(diǎn)Ss到n 的實(shí)際移動(dòng)距離；

利用柵格地圖和八鄰域節(jié)點(diǎn)擴(kuò)展法，將無人艇從當(dāng)前節(jié)點(diǎn)k 到目標(biāo)點(diǎn)Te的Euclidean 距離作為啟發(fā)式函數(shù)：

1.2 A*算法流程

圖1 為A*算法的流程圖，通過下列程序的執(zhí)行，得到所規(guī)劃的初始路徑。

圖 1 傳統(tǒng)A*算法流程圖Fig.1 Flowchart of traditional A* algorithm

2 改進(jìn)A*算法原理與實(shí)現(xiàn)

2.1 改進(jìn)A*算法

為了處理傳統(tǒng)算法規(guī)劃路徑的折線多，且易穿越障礙物之間的接觸地帶等問題，采用改進(jìn)后的算法對(duì)路徑做出判斷并進(jìn)行平滑處理。改進(jìn)算法具體步驟如下：

1）定義無人艇的原始路徑為Pi，節(jié)點(diǎn)數(shù)為Pin，改進(jìn)平滑處理后的路徑為Pp。

2）判斷原始路徑的節(jié)點(diǎn)是否等于2。若大于2，則執(zhí)行下一步，否則將原始路徑Pi的值賦給Pp。

3）判斷Pi的節(jié)點(diǎn)是否小于等于Pin。若是則執(zhí)行下一步，否則將Pi上非N 節(jié)點(diǎn)賦值給Pp。

4）調(diào)用線段lc（）所獲得節(jié)點(diǎn)i 和i+2 所在的線段。

5）調(diào)用障礙物集合Ob。

7）將Pi上節(jié)點(diǎn)i+1 置為N。

8）執(zhí)行i=i+1。

9）最終獲得改進(jìn)算法處理后所得的優(yōu)化路徑Pp。

在改進(jìn)后的算法中，新定義2 個(gè)函數(shù)Ps（）和W（）函數(shù)，Ps（）是需要處理的路徑節(jié)點(diǎn)，W（）函數(shù)用來判斷待連接節(jié)點(diǎn)的線段是否存在障礙物，有障礙物，則W（）失敗，即無通路；否則可聯(lián)通。函數(shù)Ps（）具體執(zhí)行步驟如下：

1）定義無人艇的原始路徑Pi，連接點(diǎn)為Pinit，改進(jìn)算法處理后的路徑Prp為空。

2）將連接點(diǎn)的下一個(gè)節(jié)點(diǎn)賦值給當(dāng)前節(jié)點(diǎn)。

3）判斷當(dāng)前節(jié)點(diǎn)是否為空。若是則執(zhí)行下一步，否則連接所有節(jié)點(diǎn)獲得Prp。

4）判斷連接點(diǎn)同當(dāng)前節(jié)點(diǎn)的下一節(jié)點(diǎn)之間是否能夠走通。若是則執(zhí)行下一步，否則返回第2 步。

5）刪除當(dāng)前節(jié)點(diǎn)，連接點(diǎn)的下一個(gè)節(jié)點(diǎn)賦值給當(dāng)前節(jié)點(diǎn)。

函數(shù)W（）具體執(zhí)行步驟如下：

1）定義連接點(diǎn)和當(dāng)前節(jié)點(diǎn)的下一個(gè)節(jié)點(diǎn)；

2）將連接點(diǎn)的下一個(gè)節(jié)點(diǎn)賦值給當(dāng)前節(jié)點(diǎn)；

3）用線段連接連接點(diǎn)與當(dāng)前節(jié)點(diǎn)的下一個(gè)節(jié)點(diǎn)；

4）判斷當(dāng)前線段上是否存在障礙物，若不存在執(zhí)行下一步，若存在則不存在通路退出；

5）存在通路可行。

經(jīng)過改進(jìn)的A*算法用于無人艇路徑規(guī)劃的具體流程圖，如圖2 所示。

2.2 基于改進(jìn)的A*算法路徑規(guī)劃模型

圖 2 改進(jìn)A*算法水面無人艇航跡規(guī)劃流程圖Fig.2 Improved A* algorithm flowchart of route planning for surface unmanned boat

圖 3 改進(jìn)A*算法平滑處理前后的路徑對(duì)比圖Fig.3 Path comparison before and after smoothing with improved A* algorithm

由圖3 可以看出，基于傳統(tǒng)A*算法的規(guī)劃的路徑為曲折的折線，這并不滿足無人艇的實(shí)際航行需求。而經(jīng)過改進(jìn)后的算法所規(guī)劃出的路徑更符合實(shí)際要求。

3 仿真實(shí)驗(yàn)

模型仿真實(shí)驗(yàn)在Matlab R2016b 環(huán)境中進(jìn)行，搭建20×20 的仿真地圖，通過建立直角坐標(biāo)系，定義仿真環(huán)境圖的左下角坐標(biāo)為（1，1）；右上角坐標(biāo)為（20，20）。其中黑色部分表示障礙物，白色部分表示可通行區(qū)域，障礙物模擬水閘、鉆井平臺(tái)，拋錨船只等靜態(tài)障礙物，本文的目的就是找到一條從起點(diǎn)到終點(diǎn)的無碰最優(yōu)航跡。實(shí)驗(yàn)分別在圖4 中簡(jiǎn)單環(huán)境（a）和復(fù)雜環(huán)境（b）進(jìn)行，分別采用傳統(tǒng)A*算法和改進(jìn)后的算法做出最優(yōu)路徑規(guī)劃，并進(jìn)行分組對(duì)比，仿真結(jié)果如圖5 和圖6 所示。

圖 4 仿真環(huán)境圖Fig.4 Diagram of simulation environment

圖 5 簡(jiǎn)單環(huán)境下的路徑規(guī)劃圖Fig.5 Path planning in simple environment

圖 6 復(fù)雜環(huán)境下的路徑規(guī)劃圖Fig.6 Path planning in complex environment

仿真環(huán)境中無人艇起點(diǎn)和終點(diǎn)分別用三角形和圓形圖標(biāo)表示。分別將無人艇在仿真環(huán)境中的起點(diǎn)和終點(diǎn)設(shè)為（1，1），（20，20）。分別使用傳統(tǒng)A*算法和改進(jìn)算法對(duì)水面無人艇在簡(jiǎn)單水域仿真環(huán)境下進(jìn)行仿真。由圖5（a）可以看出，利用傳統(tǒng)A*算法規(guī)劃出的路徑，很容易穿過障礙物的接觸位置以及緊挨著障礙物邊緣航行，并且在航行于障礙物水域中，規(guī)劃路線折線多，這將會(huì)導(dǎo)致無人艇在航行期間存在極大安全隱患，很容易與障礙物的邊緣發(fā)生碰撞?；诟倪M(jìn)的A*算法所規(guī)劃出的安全路徑卻能很好地避免上述問題，從圖5（b）可以看出，在同樣的仿真環(huán)境中，基于改進(jìn)的A*算法所規(guī)劃出的安全路徑完全避開了障礙物的接觸點(diǎn)，并且在路線拐角處進(jìn)行圓弧平滑處理，使得無人艇和障礙物始終保持足夠的安全距離，以保證無人艇的航行安全。

為了更好驗(yàn)證改進(jìn)的A*算法在無人艇的路徑規(guī)劃中的有效性，通過建立更加復(fù)雜的水域模擬仿真環(huán)境（見圖4），再次對(duì)基于傳統(tǒng)A*算法和改進(jìn)A*在無人艇的路徑規(guī)劃應(yīng)用性能進(jìn)行仿真驗(yàn)證。仿真結(jié)果如圖6 所示。結(jié)果表明，基于傳統(tǒng)A*算法的所規(guī)劃出的無人艇航行路線更加顯示了其不能有效通過障礙物之間接觸點(diǎn)和障礙物邊緣的缺陷，而基于改進(jìn)的A*算法在復(fù)雜模擬水域中規(guī)劃路徑依然高效且安全。

改變無人艇在復(fù)雜模擬水域中的起止點(diǎn)位置，將起點(diǎn)和終點(diǎn)改為（1，20），（20，3），仿真結(jié)果如圖7 所示?；诟倪M(jìn)的A*算法在無人艇的路徑規(guī)劃中依然有著比傳統(tǒng)的A*算法所規(guī)劃出的路線更加科學(xué)、安全，能夠更好的滿足水面無人艇航行的實(shí)際需求。

4 結(jié) 語

針對(duì)基于傳統(tǒng)A*算法無人艇路徑規(guī)劃不能安全有效避開障礙物的問題，本文提出一種基于平滑A*算法的無人艇路徑規(guī)劃方法。仿真實(shí)驗(yàn)表明，本文提出的算法可以較好地避開障礙物，并且與障礙物保持有足夠的安全距離。通過在拐點(diǎn)前適當(dāng)位置沿著預(yù)定半徑轉(zhuǎn)向使得水面無人艇的規(guī)劃路徑更加平滑，有效避免了無人艇與障礙物接觸點(diǎn)及邊緣相碰撞的危險(xiǎn)，最終使無人艇規(guī)劃出最短路徑并安全抵達(dá)目的地。

目前基于A*算法在無人艇路徑規(guī)劃的應(yīng)用仍存在一定限制，其只適用于靜態(tài)環(huán)境中的路徑規(guī)劃，而在水面無人艇的實(shí)際航行中，往往會(huì)遇到各種動(dòng)態(tài)的障礙物，如過往船只、浮漂，水域中的大型水生物等，此時(shí)應(yīng)用A*算法進(jìn)行路徑規(guī)劃將會(huì)受到一定限制。通過繼續(xù)改進(jìn)A*算法使其能夠針對(duì)動(dòng)態(tài)障礙物實(shí)現(xiàn)自主避障及安全路徑規(guī)劃將是以后的研究方向。

圖 7 復(fù)雜環(huán)境下的路徑規(guī)劃圖Fig.7 Path planning in complex environment