基于單信標(biāo)的無人水下航行器導(dǎo)航算法及改進

陳祥國,尚 凡,藍 強

基于單信標(biāo)的無人水下航行器導(dǎo)航算法及改進

陳祥國,尚 凡,藍 強

(國防科技大學(xué) 氣象海洋學(xué)院,湖南 長沙,410073)

基于單信標(biāo)的無人水下航行器(UUV)導(dǎo)航算法具有所需潛標(biāo)數(shù)量少、系統(tǒng)海上布放標(biāo)校簡單、維護成本較低等優(yōu)點,結(jié)合UUV慣性導(dǎo)航系統(tǒng)后能夠滿足未來廣域、低成本水下導(dǎo)航需求。針對單信標(biāo)導(dǎo)航算法收斂速度較慢、非全局可觀測等問題,提出增加目標(biāo)徑向速度觀測量的單信標(biāo)水下導(dǎo)航算法; 分析了引入目標(biāo)徑向速度觀測后的系統(tǒng)全局觀測性,給出新的導(dǎo)航初值估計及跟蹤方法; 使用擴展卡爾曼濾波算法,建立4狀態(tài)變量-3觀測量(4SV-3OB)單信標(biāo)導(dǎo)航模型,并通過數(shù)值仿真試驗,得出4SV-3OB模型相較于傳統(tǒng)單信標(biāo)導(dǎo)航模型收斂時間減小約75%。仿真結(jié)果表明,增加目標(biāo)徑向速度作為系統(tǒng)觀測量可有效簡化單信標(biāo)導(dǎo)航算法初值估計及跟蹤流程,并能夠顯著減小導(dǎo)航算法收斂時間。

無人水下航行器; 單信標(biāo); 水下導(dǎo)航; 全局觀測性; 擴展卡爾曼濾波

0 引言

隨著水下無人系統(tǒng)領(lǐng)域的發(fā)展,如何使用中大型無人水下航行器(unmanned undersea vehicle,UUV)運送戰(zhàn)斗部件至上千公里外戰(zhàn)略要塞海域已成為相關(guān)學(xué)術(shù)研究領(lǐng)域熱點問題。而設(shè)計實現(xiàn)高可靠性、高精度的水下導(dǎo)航系統(tǒng)對于遠航程UUV航行安全性、任務(wù)使命的達成,有著至關(guān)重要的作用。眾所周知,由于趨膚效應(yīng),電磁波在海水中衰減迅速,空氣中常用的無線電或衛(wèi)星導(dǎo)航方法(如GPS、北斗等)均無法直接應(yīng)用于UUV系統(tǒng),因此目前常見的水下導(dǎo)航方法主要可分為以下3類[1]。

1) 慣性導(dǎo)航方法。該方法使用牛頓力學(xué)定律,根據(jù)傳感器輸出加速度、旋轉(zhuǎn)角速率,實時推算得到載體運動位置。由于其運行不需要依賴載體外部信息,因此被廣泛應(yīng)用于軍用潛艇水下導(dǎo)航領(lǐng)域。但受限于傳感器精度性能及積分運算影響,慣性導(dǎo)航系統(tǒng)定位誤差隨時間推移而不斷增大,即便使用高精度光纖陀螺,并增設(shè)多普勒測速聲吶實施速度修正,其漂移約為1 n mile/96 h[2]。同時,高精度慣導(dǎo)傳感器價格昂貴、體積較大,因此,單獨使用慣性導(dǎo)航系統(tǒng)并不能完全滿足長航程UUV系統(tǒng)的使用需求。

2) 并發(fā)定位及建圖(simultaneous localization and mapping,SLAM)技術(shù)。該類水下導(dǎo)航技術(shù)需在航行器底部安裝光學(xué)或聲學(xué)海底地貌成像設(shè)備,如照明與圖像采集攝像裝置或成像聲吶,在航行過程中實時獲取海底地貌信息,并根據(jù)特征點[3]確定航行器的運動軌跡。SLAM導(dǎo)航技術(shù)對使用海域海底地貌有較高要求,當(dāng)海域內(nèi)海底地貌平緩或特征點不十分顯著時,SLAM導(dǎo)航可期效果較差[1]。

