一種結(jié)合匹配區(qū)選擇和障礙物探測(cè)的水下路徑規(guī)劃方法

李蘭玉，熊 凌,李開(kāi)寒

(武漢科技大學(xué)信息科學(xué)與工程學(xué)院，湖北 武漢，430081)

作為一種新型的水下無(wú)源導(dǎo)航技術(shù)，基于重力場(chǎng)的水下輔助導(dǎo)航方法正逐漸成為研究熱點(diǎn)[1-4]。重力儀及重力梯度儀、重力場(chǎng)基準(zhǔn)圖和匹配算法是重力場(chǎng)輔助導(dǎo)航的三要素，因此相關(guān)研究?jī)?nèi)容主要集中在重力梯度儀建模分析[5-6]、重力場(chǎng)基準(zhǔn)圖的制備[7-8]和重力場(chǎng)組合輔助導(dǎo)航匹配算法等方面。其中，關(guān)于匹配算法的研究相對(duì)較多，一般思路是將地形輔助導(dǎo)航方法應(yīng)用于重力場(chǎng)輔助導(dǎo)航中，如SITAN卡爾曼濾波算法、TERCOM匹配算法以及基于等值線迭代的ICCP 匹配算法等[9-11]。此外，也出現(xiàn)了與智能模式識(shí)別相關(guān)的匹配算法，如重力梯度多分量相融合的神經(jīng)網(wǎng)絡(luò)匹配算法[12]。

為了適應(yīng)復(fù)雜的水下環(huán)境、提高重力梯度輔助導(dǎo)航系統(tǒng)的定位精度，往往需要有合適的匹配區(qū)。研究人員利用重力梯度熵、粗糙度、相關(guān)度、均值和方差等數(shù)學(xué)統(tǒng)計(jì)特征，通過(guò)大量仿真研究得出重力輔助導(dǎo)航匹配區(qū)域選擇準(zhǔn)則，在適配區(qū)內(nèi)能達(dá)到較高的匹配概率[13-15]。

水下動(dòng)態(tài)航路規(guī)劃不僅需要考慮適配區(qū)的選擇，還需進(jìn)行實(shí)時(shí)目標(biāo)探測(cè)和有效避障，才能保證潛器的導(dǎo)航安全。而目前航路規(guī)劃研究主要側(cè)重于算法方面的研究，如距離值傳遞法、基于柵格模型的雙波傳播算法、基于信息熵遺傳算法路徑規(guī)劃等[16-19]，以此來(lái)提高優(yōu)化效率，加快規(guī)劃速度。

綜上所述，在水下潛艇重力場(chǎng)輔助導(dǎo)航領(lǐng)域，適配區(qū)選擇和路徑規(guī)劃研究往往是獨(dú)立進(jìn)行的，即在進(jìn)行適配區(qū)選擇時(shí)只是基于重力梯度特征值，而沒(méi)有考慮到實(shí)際避障問(wèn)題，或者是只考慮避障問(wèn)題而沒(méi)有顧及適配區(qū)和非適配區(qū)的具體情況。而實(shí)際上，根據(jù)重力梯度正演理論，重力梯度特征越明顯的地方，海底地形起伏越大，也就是說(shuō)，適配區(qū)存在有暗礁的可能，同時(shí)，在規(guī)劃的航路中還存在其他靜止或移動(dòng)的障礙，如他方潛器等。針對(duì)上述問(wèn)題，本文提出一種結(jié)合適配區(qū)選擇和目標(biāo)探測(cè)的航路規(guī)劃方法，即進(jìn)行水下動(dòng)態(tài)航路規(guī)劃時(shí)，不僅考慮適配區(qū)的選擇，還進(jìn)行實(shí)時(shí)目標(biāo)探測(cè)和有效避障，從而保障水下潛器的安全航行。

1 航路規(guī)劃原理

