姜言清， 馬東， 李智慧， 李曄， 馬騰， 徐雪峰， 侯冬冬， 張文君， 武皓微

(1.哈爾濱工程大學(xué) 水下機(jī)器人技術(shù)重點(diǎn)實(shí)驗(yàn)室，黑龍江 哈爾濱 150001； 2.中國(guó)船舶航海保障技術(shù)實(shí)驗(yàn)室， 天津 300131； 3.河南省水下智能裝備重點(diǎn)實(shí)驗(yàn)室，河南 鄭州 450015; 4. 北京宇航系統(tǒng)工程研究所，北京 100076)

北極被冰層大面積覆蓋，常規(guī)的導(dǎo)航手段如衛(wèi)星導(dǎo)航、水下聲學(xué)導(dǎo)航等由于冰層的限制，無(wú)法在北極安裝使用，水下慣性導(dǎo)航由于存在隨時(shí)間增加的累計(jì)誤差，在無(wú)外界信息輔助條件下無(wú)法實(shí)現(xiàn)水下遠(yuǎn)距離導(dǎo)航[1-2]。自主水下機(jī)器人(autonomous underwater vehicles，AUVs)可以攜帶單波束聲吶實(shí)現(xiàn)水下長(zhǎng)時(shí)間、大潛深作業(yè)[3]。理論上，單波束地形匹配導(dǎo)航在北極維持長(zhǎng)時(shí)間航行的同時(shí)，還能維持一定的定位精度[4-5]。由于北極地圖分辨率及濾波算法等原因，導(dǎo)航結(jié)果出現(xiàn)不穩(wěn)定情況，許多學(xué)者對(duì)其進(jìn)行研究與改進(jìn)。Zhou[6]在傳感器受限情況下使用單波束聲吶進(jìn)行地形匹配導(dǎo)航，通過(guò)估計(jì)傳感器誤差，提高導(dǎo)航性能，并通過(guò)實(shí)驗(yàn)證明該方法在低成本AUV上的可行性；José[7]對(duì)低成本傳感器上影響粒子濾波性能的因素進(jìn)行研究，研究測(cè)量噪聲、運(yùn)動(dòng)噪聲和粒子數(shù)目等的影響，為算法優(yōu)化提供方向；Liu等[8-9]對(duì)北極冰下導(dǎo)航算法進(jìn)行研究，通過(guò)改進(jìn)粒子濾波算法提高水下定位精度，為單波束聲吶水下導(dǎo)航在北極的應(yīng)用提供了解決方案；Georgios[10]對(duì)單波束地形匹配導(dǎo)航進(jìn)行深入的研究，將單波束聲吶應(yīng)用于長(zhǎng)航程導(dǎo)航中，分別采用粒子濾波、抖動(dòng)粒子濾波和Rao-Blackwellized粒子濾波進(jìn)行數(shù)據(jù)融合[11-13]，并研究導(dǎo)航參數(shù)對(duì)導(dǎo)航性能的影響，利用北極和大西洋地形數(shù)據(jù)進(jìn)行仿真驗(yàn)證，均取得不錯(cuò)的仿真結(jié)果，為長(zhǎng)航程水下導(dǎo)航提供未來(lái)發(fā)展方向。

上述方法雖能在一定范圍內(nèi)提升單波束聲吶地形匹配導(dǎo)航的性能，但由于單波束聲吶測(cè)量原理，導(dǎo)致在實(shí)際應(yīng)用中存在較大定位誤差，為此學(xué)者將單波束地形匹配導(dǎo)航引入?yún)f(xié)同導(dǎo)航中，采用多AUV協(xié)同作業(yè)，AUV之間進(jìn)行信息交流，有效改善導(dǎo)航定位精度，提高作業(yè)效率[14]。目前基于單波束測(cè)深的多AUV協(xié)同地形匹配導(dǎo)航的研究較少，Georgios[15]研究一種水面無(wú)人艇輔助AUV的單波束地形匹配導(dǎo)航方法，取得不錯(cuò)的導(dǎo)航結(jié)果；Yew[16]提出一種3個(gè)AUV單波束聲吶地形匹配導(dǎo)航方法，采用Rao-Blackwellized粒子濾波進(jìn)行數(shù)據(jù)融合，實(shí)驗(yàn)表明該協(xié)同導(dǎo)航方法能用于水下遠(yuǎn)距離航行，具有較強(qiáng)的魯棒性。文獻(xiàn)[15-16]對(duì)單波束聲吶地形匹配導(dǎo)航誤差進(jìn)行修正，但未將北極低分辨率先驗(yàn)地形圖考慮到范圍內(nèi)，對(duì)北極低分辨率地形圖無(wú)法取得較好的導(dǎo)航效果。

本文采用一種地形自適應(yīng)雙AUV協(xié)同地形匹配導(dǎo)航方法，在低分辨率地形圖基礎(chǔ)上，考慮海底地形置信度等因素，提高地形匹配定位精度。在粒子重采樣階段將地形置信度引入重采樣方程中，避免粒子退化現(xiàn)象，提高算法魯棒性。。