3) 基于信號到達時間(time of arrival,TOA)或信號到達方位(direction of arrival,DOA)的外測水聲導(dǎo)航方法,如長基線、超短基線、反向超短基線水聲導(dǎo)航方法等。以長基線導(dǎo)航系統(tǒng)為例,該方法一般需要在預(yù)定海域海底,以一定間距(3～8 km)和陣型布放多套合作水聲信號信標(biāo)或應(yīng)答器,同時需在航行器上安裝詢問信號發(fā)射換能器、接收水聽器及相應(yīng)導(dǎo)航算法計算設(shè)備。航行器在水下航行時,可實時測量與多個位置先驗已知的應(yīng)答器距離或方位,最終解算自身位置。外測方法不存在時間積累誤差,典型的長基線定位系統(tǒng)陣內(nèi)導(dǎo)航誤差約為4～5 m[4]。

長基線水聲導(dǎo)航方案原理簡單、魯棒性強,但至少需要使用3個以上水下信標(biāo)或應(yīng)答器(下文中統(tǒng)稱信標(biāo))才可完成導(dǎo)航定位功能。考慮到信標(biāo)硬件成本及海上布放、標(biāo)校費用,傳統(tǒng)長基線導(dǎo)航系統(tǒng)運營和維護成本較高,不能滿足未來廣域、低成本的UUV導(dǎo)航需求。針對該問題,大量圍繞單信標(biāo)的水聲導(dǎo)航方案相繼展開。單信標(biāo)導(dǎo)航算法是指UUV通過實時測量與單個位置先驗已知的水下信標(biāo)斜距,結(jié)合自身航向和航速信息,使用狀態(tài)空間估計確定自身位置[5-8]。單信標(biāo)導(dǎo)航算法具有所需潛標(biāo)數(shù)量少、無需測量陣型校準(zhǔn)、無時間積累誤差等優(yōu)點,原理上可以替代傳統(tǒng)多信標(biāo)水下彈道測量系統(tǒng)。該方法可與慣性導(dǎo)航系統(tǒng)結(jié)合使用,在部分關(guān)鍵航路點輔助完成慣導(dǎo)修正,以消除慣導(dǎo)時間積累誤差,滿足遠航程UUV對高精度水下導(dǎo)航的需求。

早期單信標(biāo)導(dǎo)航算法由Vaganay等[5]首先提出,主要用于完成自主水下航行器返航等問題。Vaganay等給出了單信標(biāo)返航算法初值估計方法及基于擴展卡爾曼濾波器(extended Kalman filter,EKF)的目標(biāo)位置估計算法。Gadre等[6]在綜合考慮低成本UUV可攜帶的航向、多普勒航速等多傳感器的條件下,將海流作為未知狀態(tài)量,理論分析了單信標(biāo)導(dǎo)航算法的局部可觀測性問題,完成了部分算法驗證工作。Zhu等[9-10]將有效聲速估計引入單信標(biāo)導(dǎo)航算法,并考慮多傳感器非統(tǒng)一更新速率,建立了完整的數(shù)值計算仿真模型,在多種條件下改善了導(dǎo)航精度,提高了算法的可用性。

寬帶擴頻水聲信號在水下定位及通信領(lǐng)域的成熟應(yīng)用,使得高精度UUV徑向運動估計成為可能[11]。文中在航行器徑向速度可估計的條件下,使用幾何學(xué)方法分析單信標(biāo)導(dǎo)航方案全局觀測性,并給出了新的導(dǎo)航初值估計方法; 建立了4狀態(tài)變量-3觀測量(4-state variable 3-observation,4SV- 3OB)導(dǎo)航系統(tǒng)模型,有效提高了傳統(tǒng)算法較大初值誤差條件下的收斂速度。