基于適配區(qū)選擇和目標(biāo)探測(cè)的航路規(guī)劃方法首先對(duì)潛艇要航行區(qū)域的重力梯度基準(zhǔn)圖進(jìn)行特征提取，這里選取的特征有重力梯度方差和標(biāo)準(zhǔn)差、重力梯度能量和重力梯度熵；然后應(yīng)用支持向量機(jī)根據(jù)這些特征進(jìn)行分類(lèi)，按參數(shù)閥值選擇出適配區(qū)和非適配區(qū)。在適配區(qū)，應(yīng)用A*算法進(jìn)行實(shí)時(shí)避障從而動(dòng)態(tài)規(guī)劃出航路；在非適配區(qū)，實(shí)時(shí)目標(biāo)探測(cè)和動(dòng)態(tài)航路規(guī)劃同時(shí)進(jìn)行，從而避碰其他靜止或運(yùn)動(dòng)的潛器。通過(guò)適配區(qū)的選擇和實(shí)時(shí)目標(biāo)探測(cè)相結(jié)合，實(shí)現(xiàn)潛艇在適配區(qū)和非適配區(qū)的動(dòng)態(tài)避障和路徑規(guī)劃。圖1為航路規(guī)劃示意圖，圖中潛艇從適配區(qū)中A點(diǎn)出發(fā)，要航行至適配區(qū)的B點(diǎn)，圖中黑色塊部分表示適配區(qū)里的障礙物，非適配區(qū)內(nèi)的航路上有其他潛器；實(shí)線代表預(yù)先規(guī)劃好的航路，虛線部分表示結(jié)合適配區(qū)選擇和目標(biāo)探測(cè)的實(shí)時(shí)動(dòng)態(tài)規(guī)劃后的航路。

圖1 潛艇航路規(guī)劃示意圖Fig.1 Schematic diagram of submarine path planning

2 基于支持向量機(jī)的適配區(qū)選擇

2.1 重力梯度特征

用于適配區(qū)選擇的重力梯度特征包括方差及標(biāo)準(zhǔn)差、重力梯度能量、重力梯度熵。

方差σ2及標(biāo)準(zhǔn)差σ是反映重力梯度數(shù)據(jù)分布離散程度的物理量，其計(jì)算公式如下：

(1)

重力梯度能量E反映重力梯度數(shù)據(jù)總量的大小，其計(jì)算公式如下 ：

(2)

重力梯度熵H也反映了重力梯度數(shù)據(jù)分布的離散程度。重力梯度熵越大，重力梯度數(shù)據(jù)分布越均勻，反之則重力梯度數(shù)據(jù)分布離散程度越大。H的計(jì)算公式如下：

(3)

(4)

2.2 基于支持向量機(jī)的分類(lèi)方法

在重力梯度特征空間中，利用基于支持向量機(jī)(SVM)的分類(lèi)方法將選定海域分為適配區(qū)和非適配區(qū)。

設(shè)樣本集(xi,yi)，i=1,2,…,n，x∈Rd，y∈{1,-1}。建立L2范數(shù)非線性軟間隔SVM優(yōu)化問(wèn)題的對(duì)偶形式：

(5)

s.t.αi≥0,i=1,2,…,n

若α*為式(5)的最優(yōu)解，根據(jù)Kühn-Tucker條件[21]求解后得到的最優(yōu)分類(lèi)函數(shù)為

(6)

3 基于重力梯度反演的目標(biāo)探測(cè)

基于重力梯度的水下目標(biāo)探測(cè)技術(shù)實(shí)際上是將目標(biāo)物體視為密度異常的質(zhì)體，通過(guò)高精度的重力梯度儀測(cè)得因動(dòng)態(tài)或靜態(tài)異常質(zhì)體引起的重力異常值Δg以及其導(dǎo)數(shù)的大小、變換規(guī)律等，進(jìn)而推測(cè)目標(biāo)的位置、形狀、質(zhì)量等參數(shù)。

如果探測(cè)目標(biāo)的距離遠(yuǎn)大于目標(biāo)尺寸，則全張量重力梯度值為

(7)

由式(7)進(jìn)行推導(dǎo)，可得到探測(cè)目標(biāo)質(zhì)心的球坐標(biāo)(θ,φ,R)為：

(8)

