童心赤，張華軍，郭 航

（武漢理工大學自動化學院，武漢 430070）

（?通信作者電子郵箱zhanghj@whut.edu.cn）

0 引言

近年來，隨著無人平臺在軍事和民用等企業(yè)中的推廣，無人水面艇作為一種能夠在復雜海洋環(huán)境下自主航行并完成各項任務的水面平臺受到了極大關(guān)注［1-3］。路徑規(guī)劃是無人艇自主控制部分的關(guān)鍵要素，其任務是對當前海洋環(huán)境信息進行感知，對收集的信息進行分析計算，實現(xiàn)有效且安全的路徑規(guī)劃。過去，最佳路徑通常與最短路徑相一致，隨著此問題的定義不斷發(fā)展，最佳路徑的選取已與最小行進距離、平均高度、燃油消耗、雷達輻射等諸多因素有關(guān)，因此相關(guān)研究工作通常選擇啟發(fā)式搜索算法來實現(xiàn)路徑規(guī)劃［4］。目前對無人艇路徑規(guī)劃的方法主要有遺傳算法［5-6］、粒子群算法［7-8］、人工勢場算法［9-11］、A*算法［12-14］等，其中遺傳算法與粒子群算法等仿生進化算法能有效利用已獲取的環(huán)境信息，同時又能不斷探索新路徑，避免了復雜的數(shù)學計算。另外人工勢場法通過產(chǎn)生包括排斥力和吸引力的勢場來描述障礙物和目標點對路徑搜索的影響，但在解決大規(guī)模問題時易陷入局部最優(yōu)解。A*算法是一類適用于全局環(huán)境信息已知的路徑規(guī)劃方法，它利用啟發(fā)函數(shù)值來估計任意點到目標點的遠近程度，從而減少搜索空間，提高搜索效率。

目前普遍使用A*算法實現(xiàn)無人艇在不確定的海洋環(huán)境中的路徑規(guī)劃，但該算法生成的路徑只以路徑長度為優(yōu)化目標，沒有考慮路徑的安全性與平滑度等因素。文獻［15］為避免障礙物與無人艇距離過近，對障礙物進行一定的膨脹處理，結(jié)果顯示規(guī)劃的路徑具有較好的安全性，但該方法并不適用于復雜的海況環(huán)境；文獻［16］通過評估與障礙物間應保持的最佳距離建立無人艇安全模型，并采用三次樣條插值算法擬合離散的路徑節(jié)點以獲得平滑連續(xù)的路徑；文獻［17］以三階貝塞爾曲線為基礎(chǔ)設(shè)計了滿足二階幾何連續(xù)的路徑平滑算法，并對平滑后的路徑進行碰撞檢測，仿真結(jié)果表明該算法能夠有效規(guī)劃出安全平滑的路徑。

針對無人水面艇路徑規(guī)劃問題，本文首先結(jié)合電子海圖生成柵格化環(huán)境信息，建立無人艇安全區(qū)域模型并以此作為生成最優(yōu)路徑點的安全距離約束；其次針對常規(guī)A*算法生成的路徑轉(zhuǎn)折次數(shù)多且轉(zhuǎn)折角大等問題，設(shè)計一種多方向A*優(yōu)化算法與路徑平滑算法以規(guī)劃一條距離更短且更有效的受約束的全局最優(yōu)路徑；最后仿真結(jié)果驗證了該改進算法的有效性和可行性。

1 模型建立

1.1 無人水面艇安全區(qū)域建模

為精確描述無人水面艇路徑規(guī)劃時的位置與狀態(tài)，本文所研究的無人艇均為六自由度運動學模型，在航行過程中可產(chǎn)生6 個自由度的運動信息?？紤]到無人艇在航行過程中需要較大的運動空間，在路徑規(guī)劃時不能簡單地當作一個質(zhì)點處理，因此設(shè)計一半徑為R 的圓形區(qū)域包圍無人艇作為生成最佳航路點的安全距離約束。半徑R 代表安全距離，安全距離約束下的路徑規(guī)劃應保證無人艇與障礙物的最短距離大于安全距離，解決了無人艇在實際航行中的安全問題。其搜索原理如圖1所示。

圖1 基于安全區(qū)域的路徑搜索原理Fig.1 Principle of path search based on safe area