1 單信標(biāo)導(dǎo)航全局觀測性與初值估計

單信標(biāo)導(dǎo)航算法存在非全局可觀測性問題,即對于同一組觀測值,存在多個滿足觀測條件的導(dǎo)航定位結(jié)果[12]。文中在航行器徑向速度可估計的條件下,建立單信標(biāo)導(dǎo)航算法幾何學(xué)模型,分析其非全局可觀測性產(chǎn)生機理,并給出使用幾何學(xué)方法求解目標(biāo)導(dǎo)航初值的方法。由于信標(biāo)深度信息先驗已知,同時航行器航深信息可由水壓傳感器精確給出,因此,文中只進行目標(biāo)水平方向位置估計,建立二維導(dǎo)航模型。如圖1所示,以位置先驗已知的導(dǎo)航信標(biāo)為原點,建立二維直角坐標(biāo)系,其、軸分別指向正東、正北方向。

圖1 增加徑向速度觀測的單信標(biāo)導(dǎo)航模型

圖2為使用式(7)給出的目標(biāo)航跡估計,紅色折線為UUV真實航跡,藍色和綠色信標(biāo)分別為2組模糊估計。由圖可知,單信標(biāo)導(dǎo)航系統(tǒng)具有非全局可觀測性,但2條不可區(qū)分的估計軌跡拼接成的狀態(tài)空間含有真實軌跡。

圖2 單信標(biāo)導(dǎo)航模型非全局可觀測性

2 單信標(biāo)導(dǎo)航模型建立

單信標(biāo)導(dǎo)航算法存在非全局可觀測性問題,但通過融合航行器搭載的多傳感器(如DVL、磁航向傳感器等)信息,在導(dǎo)航初值與真實位置誤差較小的條件下,單信標(biāo)導(dǎo)航算法對特定軌跡具備局部可觀測性。文獻[6]和[13]利用基于Lie導(dǎo)數(shù)的非線性觀測秩序判據(jù)證明其局部可觀測性存在條件。假設(shè)聲信號水中傳輸速度恒定且已知,傳統(tǒng)的單信標(biāo)導(dǎo)航算法一般建立4狀態(tài)變量-2觀測量(4SV-2OB)模型,文中在4SV-2OB模型基礎(chǔ)上,增加徑向速度v作為系統(tǒng)觀測量,以下給出模型建立過程。

2.1 運動學(xué)方程

使用與傳統(tǒng)4SV-2OB模型相同的動力學(xué)方程,運動學(xué)模型[8]為

2.2 測量方程

為不失一般性,設(shè)信標(biāo)坐標(biāo)為(0,0),上式可寫為

圖3 徑向速度與對地速度幾何關(guān)系

Fig. 3 Geometric relationship between radial velocity and ground velocity

由于△～△～△,則有

參照測量方程標(biāo)準(zhǔn)形式,文中建立離散非線性測量方程

2.3 EKF

其中

需要說明的是,UUV進入信標(biāo)導(dǎo)航區(qū)域的初始位置與自身慣導(dǎo)系統(tǒng)的精度有關(guān)。當(dāng)初值較大、且發(fā)生距離模糊時,可根據(jù)式(7)同時跟蹤2條直航跡,其中一條為真實航跡,另一條為模糊航跡。隨后航行器自主轉(zhuǎn)向,僅有真實航跡能夠?qū)崿F(xiàn)連續(xù)跟蹤,而模糊航跡的估計方差矩陣會突然增大,可予以舍棄。

3 數(shù)值仿真

使用傳統(tǒng)4SV-2OB和文中建立的4SV-3OB模型完成UUV單信標(biāo)導(dǎo)航算法仿真試驗。試驗主要參數(shù)如表1所示,航跡估計仿真結(jié)果如圖4所示。

表1 仿真試驗主要參數(shù)