因?yàn)槟繕?biāo)的質(zhì)量是固定的，所以只需測(cè)量?jī)纱尾⒔Y(jié)合載體的位置變化即可將R和m都求解出來(lái)，若進(jìn)行多次測(cè)量還可以提高求解精度。

4 航路規(guī)劃方法

適配區(qū)內(nèi)有著明顯的特征，可采用匹配策略進(jìn)行導(dǎo)航，而非適配區(qū)內(nèi)沒(méi)有明顯特征，因此采用跟蹤的策略進(jìn)行導(dǎo)航?？紤]到適配區(qū)與非適配區(qū)的特點(diǎn)，航路規(guī)劃在適配區(qū)內(nèi)外分別進(jìn)行，基于重力梯度反演進(jìn)行目標(biāo)探測(cè)，應(yīng)用A*算法進(jìn)行動(dòng)態(tài)航路規(guī)劃。

A*算法結(jié)合了啟發(fā)式方法和形式化方法的特點(diǎn)[22]。它通過(guò)一個(gè)估價(jià)函數(shù)來(lái)估計(jì)圖中的當(dāng)前點(diǎn)到終點(diǎn)的距離(帶權(quán)值)，并由此決定它的搜索方向，當(dāng)這條路徑失敗時(shí)，再嘗試其它路徑。

5 仿真及分析

本文采用的仿真軟件為MATLAB?；谄ヅ鋮^(qū)選擇和目標(biāo)探測(cè)的航路規(guī)劃仿真分為3個(gè)步驟進(jìn)行。

(1)重力梯度基準(zhǔn)圖特征提取和適配區(qū)選擇。仿真基準(zhǔn)圖的尺寸為512×512，分辨率為50 m×50 m。通過(guò)對(duì)重力梯度基準(zhǔn)圖的特征提取，得到各種特征分量數(shù)據(jù)，如圖2所示。對(duì)數(shù)據(jù)進(jìn)行歸一化處理后，應(yīng)用SVM方法將基準(zhǔn)圖分為適配區(qū)和非適配區(qū)，如圖3所示，圖中曲線所圍區(qū)域即為適配區(qū)。

在適配區(qū)內(nèi)選取一塊尺寸大小為5000 m×5000 m的區(qū)域，用均方差(MSD)算法進(jìn)行匹配計(jì)算，對(duì)其適配性能進(jìn)行驗(yàn)證，仿真結(jié)果如圖4所示。仿真結(jié)果表明，在適配區(qū)內(nèi)，忽略單點(diǎn)誤差進(jìn)行連續(xù)點(diǎn)匹配時(shí)，其匹配率能達(dá)到90%以上。

(a)局部均值 (b)局部標(biāo)準(zhǔn)差

(c)局部能量 (d)局部熵

圖2重力梯度特征分量

Fig.2Featuresofthegravitygradient

圖3 適配區(qū)和非適配區(qū)Fig.3 Adaptive and non-adaptive areas

圖4 適配區(qū)內(nèi)的匹配效果Fig.4 Matching effect in adaptive area

(2)目標(biāo)探測(cè)。假設(shè)重力梯度儀的精度為10-5E，障礙物是邊長(zhǎng)為100 m的立方體，其密度為2.7 t/m3，根據(jù)重力梯度反演方法得到障礙物的位置和質(zhì)量，其與障礙物真實(shí)位置和質(zhì)量的對(duì)比如表1所示。從表1中可以看出，當(dāng)障礙物在潛器前方500 m范圍內(nèi)時(shí)，可以誤差極小地反演出其位置和質(zhì)量信息。

表1障礙物位置和質(zhì)量的反演值與真實(shí)值對(duì)比

Table1Comparisonofinversionvaluesandtruevaluesoftheobstacle’spositionandmass

障礙物在X方向的位置真實(shí)值/m反演值/m誤差/% 障礙物的質(zhì)量真實(shí)值/106 t反演值/106 t誤差/%10099.600.402.72.661.52200199.700.152.72.680.59300298.560.482.72.651.88400396.120.972.72.603.78500497.080.582.72.642.29