1.2 環(huán)境建模

為便于實現(xiàn)路徑規(guī)劃，首先應建立無人艇的航行環(huán)境模型，將實際的海洋環(huán)境預處理為便于在計算機中表示的連接圖，同時保留必要的原始信息。目前常用的環(huán)境建模方法有可視圖法［18］、柵格法［19］、拓撲圖法［20］等，本文采用柵格法將實際電子海圖轉(zhuǎn)化為二進制網(wǎng)格圖，建立柵格化地圖模型，并將以經(jīng)緯度表示的導航數(shù)據(jù)轉(zhuǎn)換到柵格地圖坐標系中。在柵格圖中，定義每一單位柵格狀態(tài)為H(x，y)，其中x、y分別代表柵格的橫向與縱向位置：白色部分H(x，y)=0表示可行區(qū)域；黑色部分H(x，y)=1 表示障礙物，這使得無人艇碰撞的風險度僅取決于柵格狀態(tài)值。對某一海域進行柵格化處理，將環(huán)境信息離散化為一系列的二值化柵格，處理結(jié)果如圖2所示。

圖2 環(huán)境信息柵格處理圖Fig.2 Rasterization processing chart of environment information

為實現(xiàn)無人艇實時路徑導航，現(xiàn)將每一時刻的位置信息（經(jīng)度與緯度值）與柵格節(jié)點一一對應。假設(shè)某一海圖內(nèi)的最大經(jīng)度為maxlon，最小經(jīng)度為minlon，最大緯度為maxlat，最小緯度為minlat，對應的柵格地圖橫向劃分為a 個柵格，縱向劃分為b個柵格，則各經(jīng)緯度對應的具體柵格位置為：

其中：floor 操作為取整運算；x，y 分別為對應柵格的橫向與縱向位置；lon，lat分別為當前位置的經(jīng)度與緯度信息。

2 基于改進A*算法的路徑規(guī)劃

A*算法作為一種啟發(fā)式搜索算法，根據(jù)啟發(fā)函數(shù)以最小代價快速返回最優(yōu)路徑，保證了路徑完整性以及搜索高效性，但是常規(guī)A*算法僅以路徑長度作為啟發(fā)函數(shù)值，規(guī)劃出的路徑并不能有效引導無人艇在復雜海洋環(huán)境下安全平滑的運動。

針對該問題，本文首先根據(jù)無人艇安全區(qū)域模型，建立安全距離約束下的A*啟發(fā)函數(shù)來搜索最短路徑。然后設(shè)計一種多方向A*搜索算法減少并修正不必要的路徑節(jié)點，獲得多方向A*優(yōu)化路徑。最后建立平滑處理安全區(qū)域，在此區(qū)域約束下采用三次樣條插值算法擬合離散的關(guān)鍵路徑節(jié)點以獲得連續(xù)平滑的路徑。

2.1 安全距離約束下的A*啟發(fā)函數(shù)

基于上述構(gòu)建的柵格化地圖模型，采用具有安全距離約束的A*算法以實現(xiàn)路徑規(guī)劃。在柵格地圖中，A*算法采用啟發(fā)函數(shù)f(n)來估計地圖上任意柵格到目標柵格的代價，從而引導搜索方向。函數(shù)f(n)表達式如下：

其中：g(n)為各單元柵格到起始柵格的距離，h(n)為各單元柵格到目標柵格的啟發(fā)式代價。

如圖3 所示，在柵格地圖中，無人艇運動位置可由某一具體柵格表示，將A*搜索方式設(shè)置為八方向搜索，即與當前柵格相鄰的八個柵格作為無人艇下一時刻的可選位置。在安全距離約束下，使用八方向A*算法搜索各柵格節(jié)點，設(shè)置各柵格對應的啟發(fā)式代價為：

式中：DE為柵格到目標柵格E 的距離，l 為柵格到周圍障礙物的最短距離，R 為無人艇安全半徑。l ≥R 代表當前位置無人艇安全區(qū)域內(nèi)不存在障礙物；相反，l ＜R 則代表安全區(qū)域內(nèi)存在障礙物。

圖3 安全距離約束下的八方向搜索原理Fig.3 Principle of eight-directional search under safety distance constraint