為比較改進模型性能,仿真試驗中使用較大的初始位置誤差,即將2個導(dǎo)航模型初值設(shè)置為(0,0)。由圖4可知,在初值較大的情況下,由于加入目標(biāo)徑向速度信息,4SV-3OB模型(紅色線)具有更快的收斂速度。定義導(dǎo)航估計誤差為

圖4 2種導(dǎo)航模型航跡估計比較

圖5 2種導(dǎo)航模型導(dǎo)航估計誤差曲線

Fig. 5 Navigation estimation error curves of the two navigation models

如誤差曲線所示,傳統(tǒng)4SV-2OB模型約在第600次迭代時收斂,而文中提出的4SV-3OB導(dǎo)航模型在第150次迭代時已經(jīng)完成收斂,收斂時間減小約75%。

4 結(jié)束語

目前,在全球海域全面部署水下外測導(dǎo)航系統(tǒng)并不具備可行性,但對于近岸要港、近海要地、海峽要道和遠海要域等重點關(guān)切海域,提前預(yù)置少量導(dǎo)航信標(biāo),以實現(xiàn)平戰(zhàn)結(jié)合的單信標(biāo)UUV導(dǎo)航具有一定軍事價值。

文中提出并建立了增設(shè)目標(biāo)徑向速度作為觀測量的單信標(biāo)UUV導(dǎo)航模型。使用幾何學(xué)手段,分析了引入目標(biāo)徑向速度觀測后單信標(biāo)導(dǎo)航系統(tǒng)的非全局可觀測性,給出任意航跡下單信標(biāo)導(dǎo)航算法初值估計方法。該方法較原有使用特殊運動軌跡估計導(dǎo)航初始位置的方法更為高效便捷,有效提升了單信標(biāo)導(dǎo)航方法的實用性與可操作性。在原有模型的基礎(chǔ)上,使用EKF建立了4SV- 3OB單信標(biāo)導(dǎo)航模型。通過相同條件下的仿真試驗,驗證了4SV-3OB模型相較于傳統(tǒng)模型具有更快的導(dǎo)航收斂速度,有效提升了單信標(biāo)導(dǎo)航方法的工程可用性。同時,4SV-3OB模型對于傳統(tǒng)多信標(biāo)水下彈道測量系統(tǒng),尤其是信標(biāo)故障條件下的外測導(dǎo)航算法創(chuàng)新也具有一定的研究價值。

[1] Paull L,Saeedi S,Seto M,et al. AUV Navigation and Localization: A Review[J]. IEEE Journal of Oceanic Engineering,2014,39(1): 131-149.

[2] iXblue. Marins Series Military & Strategic Grades Inertial Navigation Systems(INS)[EB/OL]. [2021-07-20]. https:// www.ixblue.com/sites/default/files/2021-05/marion-series_2020.pdf.

[3] Wang H,Fu G,Li J,et al. Strong Tracking CKF Based SLAM Method for Unmanned Underwater Vehicle[J]. Chinese Journal of Scientific Instrument,2013,34(11): 2542-2550.

[4] Zhang J,Shi C,Sun D,et al. High-precision,Limited-beacon-aided AUV Localization Algorithm[J]. Ocean Engineering,2018,149: 106-112.

[5] Vaganay J,Baccou P,Jouvencel B. Homing by acoustic ranging to a single beacon[C]//OCEANS 2000 MTS/IEEE Conference and Exhibition. Conference Proceedings(Cat. No.00CH37158). Providence,RI,USA: IEEE,2000.

[6] Gadre A S,Stilwell D J. A Complete Solution to Underwater Navigation in the Presence of Unknown Currents Based on Range Measurements from a Single Lo- cation[C]//2005 IEEE/RSJ International Conference on Intelligent Robots and Systems. Edmonton,AB,Canada: IEEE,2005.

[7] Gadre A S,Stilwell D J. Toward Underwater Navigation Based on Range Measurements from a Single Location [C]//IEEE International Conference on Robotics and Automation. New Orleans,LA,USA: IEEE,2004.

