基于改進(jìn)A*算法的UUV冰下避障航跡規(guī)劃算法

程建華，李鵬程，管 行，葛靖宇

(哈爾濱工程大學(xué)智能科學(xué)與工程學(xué)院，哈爾濱 150001)

0 引言

北極擁有極為豐富的自然資源，還是亞、歐、北美三大洲的延伸匯集地，有聯(lián)系三大洲的最短大弧航線，具有極其重要的戰(zhàn)略意義。由于北極地區(qū)被較厚冰蓋覆蓋，冰下航行是重要的戰(zhàn)略手段，因此開展水下無(wú)人潛航器(Unmanned Underwater Vehicle，UUV)北極冰下航跡自主規(guī)劃技術(shù)研究，對(duì)于提高我國(guó)載人/無(wú)人潛航器極區(qū)冰下安全航行能力，具有重要的實(shí)際意義和應(yīng)用價(jià)值[1]。

UUV在實(shí)際的航行過程中，其運(yùn)動(dòng)過程較為復(fù)雜，所以規(guī)劃出一條符合UUV運(yùn)動(dòng)狀態(tài)的航跡難度較高。國(guó)內(nèi)外雖有多項(xiàng)研究成果，如蟻群優(yōu)化算法[2]、遺傳算法[3]和人工勢(shì)場(chǎng)算法[4]等，但這些航跡規(guī)劃算法各有其局限性[5]。在復(fù)雜環(huán)境下，UUV的航跡規(guī)劃是一個(gè)復(fù)雜的多約束問題[6]，且冰下航跡規(guī)劃領(lǐng)域研究幾乎為空白。航跡規(guī)劃需要綜合考慮地形碰撞約束、海冰碰撞約束、潛航器航行性能以及任務(wù)需求等各種因素，規(guī)劃出一條符合上述所有約束條件的航跡。忽略約束條件或單純強(qiáng)調(diào)目標(biāo)搜索的規(guī)劃方法很難適應(yīng)UUV的真實(shí)運(yùn)動(dòng)情況[7]。

本文結(jié)合傳統(tǒng)人工勢(shì)場(chǎng)航跡規(guī)劃算法的思想，將海底地形碰撞約束、海冰碰撞約束和UUV巡航高度約束重新編排為人工勢(shì)場(chǎng)斥力約束，并給出了這三種約束的人工勢(shì)場(chǎng)設(shè)計(jì)方法。將這些約束因素與A*搜索算法的代價(jià)函數(shù)組合，得到改進(jìn)的A*冰下航跡規(guī)劃算法。最后，通過仿真驗(yàn)證了該算法可生成滿足任務(wù)需求的冰下航跡。

1 UUV航跡規(guī)劃約束條件

1.1 動(dòng)力學(xué)約束

UUV在水下正常航行過程中，可將其視為一個(gè)滿足動(dòng)力學(xué)約束條件的六自由度運(yùn)動(dòng)剛體結(jié)構(gòu)。由于橫滾角與航跡無(wú)直接聯(lián)系，因此本文主要考慮UUV的縱傾與艏搖約束條件[8]。設(shè)其縱傾角速度為q，縱傾角為θ，最大縱傾安全角速度為qmax，最大縱傾安全角為θmax，所以UUV的縱傾約束為[6]

(1)

同理，可以得知UUV的艏搖約束為

(2)

其中，艏搖角速度為r；艏搖角為ψ；最大艏搖安全角速度為rmax；最大艏搖安全角為ψmax。若UUV運(yùn)動(dòng)角速度或者角度超出安全范圍，則會(huì)出現(xiàn)失控狀態(tài)。

1.2 防碰撞約束

在進(jìn)行UUV冰下航跡規(guī)劃時(shí)，需要使航跡與海底地形以及海冰保持安全距離，防止發(fā)生碰撞。設(shè)UUV距地形與海冰的垂直安全距離分別為εobs和εice，距地形與海冰的實(shí)際垂直距離分別為xobs和xice，在整個(gè)航跡規(guī)劃過程中，需滿足如下約束

(3)

1.3 定深巡航高度約束

UUV在執(zhí)行勘測(cè)或巡邏任務(wù)時(shí)，為了獲得更好的隱蔽效果，減小被聲納發(fā)現(xiàn)的概率，需要使UUV在某一確定高度從起點(diǎn)保持定深巡航至終點(diǎn)。定深巡航高度約束是整個(gè)航跡規(guī)劃過程中必須考慮的問題。設(shè)UUV執(zhí)行任務(wù)的目標(biāo)深度為htask，其實(shí)際巡航深度為h，任務(wù)可接受的巡航深度最大偏差為he。定深巡航約束應(yīng)滿足如下公式