根據(jù)式（3），安全距離約束下各柵格的啟發(fā)函數(shù)可定義為：

式中DS為柵格到起始柵格S的距離。

2.2 多方向A*搜索算法

八方向A*搜索算法只能以45°和90°方向作為路徑搜索選項，就整體路徑而言，該搜索方式并不能保證最優(yōu)性。因此以路徑距離最短為優(yōu)化目標對已規(guī)劃出的路徑節(jié)點進行調(diào)整以實現(xiàn)多方向A*搜索，定義某一路徑起點為PS、終點為PE、中間節(jié)點為Pi(i=1，2，…，n)，其具體步驟如下。

步驟1 提取安全距離約束下八方向A*算法規(guī)劃的路徑PS→P1→P2→…→Pi→PE中所有節(jié)點。

步驟2 從起點PS開始判斷節(jié)點Pi(Pi=PS)與剩余節(jié)點的相連路徑Pi→Pi+1(Pi+1=P1，P2，…，PE)是否滿足安全距離約束，即判斷相連路徑上所有節(jié)點到障礙物的距離是否小于安全半徑。記錄滿足約束條件的最靠后節(jié)點Pj1。若Pj1=PE則執(zhí)行步驟4，若Pj1≠PE則執(zhí)行步驟3。

步驟3 判斷節(jié)點Pjk(Pjk=Pj1)與剩余節(jié)點的相連路徑是否滿足安全距離約束，記錄滿足約束條件的最靠后節(jié)點Pj2。若Pj2=PE則執(zhí)行步驟4，若Pj2≠PE則重復執(zhí)行步驟3。

步驟4 輸出節(jié)點Pi的最終優(yōu)化路徑Path_i 為：PS→…→Pi→Pj1→Pj2→…→PE。若Pi=PE則執(zhí)行步驟5，若Pi≠PE則令Pi=Pi+1并執(zhí)行步驟2。

步驟5 比較所有節(jié)點優(yōu)化路徑長度，選擇其中最短路徑作為多方向A*搜索算法優(yōu)化路徑。

在柵格圖中，假設(shè)某一路徑的路徑節(jié)點為P1，P2，…，Pm，則該路徑長度DP可計算為：其中：Pi(x)、Pi(y)表示節(jié)點的橫坐標和縱坐標，m為路徑的節(jié)點數(shù)。

其次在多方向優(yōu)化過程中，需判斷兩節(jié)點相連路徑是否安全，因此需計算路徑所經(jīng)過的所有柵格是否滿足安全距離約束。假設(shè)兩柵格P(i1，j1)和P(i2，j2)之間的坐標位置差異為Δi、Δj，且i1＜i2、j1＜j2。若Δi ＜Δj 則路徑所有柵格坐標位置計算方法如下：

同理，若Δi ＞Δj 則被路徑所有柵格坐標位置計算方法如下：

如圖4 所示，現(xiàn)基于傳統(tǒng)八方向A*算法得到路徑1 為PS→P1→P2→P3→P4→P5→P6→P7→PE，對 該路徑進行多方向搜索優(yōu)化處理，其具體優(yōu)化步驟如下。為便于描述優(yōu)化過程，圖中柵格為實際柵格放大所得，并不代表相連路徑與障礙物間的實際距離。

圖4 多方向A*搜索算法示例圖Fig.4 Example of multi-directional A*search algorithm

取出路徑所有節(jié)點，從節(jié)點PS開始依次判斷該點與節(jié)點P1，P2，P3，P4，P5，P6，P7，PE的相連路徑是否滿足安全距離約束，取出滿足約束條件的最靠后節(jié)點P4。然后依次判斷節(jié)點P4與節(jié)點P5，P6，P7，PE的相連路徑是否滿足約束條件，同樣取出滿足約束條件的最靠后節(jié)點PE，最后輸出節(jié)點PS的優(yōu)化路徑為Path_S：PS→P4→PE。

路徑剩余節(jié)點P1，P2，P3，P4，P5，P6，P7的優(yōu)化步驟與節(jié)點PS一致，依次得到各節(jié)點的優(yōu)化路徑為：

Path_1：PS→P1→P4→PE

Path_2：PS→P1→P2→P4→PE

Path_3：PS→P1→P2→P3→P6→P7→PE

Path_4：PS→P4→PE

