陳建華，朱　海，郭正東，欒祿雨（海軍潛艇學(xué)院，山東　青島 266042）

?

基于水聲傳播時(shí)延補(bǔ)償?shù)膽T導(dǎo)誤差修正方法

陳建華，朱海，郭正東，欒祿雨
（海軍潛艇學(xué)院，山東青島 266042）

摘要:針對(duì)潛器慣導(dǎo)定位誤差修正問(wèn)題，提出慣導(dǎo)/多信標(biāo)水聲測(cè)距組合導(dǎo)航實(shí)現(xiàn)方法，并主要針對(duì)由于潛器運(yùn)動(dòng)與水聲傳播時(shí)間延遲導(dǎo)致的誤差進(jìn)行分析，提出一種基于水聲傳播時(shí)延補(bǔ)償?shù)乃聭T導(dǎo)定位誤差修正方法，該方法利用擴(kuò)展卡爾曼濾波，通過(guò)對(duì)慣導(dǎo)系統(tǒng)位置誤差狀態(tài)的前推，重構(gòu)量測(cè)方程，實(shí)現(xiàn)量測(cè)方程與系統(tǒng)量測(cè)量時(shí)間的一致性，補(bǔ)償時(shí)間延遲產(chǎn)生的誤差。仿真結(jié)果表明，該方法可有效提高慣導(dǎo)/多信標(biāo)水聲測(cè)距組合導(dǎo)航系統(tǒng)對(duì)慣導(dǎo)定位誤差修正的精度。

關(guān)鍵詞：慣導(dǎo)；定位誤差；EKF；水聲傳播時(shí)間延遲

0　引　言

目前，長(zhǎng)航時(shí)潛器水下自主導(dǎo)航主要依靠推算導(dǎo)航與慣性導(dǎo)航[1]，但2種導(dǎo)航方式都存在定位誤差隨時(shí)間積累的缺點(diǎn)，當(dāng)前水下隱蔽定位修正比較實(shí)用的是水聲定位方法，因此，國(guó)內(nèi)外在水聲定位系統(tǒng)輔助潛器定位方面進(jìn)行了大量研究，如文獻(xiàn)[2]將測(cè)距聲信標(biāo)與載體低成本導(dǎo)航傳感器的導(dǎo)航數(shù)據(jù)相融合，提出基于 EKF 的導(dǎo)航數(shù)據(jù)融合算法，解決復(fù)雜環(huán)境下的深水機(jī)器人位置估計(jì)問(wèn)題。文獻(xiàn)[3-4]研究了基于移動(dòng)長(zhǎng)基線的 AUV 協(xié)同導(dǎo)航算法。文獻(xiàn)[5-6]研究了只依靠航跡推算和潛器之間相對(duì)測(cè)距的情況下，潛器的相對(duì)定位問(wèn)題。文獻(xiàn)[7–8]則針對(duì)移動(dòng)長(zhǎng)基線 AUV 協(xié)同導(dǎo)航中的水聲傳播延遲誤差進(jìn)行分析，并給出補(bǔ)償方法。然而這些研究的前提都是 AUV 上不裝備慣導(dǎo)系統(tǒng)，只裝備簡(jiǎn)單的導(dǎo)航設(shè)備，系統(tǒng)模型則為 AUV 運(yùn)動(dòng)學(xué)模型。而隨著大型遠(yuǎn)程 AUV 的發(fā)展，以及其他潛器慣導(dǎo)系統(tǒng)水下校正的需求日益迫切，針對(duì)慣導(dǎo)/水聲測(cè)距組合導(dǎo)航的研究具有重要意義。本文以慣導(dǎo)系統(tǒng)誤差模型為基礎(chǔ)，提出慣導(dǎo)/多信標(biāo)測(cè)距組合導(dǎo)航實(shí)現(xiàn)方法，用于潛器慣導(dǎo)定位誤差修正，并主要針對(duì)由于潛器運(yùn)動(dòng)及水聲傳播時(shí)間延遲導(dǎo)致的誤差進(jìn)行分析及補(bǔ)償方法研究。

1　時(shí)間延遲誤差分析