|h-htask|≤he

(4)

除此之外，UUV還會(huì)受材料、工藝技術(shù)以及密封性的限制，存在一個(gè)最大工作深度H，所以其巡航深度應(yīng)同時(shí)滿足

h≤H

(5)

1.4 能量約束

因?yàn)閁UV是攜帶燃料或者蓄能電池的水下無(wú)人潛航器，所以其航行距離s不能超過UUV的最大航程S，其約束為

s≤S

(6)

2 基于改進(jìn)的A*航跡規(guī)劃算法

2.1 傳統(tǒng)A*航跡搜索算法

A*航跡搜索算法是一種常見的航跡搜索算法，適用于較大范圍地形的航跡搜索。該算法為基于迪杰斯特拉(Dijkstra)航跡搜索算法，同時(shí)結(jié)合廣度優(yōu)先搜索(Breadth-First-Search，BFS)算法的啟發(fā)式航跡搜索算法[9]。所以，A*航跡搜索算法同時(shí)具備Dijkstra搜索算法與BFS算法的特點(diǎn)，既可以按照最短路徑進(jìn)行搜索，同時(shí)又能在搜索過程中添加啟發(fā)函數(shù)和代價(jià)函數(shù)，使得航跡按照任務(wù)進(jìn)行規(guī)劃。A*航跡規(guī)劃算法的代價(jià)函數(shù)如下所示

f(n)=g(n)+h(n)

(7)

其中，n代表當(dāng)前待搜索節(jié)點(diǎn)；g(n)代表從航跡初始點(diǎn)到當(dāng)前待搜索節(jié)點(diǎn)的實(shí)際代價(jià)；h(n)代表對(duì)從當(dāng)前待搜索節(jié)點(diǎn)到航跡規(guī)劃目標(biāo)點(diǎn)的實(shí)際代價(jià)的估計(jì)值，又被稱為啟發(fā)函數(shù)[6]。這樣，f(n)就可以代表從航跡規(guī)劃初始點(diǎn)經(jīng)過點(diǎn)n到達(dá)目標(biāo)點(diǎn)的最小代價(jià)估計(jì)值[10-11]，上述代價(jià)一般指路徑長(zhǎng)度，可使用歐氏距離或者曼哈頓距離進(jìn)行計(jì)算。

利用A*搜索算法進(jìn)行三維航跡規(guī)劃之前，需要將空間三維網(wǎng)格化，利用網(wǎng)格點(diǎn)完成航跡規(guī)劃。在相鄰3×3×3共27個(gè)三維網(wǎng)格點(diǎn)中，除去當(dāng)前節(jié)點(diǎn)n，可對(duì)剩余26個(gè)網(wǎng)格點(diǎn)進(jìn)行搜索。在使用A*搜索算法進(jìn)行航跡規(guī)劃時(shí)，需要建立2個(gè)數(shù)據(jù)表：Open表和Close表,Open表用于儲(chǔ)存還未進(jìn)行搜索的節(jié)點(diǎn)，Close表用于儲(chǔ)存已經(jīng)完成搜索的節(jié)點(diǎn)[12-15]。

2.2 改進(jìn)A*搜索算法代價(jià)函數(shù)設(shè)計(jì)

冰下航跡規(guī)劃需要從地形與海冰避障約束、航程約束、定深巡航約束以及能量約束4個(gè)方面展開。傳統(tǒng)的代價(jià)函數(shù)很難適應(yīng)該任務(wù)需求，所以本文將A*航跡規(guī)劃算法的代價(jià)函數(shù)進(jìn)行了如下修改

f(n)=αg(n)+βh1(n)+μh2(n)+γh3(n)

(8)

其中，g(n)代表從航跡初始點(diǎn)到當(dāng)前待搜索節(jié)點(diǎn)的距離約束代價(jià);h1(n)代表對(duì)從當(dāng)前待搜索節(jié)點(diǎn)到航跡規(guī)劃目標(biāo)點(diǎn)的距離約束代價(jià)的估計(jì)值；h2(n)代表對(duì)從當(dāng)前待搜索節(jié)點(diǎn)到航跡規(guī)劃目標(biāo)點(diǎn)的地形約束代價(jià)的估計(jì)值；h3(n)代表對(duì)從當(dāng)前待搜索節(jié)點(diǎn)到航跡規(guī)劃目標(biāo)點(diǎn)的定深巡航約束代價(jià)的估計(jì)值。α、β、μ、γ為各項(xiàng)的權(quán)值系數(shù)，且滿足α，β，μ，γ≥0。