(3)動(dòng)態(tài)航路規(guī)劃。假設(shè)潛艇要從圖3中適配區(qū)內(nèi)的A點(diǎn)向東航行至另一個(gè)適配區(qū)內(nèi)的B點(diǎn)。航路規(guī)劃在適配區(qū)內(nèi)外分別進(jìn)行，仿真過(guò)程中利用重力梯度反演進(jìn)行目標(biāo)探測(cè)。

在匹配區(qū)內(nèi)，障礙物區(qū)域?yàn)?00 m×200 m，潛艇與障礙物安全距離為200 m，潛艇仿真行駛速度為50 m/步。航路規(guī)劃仿真結(jié)果如圖5所示。

圖5 適配區(qū)內(nèi)的航路規(guī)劃Fig.5 Path planning in adaptive area

在匹配區(qū)外，假設(shè)障礙物是靜止的，動(dòng)態(tài)規(guī)劃航路如圖6所示，潛艇航行速度為50 m/步。從圖6中可以看出，當(dāng)潛艇靠近目標(biāo)且距離達(dá)到設(shè)定的閥值時(shí)，潛艇向上繞行過(guò)障礙。

圖6 適配區(qū)外存在固定障礙物時(shí)的航路規(guī)劃

Fig.6Pathplanningwithfixedobstacleoutsideadaptivearea

在匹配區(qū)外，假設(shè)障礙物是運(yùn)動(dòng)的，且障礙物和潛艇以相同的速度同時(shí)從起點(diǎn)沿各自方向前進(jìn)，其動(dòng)態(tài)規(guī)劃航路如圖7所示。從圖7中的航跡可以看出，潛艇與障礙物的距離達(dá)到設(shè)定閥值時(shí)，潛艇向上繞行一小段，當(dāng)探測(cè)到兩者之間的距離大于閥值時(shí)，潛艇迅速回歸原航跡。

以上仿真結(jié)果表明，潛艇在水下航行時(shí)，能及時(shí)發(fā)現(xiàn)并繞開(kāi)障礙物，同時(shí)又能高效快速地航行至目標(biāo)區(qū)域?？梢?jiàn)本文提出的水下路徑規(guī)劃方法是有效的，達(dá)到了預(yù)期目的。

圖7 適配區(qū)外存在移動(dòng)障礙物時(shí)的航路規(guī)劃

Fig.7Pathplanningwithmovingobstacleoutsideadaptivearea

6 結(jié)語(yǔ)

本文針對(duì)重力梯度輔助導(dǎo)航系統(tǒng)里的航路規(guī)劃、導(dǎo)航與避障問(wèn)題，綜合考慮匹配區(qū)選擇和目標(biāo)探測(cè)，充分利用匹配區(qū)內(nèi)的有效特征，同時(shí)利用基于重力梯度反演的目標(biāo)探測(cè)進(jìn)行適配區(qū)內(nèi)和適配區(qū)外的動(dòng)態(tài)航路規(guī)劃，既保證了適配區(qū)內(nèi)的匹配導(dǎo)航策略，又保證了適配區(qū)外的跟蹤導(dǎo)航策略。通過(guò)仿真分析驗(yàn)證了文中提出的水下路徑規(guī)劃方法的有效性。

[1] Rice H, Mendelsohn L, Aarons R, et al. Next generation marine precision navigation system[C]//Position Location and Navigation Symposium, IEEE 2000. San Diego,CA,2000: 200-206.

[2] Xiong Ling, Ma Jie, Zhang Li, et al. Submarine navigation based on gravity gradient-terrain matching[C]//Proceedings of the Fifth SPIE International Symposium on Multi-spectral Image Processing and Pattern Recognition. Wuhan，China，2007,Vol.6790: 67904S.

[3] 呂良，劉敏，紀(jì)兵，等. 重力梯度儀在水下安全航行中的應(yīng)用[J].海洋測(cè)繪，2010,30(4):23-25.