由于水聲信號(hào)在水中傳播速度較慢（1500 m/s），而潛器處于運(yùn)動(dòng)狀態(tài)，因此必須考慮時(shí)間延遲導(dǎo)致的誤差。如圖1 所示，潛器發(fā)送 Ping 詢問(wèn)信號(hào)后收到信標(biāo)的應(yīng)答信號(hào)需要一定的時(shí)間。同時(shí)，由于潛器與各信標(biāo)之間的距離不等，潛器收到每個(gè)信標(biāo)應(yīng)答信號(hào)的時(shí)刻也不相同，加上潛器的自身運(yùn)動(dòng)，因而潛器在不同位置不同時(shí)刻收到信標(biāo)應(yīng)答信號(hào)。文獻(xiàn)[7]對(duì)此進(jìn)行分析，但存在問(wèn)題，下面給出詳細(xì)分析。

圖1　時(shí)間延遲誤差示意圖Fig. 1　Schematic diagram of the time-delay error

通常長(zhǎng)基線定位系統(tǒng)布放 4 個(gè)信標(biāo)，如圖2 所示。若不考慮時(shí)間延遲，根據(jù)長(zhǎng)基線定位原理，可得潛器與信標(biāo) 1 之間的定位方程如下[9]：

其中（x, y, z）為潛器在陣位坐標(biāo)系下的位置坐標(biāo)。此處，陣位坐標(biāo)系原點(diǎn)設(shè)在信標(biāo) 1 所在位置，坐標(biāo)軸指向與東北天地理坐標(biāo)系相同。

在陣位坐標(biāo)系下，潛器從 t0時(shí)刻發(fā)出 Ping 到應(yīng)答器收到應(yīng)答信號(hào)用時(shí)為，其中詢問(wèn)信號(hào)傳播時(shí)間，應(yīng)答信號(hào)傳播時(shí)間，其間潛器定深航行位移為 ΔS1= (Δx1,Δy1) ?？紤]到慣導(dǎo)具有較高的短時(shí)相對(duì)精度，該位移可由慣導(dǎo)輸出速度進(jìn)行推算。從而可得方程

圖2　長(zhǎng)基線定位系統(tǒng)Fig. 2　The LBL positioning system

上式兩邊平方并相加

整理為

對(duì)于等式右邊，令Δt1≈Δt2=ΔT1，則有

值得指出的是，上述推導(dǎo)過(guò)程中的聲速 C 為水聲系統(tǒng)利用水聲傳播特性相關(guān)理論進(jìn)行修正過(guò)的聲速，即 R1為水聲系統(tǒng)輸出的距離量測(cè)值。

2　考慮水聲傳播時(shí)間延遲的慣導(dǎo)定位誤差修正算法

2.1系統(tǒng)方程

在本系統(tǒng)中，深度 z 可由深度傳感器直接獲取，故誤差模型中不考慮 z 方向速度誤差及深度 z 的誤差。

選取狀態(tài)變量X=[αβγδVXδVYδLδλ]T，可根據(jù)捷聯(lián)慣導(dǎo)系統(tǒng)誤差模型寫出系統(tǒng)方程

系統(tǒng)矩陣F及w的具體形式見(jiàn)文獻(xiàn)[10]。

2.2重構(gòu)量測(cè)方程

由前述推導(dǎo)，可得量測(cè)方程(以信標(biāo) 1 為例)

為補(bǔ)償時(shí)間延遲造成的誤差，重構(gòu)量測(cè)方程。

潛器在 t0時(shí)刻的位置狀態(tài)可由 t4時(shí)刻位置向前推求得

其中，（Δx4, Δy4）為潛器從 t0時(shí)刻到 t4時(shí)刻的航行位移，（xk，yk）是 t4時(shí)刻真實(shí)位置。

代入上式有

以 R 為量測(cè)量，可得 EKF 的量測(cè)方程，

式中：vz為量測(cè)噪聲，設(shè)為獨(dú)立且不相關(guān)的零均值高斯白噪聲，其方差為

量測(cè)方程的雅克比矩陣為

其中，

可得系統(tǒng)量測(cè)矩陣為

3　仿真分析