[8] Zhu Z,Hu S J. Model and Algorithm Improvement on Single Beacon Underwater Tracking[J]. IEEE Journal of Oceanic Engineering,2018,43(4): 1143-1160.

[9] Zhu Z,Hu S J. Model and Algorithm Improvement on Single Beacon Underwater Tracking[J]. IEEE Journal of Oceanic Engineering,2018,43(4): 1143-1160.

[10] Deng Z,Yu X,Zhu Z,et al. Adaptive Kalman Filter Based Single Beacon Underwater Tracking With Unknown Effective Sound Velocity[J]. Sensors,2018,18(12): 1-18.

[11] Qiao G,Asim I,Feng Z,et al. Experimental Assessment of a Multiple Sequence Direct Sequence Spread Spectrum (MS-DSSS) Underwater Acoustic Communication Scheme [C]//2013 OCEANS-San Diego. San Diego,CA,USA: IEEE. 2013.

[12] 韓正之,潘丹杰,張鐘俊. 非線性系統(tǒng)的能觀性和狀態(tài)觀測器[J]. 控制理論與應(yīng)用,1990(4): 1-9.

Han Zheng-zhi,Pan Dan-jie,Zhang Zhong-jun. Observability and Observers of Nonlinear Systems[J]. Control Theory & Applications,1990(4): 1-9.

[13] 劉明雍,李聞白,劉富檣,等. 基于單信標(biāo)測距的水下導(dǎo)航系統(tǒng)可觀測性分析[J]. 西北工業(yè)大學(xué)學(xué)報,2011,29(1): 87-92.

Liu Ming-yong,Li Wen-bai,Liu Fu-qiang,et al. Observability Analysis for Underwater Navigation System Based on Range Measurements with a Single Beacon[J]. Journal of Northwestern Polytechnical University,2011,29(1): 87-92.

[14] Wang S,Chen L,Hu H,et al. Single Beacon Based Localization of AUVs Using Moving Horizon Estimation [C]//2013 IEEE/RSJ International Conference on Intelligent Robots and Systems. Tokyo,Japan: IEEE,2013.

Model and Algorithm Improvement for Unmanned Undersea Vehicle Navigation Based on Single Beacon

,,

(College of Meteorology and Oceanology,National University of Defense Technology,Changsha 410073,China)

In the single beacon-based unmanned undersea vehicle(UUV) navigation algorithm,a small number of beacons,simple deployment and calibration at sea,and low maintenance costs are required. If combined with the inertial navigation system,this algorithm is expected to fulfill the needs for wide-area and low-cost underwater navigation. To solve the problems regarding its slow convergence speed and non-global observability,a single beacon underwater navigation algorithm,which increased target radial velocity observations,is proposed. The global observability of the system after the introduction of the target radial velocity observation is thereafter analyzed,and a new method to estimate the initial value and track the navigation is derived. Afterward,by combining this method with the extended Kalman filter algorithm,a 4-state 3-observation(4SV-3OB) single beacon navigation model is established. Finally,numerical simulations indicate that the convergence time of the 4SV-3OB is approximately 75% lower than that of the traditional single beacon navigation model. Simulation results show that adding the target radial velocity as a system observation can effectively simplify the process of initial value estimation and tracking in the single beacon navigation algorithm and can significantly reduce the convergence time of the navigation algorithm.

unmanned undersea vehicle; single beacon; underwater navigation; global observability; extended Kalman filter

U666.1; TB56

A

2096-3920(2021)04-0428-07

10.11993/j.issn.2096-3920.2021.04.009

陳祥國,尚凡,藍強. 基于單信標(biāo)的無人水下航行器導(dǎo)航算法及改進[J]. 水下無人系統(tǒng)學(xué)報,2021,29(4): 428-434.

2020-12-08;

2021-03-02.

陳祥國(1979-),男,博士,助理研究員,主要研究方向為智能優(yōu)化算法及海洋環(huán)境大數(shù)據(jù)等.

(責(zé)任編輯: 陳 曦)