1)航程約束代價(jià)設(shè)計(jì)

航程約束代價(jià)主要分為兩部分：從航跡初始點(diǎn)到當(dāng)前待搜索節(jié)點(diǎn)的歐氏距離g(n)與當(dāng)前待搜索節(jié)點(diǎn)到終點(diǎn)的曼哈頓距離h1(n)。

2)地形與海冰避障約束代價(jià)設(shè)計(jì)

在實(shí)際航行過程中，當(dāng)UUV距離障礙物較近時(shí)，危險(xiǎn)程度會(huì)隨著與地形(海冰)距離的縮短而逐漸增大；而當(dāng)UUV距離地形(海冰)較遠(yuǎn)時(shí)，UUV則處于安全狀態(tài)(圖1)。可依據(jù)UUV航行時(shí)距地形與海冰的垂直安全距離εobs、εice和實(shí)際垂直距離xobs、xice對(duì)地形與海冰避障約束代價(jià)進(jìn)行定義

h2(n)=hobs(n)+hice(n)

(9)

其中，hobs(n)為地形避障約束代價(jià)；hice(n)為海冰避障約束代價(jià)，它們的表達(dá)式分別為

hobs(n)=

(10)

(11)

式中，kobs與kice分別為地形避障約束代價(jià)與海冰避障約束代價(jià)的調(diào)整參數(shù)。

圖1 UUV水下航行剖面圖Fig.1 UUV underwater navigation profile

3)定深巡航約束代價(jià)設(shè)計(jì)

當(dāng)UUV在執(zhí)行任務(wù)時(shí)，會(huì)有定深巡航高度的要求，在任務(wù)允許的范圍內(nèi)，調(diào)整UUV航行深度不會(huì)增加定深巡航代價(jià);當(dāng)UUV航行深度超出允許范圍時(shí)，定深巡航高度約束代價(jià)會(huì)快速增加，可定義定深巡航高度約束代價(jià)為

h3(n)=

(12)

式中，kh為定深巡航約束代價(jià)的調(diào)整參數(shù)。

4)能量約束代價(jià)設(shè)計(jì)

當(dāng)UUV在能量耗盡之前完成任務(wù)可不受到約束的影響;當(dāng)UUV未能在能量耗盡之前完成任務(wù)，則判定航跡規(guī)劃失敗。

經(jīng)過前期仿真驗(yàn)證可得：當(dāng)α=β=1，γ=1，μ≤0.035時(shí)，規(guī)劃出的航跡會(huì)不受距離約束代價(jià)的約束；當(dāng)α=β=1，μ=1，γ≥62時(shí)，規(guī)劃出的航跡會(huì)不受距離約束代價(jià)與地形約束代價(jià)的約束。綜上所述，可得本文的權(quán)值系數(shù)取值范圍為：當(dāng)α=β=1時(shí)，需滿足如下不等式

(13)

3 仿真驗(yàn)證

以UUV穿越某一覆蓋有海冰的復(fù)雜地形這一情形為例，進(jìn)行多次仿真驗(yàn)證。地形范圍為100km×100km，格網(wǎng)精度為(1km，1km，1m)，仿真步長(zhǎng)為r=1。

任務(wù)1：UUV 要安全穿越某一被海冰完全覆蓋的復(fù)雜地形，任務(wù)要求UUV在深度-60m保持定深航行，且要完成避障。航跡規(guī)劃的起點(diǎn)為(5km，5km，-60m)，航跡規(guī)劃的終點(diǎn)為(95km，95km，-60m)。設(shè)定定深巡航高度htask=-60m，可接受巡航深度最大偏差he=0，地形安全距離εobs=5m，海冰安全距離εice=10m，仿真中各項(xiàng)權(quán)值系數(shù)α=1，β=1，μ=3，γ=2。UUV既需要保證航行安全，又需要完成定深航行任務(wù)，所以地形(海冰)避障約束權(quán)值系數(shù)與定深巡航高度約束權(quán)值系數(shù)需取較大值，并且前者大于后者。仿真結(jié)果如圖2～圖5所示。