為驗(yàn)證算法的有效性，進(jìn)行仿真試驗(yàn)。在仿真試驗(yàn)中，在水下 500 m 深度布放 4 個(gè)浮標(biāo)，組成 4 000 m × 2 000 m 的長(zhǎng)方形基陣。潛器直線航行，速度 2 m/s，航向 90°，深度 50 m。潛器 SINS 數(shù)據(jù)更新周期為 1 s。其中慣性元件參數(shù)如表 1 所示。

在進(jìn)入信標(biāo)作用范圍之前， SINS 導(dǎo)航誤差為：緯度誤差–200 m，經(jīng)度誤差 200 m，失準(zhǔn)角為（2′, 2′, 1′）。信標(biāo) 1 位置為 L = 30°， λ = 120°。潛器開(kāi)始發(fā)射第1個(gè)Ping 信號(hào)的位置在陣位坐標(biāo)系中表示為（–2 000 m, 1 000 m），潛器每隔 10 s 向外發(fā)送一次 Ping 詢問(wèn)信號(hào)，完全收到各信標(biāo)的應(yīng)答信號(hào)后進(jìn)行慣導(dǎo)定位誤差解算。修正過(guò)程持續(xù) 500 s。位置誤差曲線如圖3 所示。

表1　SINS誤差源參數(shù)Tab. 1　SINS error source parameters

圖3　慣導(dǎo)定位誤差曲線Fig. 3　INS positioning error curve

由圖3 可以看出，如果不進(jìn)行水聲傳播時(shí)延誤差補(bǔ)償，則經(jīng)過(guò)水聲測(cè)距修正后，慣導(dǎo)定位誤差仍然超過(guò) 30 m，采用本文方法進(jìn)行補(bǔ)償后，定位精度可達(dá)到10 m 以內(nèi)。可見(jiàn)，本文方法可有效補(bǔ)償潛器運(yùn)動(dòng)和水聲傳播時(shí)延帶來(lái)的誤差。

4　結(jié)　語(yǔ)

本文提出慣導(dǎo)/多信標(biāo)測(cè)距組合導(dǎo)航實(shí)現(xiàn)方法，用于潛器慣導(dǎo)定位誤差修正，并主要針對(duì)由于潛器運(yùn)動(dòng)及水聲傳播時(shí)間延遲導(dǎo)致的誤差進(jìn)行分析，并給出補(bǔ)償方法。即利用潛器在一個(gè) Ping 采樣周期內(nèi)，慣導(dǎo)短時(shí)相對(duì)精度較高的特點(diǎn)，通過(guò)對(duì)慣導(dǎo)位置誤差狀態(tài)的前推，重構(gòu)量測(cè)方程，與量測(cè)信息時(shí)間匹配，以修正時(shí)間延遲造成的誤差。最后進(jìn)行數(shù)學(xué)仿真，驗(yàn)證了方法的有效性。

參考文獻(xiàn)：

[1]劉芙蓉, 陳輝. 自主式水下潛器研究開(kāi)發(fā)綜述[J]. 艦船科學(xué)技術(shù), 2008, 30(5): 20–23. LIU Fu-rong, CHEN Hui. A review of the development and research of autonomous underwater vehicles[J]. Ship Science and Technology, 2008, 30(5): 20–23.

[2]冀大雄, 劉健, 周波, 等. 深水機(jī)器人低成本導(dǎo)航系統(tǒng)的位置估計(jì)方法研究[J]. 儀器儀表學(xué)報(bào), 2009, 30(1): 35–38. JI Da-xiong, LIU Jian, ZHOU Bo, et al. Position estimation method for deep water vehicle using low-cost navigation sensors[J]. Chinese Journal of Scientific Instrument, 2009, 30(1): 35–38.

[3]張立川, 徐德民, 劉明雍. 雙偽測(cè)量的多水下航行器移動(dòng)長(zhǎng)基線協(xié)同導(dǎo)航算法[J]. 火力與指揮控制, 2013, 38(1): 34–36. ZHANG Li-chuan, XU De-min, LIU Ming-yong. Cooperative navigation method of multiple autonomous underwater vehicles based on double acoustic measurement[J]. Fire Control &Command Control, 2013, 38(1): 34–36.