Path_5：PS→P4→P5→P6→P7→PE

Path_6：PS→P4→P5→P6→PE

Path_7：PS→P4→P5→P6→P7→PE

最后依次比較各優(yōu)化路徑長度大小并取出其中較短路徑，得到最終優(yōu)化路徑2為PS→P4→PE，明顯相較于路徑1該路徑長度大大減小且規(guī)劃更為合理。

2.3 多方向A*搜索算法

路徑由多方向A*算法優(yōu)化后，雖刪除并整合了不必要的節(jié)點，但優(yōu)化路徑仍由許多剩余節(jié)點相連的短線段組成，導致路徑轉(zhuǎn)折角較大。針對該問題，本文采用與原始路徑偏差較小的三次樣條插值算法進一步改善路徑的平滑性。該算法的主要原理是使用由三階多項式組成的樣條來連接一組路徑節(jié)點，通過求解此多項式以擬合原路徑。若給定n個路徑節(jié)點，則存在n -1個節(jié)點區(qū)間，設(shè)每個區(qū)間[xk，xk+1]的三次樣條函數(shù)為：

為保證生成的路徑光滑連續(xù)，該多項式應滿足以下約束條件：

根據(jù)式（9），對所有路徑節(jié)點區(qū)間進行平滑處理可獲得原路徑的擬合平滑曲線。而多方向A*搜索算法新生成的路徑相鄰節(jié)點距離較遠，導致平滑曲線與原路徑擬合程度較差。因此本研究僅對路徑關(guān)鍵節(jié)點即航向改變節(jié)點進行平滑處理，假設(shè)某一關(guān)鍵節(jié)點為Pi(xi，f(xi))，則取該節(jié)點前后相鄰四個節(jié)點Pi-2、Pi-1、Pi+1、Pi+2生成4 個節(jié)點區(qū)間以獲得關(guān)鍵節(jié)點平滑曲線。

為保障路徑的安全性，消除平滑處理所造成的路徑變化影響，設(shè)計一半徑為RS的圓形區(qū)域包圍路徑關(guān)鍵節(jié)點作為平滑區(qū)域。如圖5 所示，結(jié)合安全半徑R，將關(guān)鍵節(jié)點的安全區(qū)域擴大為半徑R+RS的圓形區(qū)域。因此對關(guān)鍵節(jié)點平滑處理應滿足條件：

式中l(wèi)為關(guān)鍵節(jié)點到障礙物的最短距離。

圖5 關(guān)鍵節(jié)點平滑處理的安全區(qū)域示例圖Fig.5 Example diagram of safe area smoothed by key nodes

3 基于改進A*算法的路徑規(guī)劃

本實驗采用微軟Core i5-4200M 處理器，主頻為2.5 GHz，內(nèi)存為8 GB。在Python 3.7 環(huán)境下，為驗證上述算法的可行性，現(xiàn)提取經(jīng)度范圍為112.513 926°E ～112.700 695°E、緯度范圍為21.570 176°N ～21.735 468°N 的實際海上環(huán)境信息，并對其進行柵格化處理，設(shè)置每個柵格代表的橫向與縱向?qū)嶋H距離均為20 m，處理結(jié)果如圖2 所示。根據(jù)無人艇操縱特性與航行環(huán)境，將安全半徑R 設(shè)置為20 m、60 m 與100 m，分別對應1個、3個、5個柵格距離。

圖6 不同安全距離下的路徑規(guī)劃對比Fig.6 Comparison of path planning under different safety distances

將所設(shè)置的三種不同安全半徑作為無人艇的安全距離約束，設(shè)置路徑起始點為（100，280），目標點為（320，20），重復進行20次路徑規(guī)劃。圖6顯示了不同安全距離約束下的路徑規(guī)劃結(jié)果，三種路徑存在明顯差別，其中安全半徑越大的路徑距障礙物越遠。

各路徑上的節(jié)點到周圍障礙物的最短距離如圖7 所示，橫坐標代表路徑節(jié)點位置，縱坐標代表各路徑節(jié)點到周圍障礙物的最短距離。其中所標記的黑色節(jié)點為各路徑上距障礙物最近的節(jié)點，該點縱坐標值均等于所設(shè)置的安全半徑大小，可見路徑節(jié)點均符合安全距離約束。