1 海底地形置信度分析

本文的先驗(yàn)地形圖為圖1所示北冰洋國(guó)際水深圖[17]，利用地形高程標(biāo)準(zhǔn)差描述地形置信度，地形高程標(biāo)準(zhǔn)差為地形高程數(shù)據(jù)方差開方，能夠表述地形表面復(fù)雜度。地形特征變化明顯區(qū)域，連續(xù)2次采集的數(shù)據(jù)存在較大不同，較平坦區(qū)域具有更好的匹配效果。

地形高程方程為h=h(x,y)，對(duì)先驗(yàn)地形圖離散處理，得到地形平均高程和標(biāo)準(zhǔn)差分別為：

(1)

(2)

式中：A為地形區(qū)域面積；i和j為地形區(qū)域中X軸和Y軸坐標(biāo)位置。

圖1 北冰洋國(guó)際水深Fig.1 International bathymetric chart of the arctic ocean

海底地形高程標(biāo)準(zhǔn)差與海底地形起伏度有關(guān)，地形起伏越快，地形高程標(biāo)準(zhǔn)差越大[18]。利用地形高程標(biāo)準(zhǔn)差對(duì)地形平坦度分類，當(dāng)?shù)匦螛?biāo)準(zhǔn)差位于[0,18)時(shí)，區(qū)域?yàn)槠教沟匦?；?dāng)?shù)匦螛?biāo)準(zhǔn)差位于區(qū)間[18,61]時(shí)，區(qū)域?yàn)橹械葏^(qū)域；當(dāng)?shù)匦螛?biāo)準(zhǔn)差位于區(qū)間(61,+∞]時(shí)，區(qū)域?yàn)榇植趨^(qū)域。

利用海底地形平坦度劃分標(biāo)準(zhǔn)，建立海底地形置信度模型。海底地圖柵格化處理，每個(gè)柵格化區(qū)域大小為20×20，網(wǎng)格分辨率為200 m，計(jì)算柵格區(qū)域內(nèi)的地形置信度，結(jié)果如圖2所示。

圖2 地形置信度Fig.2 Terrain confidence map

2 雙自主水下機(jī)器人地形匹配導(dǎo)航方法

單波束聲吶每次測(cè)量獲得一處水深數(shù)據(jù)，遇到地形平坦區(qū)域?qū)?dǎo)致地形匹配導(dǎo)航失效。本文提出一種雙AUV協(xié)同地形匹配導(dǎo)航方法，AUV間利用水聲通信，保證時(shí)間一致，水聲通信周期內(nèi)采用地形匹配導(dǎo)航修正自身位置，每隔通信周期T進(jìn)行一次水聲通信，獲得AUV間距離、位置和地形置信度信息[19]，如圖3所示。采用擴(kuò)展卡爾曼濾波進(jìn)行數(shù)據(jù)融合[20]，利用AUV中位置置信度高對(duì)位置置信度低進(jìn)行修正，提高導(dǎo)航方法的魯棒性。

圖3 雙AUV協(xié)同地形匹配導(dǎo)航示意Fig.3 Schematic diagram of dual AUVs cooperative terrain matching navigation

在AUV協(xié)同地形匹配導(dǎo)航中，未考慮粒子重采樣帶來(lái)的粒子貧化和地形平坦區(qū)域誤匹配等問(wèn)題。粒子多次迭代后，權(quán)重可能集中在小部分粒子上，導(dǎo)致粒子退化，造成地形匹配導(dǎo)航定位出現(xiàn)偏差[21]。粒子多次重采樣后，會(huì)出現(xiàn)多數(shù)粒子來(lái)自同一粒子，造成粒子耗盡問(wèn)題。為此對(duì)權(quán)值更新和重采樣步驟進(jìn)行改進(jìn)，將地形置信度融入到重采樣步驟中，抑制粒子在地形平坦區(qū)域的重采樣，減少粒子貧化和誤匹配問(wèn)題，提升地形匹配導(dǎo)航定位的魯棒性。

1)地形匹配導(dǎo)航。

(3)

式中ξ為權(quán)重調(diào)節(jié)系數(shù)。

權(quán)重歸一化為：

(4)

AUV位置狀態(tài)估計(jì)和方差估計(jì)分別為：

(5)

(6)

計(jì)算有效粒子數(shù)目為：

(7)

將有效粒子數(shù)目與粒子閾值比較。若小于粒子閾值，進(jìn)行重采樣，反之不操作。

對(duì)Xk+1位置進(jìn)行估計(jì):

Xk+1=Xk+uk+1+δk+1

(8)

2)協(xié)同地形匹配導(dǎo)航為：

xk+1=xk+Tvx(k)

(9)

yk+1=yk+Tvy(k)

(10)