從圖2與圖3的航跡三維圖可以看出，UUV具有較好的地形回避能力，且可以順利完成從起點(diǎn)至終點(diǎn)的航跡規(guī)劃，航跡總長(zhǎng)度為142.65km。從圖4與圖5的航跡高度示意圖與安全距離示意圖可以看出，UUV在整個(gè)航跡規(guī)劃過程中，航跡大部分時(shí)間處于設(shè)定的航行高度htask=-60m且基本保持高度穩(wěn)定，距離地形的實(shí)際距離始終大于或者等于設(shè)定的地形安全距離，符合任務(wù)需求。

圖2 任務(wù)1地形高度及三維航跡Fig.2 Terrain height and 3D track of task 1

圖3 任務(wù)1冰層下表面高度及三維航跡Fig.3 The height of the lower surface of the ice layer and its 3D track of task 1

圖4 任務(wù)1航跡高度及地形與冰層下表面高度Fig.4 Track height, terrain and ice surface height of task 1

圖5 任務(wù)1航跡上安全距離與實(shí)際距離Fig.5 Safe distance and actual distance on track of task 1

任務(wù)2：UUV 要安全穿越某一被海冰覆蓋的復(fù)雜地形，任務(wù)要求UUV在整個(gè)航行過程中需要完成避障，并且盡可能縮短航程。航跡規(guī)劃的起點(diǎn)為(5km，5km，-40m)，航跡規(guī)劃的終點(diǎn)為(95km，95km，-40m)。設(shè)定定深巡航高度htask=-40m，可接受巡航深度最大偏差he=10m，地形安全距離εobs=15m，海冰安全距離εice=10m，各項(xiàng)的權(quán)值系數(shù)α=2，β=2，μ=3，γ=1。UUV既需要保證航行安全且完成避障，又需要縮短航程，所以初始點(diǎn)到當(dāng)前待搜索節(jié)點(diǎn)的距離約束權(quán)值系數(shù)、當(dāng)前待搜索節(jié)點(diǎn)到航跡規(guī)劃目標(biāo)點(diǎn)的距離約束權(quán)值系數(shù)與地形(海冰)避障約束權(quán)值系數(shù)需要取較大值，并且后者大于前兩者。仿真結(jié)果如圖6～圖9所示。

從圖6與圖7的航跡三維圖可以看出，UUV具有良好的地形回避能力，可以順利完成從起點(diǎn)至終點(diǎn)的航跡規(guī)劃，且規(guī)劃出的航跡較為平直，航跡總長(zhǎng)度為135.72km。從圖8與圖9的航跡高度示意圖與安全距離示意圖可以看出，UUV在整個(gè)航跡規(guī)劃過程中，由于定深巡航約束代價(jià)取值較小，所以初始設(shè)定的航行高度htask=-40m對(duì)航跡規(guī)劃約束不強(qiáng)。規(guī)劃的航跡距離地形的實(shí)際距離始終大于或者等于設(shè)定的地形安全距離，相較于任務(wù)1，航程縮短，符合任務(wù)需求。

圖6 任務(wù)2地形高度及三維航跡Fig.6 Terrain height and 3D track of task 2

圖7 任務(wù)2冰層下表面高度及三維航跡Fig.7 The height of the lower surface of the ice layer and its 3D track of task 2

圖8 任務(wù)2航跡高度及地形與冰層下表面高度Fig.8 Track height, terrain and ice surface height of task 2

圖9 任務(wù)2航跡上安全距離與實(shí)際距離Fig.9 Safe distance and actual distance on track of task 2

4 結(jié)論

本文針對(duì)冰下避障航跡規(guī)劃問題，結(jié)合傳統(tǒng)人工勢(shì)場(chǎng)航跡規(guī)劃算法的思想，設(shè)計(jì)了一種基于改進(jìn)A*算法的UUV冰下避障航跡規(guī)劃算法，充分考慮了海冰避障約束、地形避障約束、定深巡航高度約束以及能量約束,設(shè)計(jì)了針對(duì)上述約束的代價(jià)函數(shù)，將代價(jià)函數(shù)與A*航跡搜索算法結(jié)合獲得改進(jìn)A*航跡搜索算法，并完成了三維航跡仿真。

通過仿真驗(yàn)證了該算法規(guī)劃的路徑能夠有效完成地形與海冰避障、航行深度控制以及航行距離控制。通過調(diào)整代價(jià)函數(shù)的權(quán)值系數(shù)，仿真所得航跡能夠契合任務(wù)要求。

本文仿真只是在已知的環(huán)境下進(jìn)行的，而UUV實(shí)際航行時(shí)的環(huán)境是動(dòng)態(tài)變化的，針對(duì)動(dòng)態(tài)變化環(huán)境的航跡規(guī)劃是下一步工作的重點(diǎn)。