[4] 王虎彪，王勇，許大欣，等. 重力異常和重力梯度聯(lián)合輔助導(dǎo)航算法及仿真[J].地球物理學(xué)進(jìn)展，2011，26(1)：116-122.

[5] 李海兵，蔡體菁. 全張量重力梯度儀測(cè)量方程及誤差分析[J].東南大學(xué)學(xué)報(bào)：自然科學(xué)版,2010,40(3):518-604.

[6] 袁贛南，張紅偉，朱嶺. 重力梯度傳感器數(shù)據(jù)的模擬及其輔助導(dǎo)航[J].傳感器與微系統(tǒng),2012,31(10):23-26.

[7] 武凜,馬杰，周瑤，等. 重力場(chǎng)匹配導(dǎo)航的全張量重力梯度基準(zhǔn)圖模擬[J].系統(tǒng)仿真學(xué)報(bào),2009,21(22):7037-7041.

[8] Ma Jie, Yang Fan, Yan Zu, et al. Density compensation to reference map of gravity navigation[C]//Proceedings of 2012 International Conference on Modelling, Identification and Control. Wuhan, China, 24-26 June, 2012:1129-1134.

[9] 王虎彪，王勇，許大欣.重力垂直梯度和重力異常輔助導(dǎo)航SITAN算法結(jié)果分析[J].機(jī)器人，2011，31(1)：128-134.

[10]熊凌，馬杰，田金文. 基于粒子濾波的重力梯度與地形信息融合輔助導(dǎo)航方法[J]. 計(jì)算機(jī)技術(shù)與軟件，2010,27(2),85-87,110.

[11]吳宏悅. 基于地形熵和ICCP算法的水下輔助導(dǎo)航組合方法研究[J].艦船科學(xué)技術(shù),2011,33(3):53-58.

[12]Xiong Ling, Xiao Linwei, Dan Binbin, et al. Full tensor gravity gradient aided navigation based on nearest matching neural network[C]∥Proceedings of Cross-Strait Quad-Regional Radio Science and Wireless Technology Conference(CSQRWC),2013.Chengdu,China,21-25 July,2013:472-475.

[13]程力，張雅杰，蔡體菁. 重力輔助導(dǎo)航匹配區(qū)域選擇準(zhǔn)則[J].中國(guó)慣性技術(shù)學(xué)報(bào)，2007，15(5):559-563.

[14]王虎彪,許大欣. 重力垂直梯度數(shù)據(jù)地圖特征及其輔助導(dǎo)航[J].中國(guó)慣性技術(shù)學(xué)報(bào),2010,18(1):93-96.

[15]吳華玲，徐效波，劉波．重力場(chǎng)可匹配區(qū)選擇研究[J]．測(cè)繪科學(xué)，2012，37(2)：14-16，68．

[16]臧濤，笪良龍，楊廷武. 基于水下聲場(chǎng)信息的三維航路規(guī)劃研究[J].指揮控制與仿真，2008，30(6)：37-40.

[17]儲(chǔ)廣風(fēng).雙波算法在水下潛器路徑規(guī)劃中的應(yīng)用[D]. 哈爾濱：哈爾濱工程大學(xué)，2011.

[18]羅翔，陳戈，陳希真，等. 基于遺傳算法的自航水雷航路規(guī)劃[J].火力與指揮控制，2009,34(3)：99-101.

[19]李奎星.基于信息熵遺傳算法的艦船導(dǎo)航路徑規(guī)劃技術(shù)研究[D]. 哈爾濱：哈爾濱工程大學(xué)，2009.

[20]張莉，周偉達(dá)，焦李成.核聚類(lèi)算法[J].計(jì)算機(jī)學(xué)報(bào)，2002，25(6)：587-590.

[21]Sch?lkopf B，Mika S，Burges C，et al. Input space versus feature space in kernel-based methods[J].IEEE Trans on Neural Networks，1999，10(5)：1000-1017.

[22]Michalewicz Z，F(xiàn)ogel D B.如何求解問(wèn)題：現(xiàn)代啟發(fā)式方法[M].曹宏慶，譯.北京：中國(guó)水利水電出版社，2003:81-84.