式中：vx(k)和vy(k)為AUV的速度u在x軸和y軸上的投影，通信周期為T，每進(jìn)行一次采樣便進(jìn)行一次信息交換。

該模型未考慮噪聲影響，設(shè)定傳感器噪聲為高斯噪聲，得到速度為：

umk=uk+δk

(11)

噪聲協(xié)方差矩陣為：

(12)

AUV平面狀態(tài)方程為：

Xk+1=f(Xk,uk,δk)=Xk+Γ(uk,δk)=

(13)

式中Γ(uk,δk)為非線性項(xiàng)。

在擴(kuò)展卡爾曼濾波中，根據(jù)2條路徑上AUV所在位置的地形置信度對(duì)擴(kuò)展卡爾曼濾波參數(shù)進(jìn)行調(diào)整，利用位置置信度高的對(duì)位置置信度低的進(jìn)行修正，提高導(dǎo)航方法的魯棒性。

通過(guò)擴(kuò)展卡爾曼濾波量測(cè)方程得到系統(tǒng)的量測(cè)方程為：

(14)

式中wz為量測(cè)噪聲。

根據(jù)協(xié)同導(dǎo)航非線性量測(cè)方程，得到擴(kuò)展卡爾曼濾波量測(cè)雅可比矩陣為：

(15)

采用擴(kuò)展卡爾曼濾波進(jìn)行更新，更新方程為：

Xk+1,k=f(Xk,uk,δk)=Xk+Γ(uk,δk)

(16)

更新方程的協(xié)方差矩陣為：

(17)

在協(xié)同導(dǎo)航中考慮通信中斷的影響，當(dāng)通信中斷時(shí)，AUV利用地形匹配導(dǎo)航進(jìn)行位置修正，當(dāng)通信連接時(shí)利用擴(kuò)展卡爾曼濾波進(jìn)行信息融合，對(duì)通訊中斷時(shí)的偏差進(jìn)行修正，提高定位精度。算法流程如圖4所示。

圖4 雙AUV協(xié)同地形匹配導(dǎo)航流程Fig.4 Flowchart of dual AUVs cooperative terrain matching navigation

3 雙自主水下機(jī)器人協(xié)同地形匹配導(dǎo)航模擬仿真

3.1 仿真參數(shù)設(shè)置

將地形自適應(yīng)雙AUV協(xié)同導(dǎo)航與單AUV地形匹配導(dǎo)航和基于擴(kuò)展卡爾曼濾波的雙AUV協(xié)同導(dǎo)航進(jìn)行比較。仿真中考慮通信中斷，存在一定水聲信息丟失的情況，全程隨機(jī)設(shè)置200個(gè)斷點(diǎn)。在雙AUV協(xié)同地形匹配導(dǎo)航中，先驗(yàn)地圖分辨率為200 m，水聲通信延遲時(shí)間約3 s，AUV 速度較低，航行距離遠(yuǎn)小于一個(gè)網(wǎng)格區(qū)域，不會(huì)對(duì)導(dǎo)航精度產(chǎn)生較大影響，通信延遲可忽略。當(dāng)?shù)匦螛?biāo)準(zhǔn)差小于18時(shí)，認(rèn)為此時(shí)地形平坦。仿真參數(shù)設(shè)置如表1所示。

表1 AUV協(xié)同地形匹配導(dǎo)航參數(shù)設(shè)置

3.2 仿真結(jié)果

根據(jù)上述仿真參數(shù)進(jìn)行模擬仿真，得到的仿真結(jié)果對(duì)比如表2所示。仿真結(jié)果對(duì)比圖如圖5所示。

表2 AUV仿真定位結(jié)果Table 2 AUV simulation positioning results

圖5 匹配誤差與匹配成功次數(shù)對(duì)比Fig.5 Comparison of matching errors and number of successful matches

4 結(jié)論

1)北極海面被冰層覆蓋，水下長(zhǎng)時(shí)間遠(yuǎn)距離精準(zhǔn)導(dǎo)航成為水下探索的難點(diǎn)。本文提出的基于單波束聲吶的雙AUV協(xié)同地形匹配導(dǎo)航方法，能夠在水下無(wú)源條件下實(shí)現(xiàn)長(zhǎng)時(shí)間精準(zhǔn)定位，提高AUV水下作業(yè)能力。

2)對(duì)北極稀疏先驗(yàn)地形圖，將海底地形置信度融入?yún)f(xié)同地形匹配導(dǎo)航算法中，減少粒子退化現(xiàn)象，同時(shí)利用AUV中位置置信度高的對(duì)位置置信度低的進(jìn)行修正，提高地形匹配成功次數(shù)，提升導(dǎo)航算法的魯棒性。

3)對(duì)北極冰下遠(yuǎn)距離導(dǎo)航進(jìn)行仿真驗(yàn)證，通過(guò)仿真結(jié)果證明方法的可行性與魯棒性，為北極冰下資源探索與定位提供新的發(fā)展方向。