[4]張福斌, 馬朋. 一種基于虛擬移動(dòng)長(zhǎng)基線的多AUV協(xié)同導(dǎo)航算法[J]. 魚雷技術(shù), 2013, 21(2): 115–119. ZHANG Fu-bin, MA Peng. A cooperative navigation algorithm of multiple AUVs based on synthetic moving long baseline[J]. Torpedo Technology, 2013, 21(2): 115–119.

[5]ENGEL R, KALWA J. Relative positioning of multiple underwater vehicles in the GREX project[C]//Proceedings of the 2009-EUROPE Oceans. Bremen: IEEE, 2009: 1–7.

[6]WOITHE H C, KREMER U. Using slocum gliders for coordinated spatial sampling[C]//Proceedings of the 2011 IEEESpain Oceans. Santander: IEEE, 2011: 1–8.

[7]劉明雍, 黃博, 蔡挺. 一種基于量測(cè)偽距的EKF移動(dòng)長(zhǎng)基線AUV協(xié)同導(dǎo)航方法[J]. 魚雷技術(shù), 2012, 20(6): 432–436. LIU Ming-yong, HUANG Bo, CAI Ting. A cooperative navigation method of EKF moving long baseline for AUV based on pseudo-range measurements[J]. Torpedo Technology, 2012, 20(6): 432–436.

[8]高偉, 楊建, 劉菊, 等. 基于水聲通信延遲的多UUV協(xié)同定位算法[J]. 系統(tǒng)工程與電子技術(shù), 2014, 36(3): 539–545. GAO Wei, YANG Jian, LIU Ju, et al. Cooperative location of multiple UUVs based on hydro-acoustic communication delay[J]. Systems Engineering and Electronics, 2014, 36(3): 539–545.

[9]陳云飛, 李桂娟, 賈兵, 等. 水下合作目標(biāo)三維定位技術(shù)[J].艦船科學(xué)技術(shù), 2010, 32(5): 48–51. CHEN Yun-fei, LI Gui-juan, JIA Bing, et al. Underwater cooperate target 3D positioning[J]. Ship Science and Technology, 2010, 32(5): 48–51.

[10]葛錫云, 申高展, 潘瓊文, 等. 基于慣性導(dǎo)航/地磁的水下潛器組合導(dǎo)航定位方法[J]. 艦船科學(xué)技術(shù), 2014, 36(11): 120 –124. GE Xi-yun, SHEN Gao-zhan, PAN Qiong-wen, et al. The underwater integrated navigation method of underwater vehicle based on INS/geomagnetic[J]. Ship Science and Technology, 2014, 36(11): 120–124.

INS positioning error correction based on acoustic propagation time compensation

CHEN Jian-hua, ZHU Hai, GUO Zheng-dong, LUAN Lu-yu
(Navy Submarine Academy, Qingdao 266042, China)

Abstract:In allusion to positioning error correction for underwater vehicle INS(Inertial Navigation System), the INS/multi-becons acoustic range measurements integrating method is proposed. The error result from vehicle motion and acoustic propagation time is mainly analyzed, and a correcting positioning error algorithm based on acoustic propagation time compensation is presented. In this algorithm, the EKF(Extended Kalman Filters) is adopted, and the measurement equation is reconstructed via pushing-forward the INS’s position error states. As a result, the measurement equation and the system measurement become synchronous, and the error from time delay is eliminated. The simulation results indicate that the algorithm can significantly improve the accuracy of correcting INS positioning error with INS/multi-becons acoustic range measurements integrated navigation system.

Key words:INS；positioning error；EKF；acoustic propagation time

作者簡(jiǎn)介：陳建華(1987–)，男，博士研究生，主要從事水下導(dǎo)航及信息融合技術(shù)研究。

收稿日期：2015–07–16； 修回日期： 2015–09–06

文章編號(hào)：1672–7619(2016)03–0097–04

doi：10.3404/j.issn.1672–7619.2016.03.020

中圖分類號(hào)：U666.11

文獻(xiàn)標(biāo)識(shí)碼：A