圖7 不同路徑節(jié)點與障礙物的最短距離對比Fig.7 Comparison of the shortest distances between different path nodes and obstacles

因此由圖6、圖7 可知：安全距離約束下的A*啟發(fā)函數(shù)能夠準確搜索到一條從起點到終點的安全路徑。

在不同安全距離約束下，各路徑的總節(jié)點數(shù)、路徑長度與規(guī)劃時間具體對比結(jié)果如表1 所示。隨著安全半徑的增加，路徑總節(jié)點數(shù)以及路徑長度均小幅度地增加。另外安全半徑為R=60 m 與R=100 m 的規(guī)劃時間較R=20 m 分別增加了0.4 s 和1.77 s，這是由于路徑規(guī)劃過程中安全半徑設(shè)置得越大，需搜索的柵格節(jié)點增加得越多，所需的計算成本也更高。

表1 不同安全距離下的路徑規(guī)劃結(jié)果Tab.1 Path planning results under different safety distances

另外，為驗證多方向A*搜索算法以及路徑平滑算法性能，本文在上述仿真的基礎(chǔ)上設(shè)置平滑區(qū)域半徑RS=40 m，并對R=60 m 的常規(guī)A*搜索路徑重復進行20 次優(yōu)化平滑處理。常規(guī)A*搜索路徑經(jīng)多方向A*搜索算法優(yōu)化后，僅由起始點、目標點以及三個關(guān)鍵節(jié)點相連而成。多方向優(yōu)化路徑上各節(jié)點到障礙物的最短距離如圖8 所示，其中橫坐標代表節(jié)點位置，縱坐標代表節(jié)點距障礙物的最短距離。另外所標記的黑色實心節(jié)點代表優(yōu)化路徑上距離障礙物最近的節(jié)點，黑色空心節(jié)點則為路徑上的三個關(guān)鍵節(jié)點。

圖8 多方向優(yōu)化路徑節(jié)點與障礙物最短距離圖Fig.8 The shortest distance between nodes of multi-directional optimized path and obstacles

由圖8 可知：多方向優(yōu)化路徑上距障礙物的最短距離為63.2 m，符合半徑R=60 m 的安全約束。另外路徑上關(guān)鍵節(jié)點距障礙物的最短距離分別為174.1 m、72.1 m、72.6 m，而關(guān)鍵節(jié)點平滑處理安全半徑為R+RS=100 m，可見僅第一個關(guān)鍵節(jié)點滿足平滑處理約束條件。對該關(guān)鍵節(jié)點進行平滑處理后的最終優(yōu)化路徑及優(yōu)化結(jié)果如圖9以及表2所示。

圖9 不同算法優(yōu)化的路徑Fig.9 Optimized paths of different algorithms

由圖9、表2 可知：改進A*多方向搜索算法可以有效地整合并去除傳統(tǒng)路徑中多余的節(jié)點，減少路徑長度，其中路徑節(jié)點數(shù)與路徑長度均明顯小于其余三種算法。同樣改進A*算法消除了路徑轉(zhuǎn)折次數(shù)多且轉(zhuǎn)折角較大的問題，轉(zhuǎn)向次數(shù)減少至3 次，在一定程度上達到最優(yōu)。另外改進A*算法與其余三種算法時間復雜度均為O(n2)，雖然搜索節(jié)點數(shù)的增加導致路徑規(guī)劃時間小幅度地提高，但它始終提供一條距離成本更低且轉(zhuǎn)向次數(shù)更少的路徑，這更加符合無人艇路徑規(guī)劃時的實際需求。

4 結(jié)語

針對無人水面艇路徑規(guī)劃時的安全性、經(jīng)濟性及實用性要求，本研究改進A*算法取得了較好的效果。通過建立無人艇安全區(qū)域模型，使該模型作為生成最優(yōu)路徑點的安全距離約束，并在八方向A*算法上設(shè)計一種多方向搜索算法以及路徑平滑算法以獲得更符合無人艇操縱特性的全局最優(yōu)路徑。所提出的改進A*算法經(jīng)過對比分析，結(jié)果表明該方法可生成更安全、距離更短、平滑度更好的路徑，非常適用于復雜海洋環(huán)境下無人艇的實時路徑規(guī)劃。