張亞文，莫明崗，馬小艷，鄧?yán)^權(quán)

(北京自動(dòng)化控制設(shè)備研究所, 北京 100074)

?

三應(yīng)答器SINS/USBL緊耦合導(dǎo)航算法

張亞文，莫明崗，馬小艷，鄧?yán)^權(quán)

(北京自動(dòng)化控制設(shè)備研究所, 北京 100074)

SINS/USBL組合導(dǎo)航誤差因素主要包括USBL測(cè)距精度誤差、USBL測(cè)角誤差、USBL到SINS的安裝誤差以及應(yīng)答器的位置誤差，且現(xiàn)有USBL設(shè)備的普遍特點(diǎn)為：測(cè)距精度高而測(cè)角精度相對(duì)較差。針對(duì)上述誤差因素和特點(diǎn)，提出一種三應(yīng)答器SINS/USBL緊耦合導(dǎo)航算法，避免了USBL測(cè)角誤差以及USBL到SINS的安裝角誤差等對(duì)導(dǎo)航精度的影響，仿真結(jié)果表明能夠較大程度上提高組合導(dǎo)航定位精度。此外，該算法無(wú)需對(duì)安裝角進(jìn)行標(biāo)定，使用更加便捷。

三應(yīng)答器；捷聯(lián)慣性導(dǎo)航；超短基線；緊耦合

0 引言

捷聯(lián)慣導(dǎo)系統(tǒng)(SINS)能夠?yàn)樗螺d體提供完備的導(dǎo)航信息，但隨著時(shí)間推移，系統(tǒng)誤差會(huì)累積發(fā)散[1]。超短基線(USBL)定位系統(tǒng)能夠測(cè)量應(yīng)答器相對(duì)于基陣的位置信息[2-3]，可以作為輔助導(dǎo)航系統(tǒng)來(lái)抑制慣導(dǎo)系統(tǒng)誤差的發(fā)散。目前，SINS/USBL組合導(dǎo)航已成為水下導(dǎo)航技術(shù)研究的熱點(diǎn)。針對(duì)傳統(tǒng)單應(yīng)答器SINS/USBL組合導(dǎo)航受USBL測(cè)角誤差和安裝角誤差影響較大的問(wèn)題，以及現(xiàn)有USBL設(shè)備測(cè)距精度高而測(cè)角精度相對(duì)較低的普遍特點(diǎn)，本文提出一種三應(yīng)答器SINS/USBL緊耦合導(dǎo)航算法，能夠避免USBL測(cè)角誤差和安裝角誤差對(duì)導(dǎo)航精度帶來(lái)的影響，且無(wú)需對(duì)安裝角進(jìn)行標(biāo)定，使用方便，理論仿真結(jié)果表明本算法能夠有效提高導(dǎo)航精度。

圖1 三應(yīng)答器USBL定位原理示意圖Fig.1 The scheme of tri-responder USBL position theory

1 三應(yīng)答器超短基線絕對(duì)定位原理及定位誤差分析

(1)

利用事先標(biāo)定測(cè)量得到的應(yīng)答器位置和USBL實(shí)時(shí)測(cè)量的斜距值，便可求解該方程組得到基陣的絕對(duì)位置。這便是三應(yīng)答器USBL絕對(duì)定位的幾何工作原理。

傳統(tǒng)超短基線系統(tǒng)的定位誤差源可分為以下兩類：

1) 傳感器誤差：USBL測(cè)距誤差、USBL基陣姿態(tài)測(cè)量誤差、USBL測(cè)角誤差、測(cè)距交會(huì)求解造成的誤差、超短基線基陣陣元相位差引起的誤差、聲傳播引起的誤差、聲學(xué)噪聲引起的誤差等，此類誤差大都受制于超短基線設(shè)備本身的性能指標(biāo)。

2)系統(tǒng)誤差：超短基線基陣和應(yīng)答器安裝引起的誤差、校準(zhǔn)誤差，此類誤差常存在于使用過(guò)程中，包括：基陣坐標(biāo)系和姿態(tài)傳感器坐標(biāo)系安裝不完全一致；應(yīng)答器的初始絕對(duì)位置標(biāo)校不準(zhǔn)確。

本文提出的三應(yīng)答器SINS/USBL緊耦合導(dǎo)航算法將避免USBL測(cè)角誤差以及系統(tǒng)誤差中的兩坐標(biāo)系間的安裝角誤差等誤差項(xiàng)對(duì)組合導(dǎo)航精度的影響，并對(duì)USBL測(cè)距誤差進(jìn)行在線估計(jì)補(bǔ)償。

2 緊耦合導(dǎo)航算法濾波模型及濾波方程

2.1 三應(yīng)答器SINS緊耦合導(dǎo)航狀態(tài)模型

三應(yīng)答器SINS/USBL緊耦合導(dǎo)航算法誤差狀態(tài)模型包括SINS的誤差狀態(tài)模型和USBL的誤差狀態(tài)模型。

2.1.1SINS誤差狀態(tài)模型

(2)

捷聯(lián)慣性導(dǎo)航誤差狀態(tài)方程為

(3)

2.1.2USBL誤差狀態(tài)模型

和單應(yīng)答器USBL絕對(duì)定位誤差相比，三應(yīng)答器USBL絕對(duì)定位誤差因素相對(duì)減小，主要包括應(yīng)答器初始位置誤差和USBL實(shí)時(shí)測(cè)距誤差。應(yīng)答器初始位置誤差一般經(jīng)過(guò)布放時(shí)的標(biāo)定測(cè)量便可以達(dá)到較高的精度。本算法將USBL測(cè)距精度誤差擴(kuò)展為系統(tǒng)狀態(tài)，即

(4)

(5)

其中

(6)

2.1.3 緊耦合導(dǎo)航系統(tǒng)誤差狀態(tài)模型

結(jié)合2.1.1和2.1.2可得，18維的三應(yīng)答器SINS/USBL緊耦合導(dǎo)航算法誤差狀態(tài)量為：

(7)

SINS/USBL組合導(dǎo)航的卡爾曼濾波狀態(tài)方程為

(8)

其中：

(9)

2.2 三應(yīng)答器SINS/USBL緊耦合算法觀測(cè)方程

本算法以載體(SINS)相對(duì)三個(gè)應(yīng)答器的距離信息和USBL直接測(cè)量得到的載體相對(duì)于三個(gè)應(yīng)答器的斜距信息之差為觀測(cè)量。

(10)

對(duì)式(10)求偏微分可得

(11)

則有

(12)

其中

(13)

化簡(jiǎn)得

(14)

其中

聯(lián)立式(12)和式(14)可得

(15)

(16)

綜上可得，三應(yīng)答器SINS/USBL緊耦合導(dǎo)航算法的觀測(cè)量為

(17)

三應(yīng)答器SINS/USBL緊耦合導(dǎo)航算法觀測(cè)方程為

(18)

三應(yīng)答器SINS/USBL緊耦合導(dǎo)航算法觀測(cè)矩陣為

(19)

三應(yīng)答器SINS/USBL緊耦合導(dǎo)航算法量測(cè)方程為

Z=HX+v

(20)

3 仿真與分析

以某淺水區(qū)域探測(cè)為應(yīng)用背景，設(shè)計(jì)仿真條件為：慣導(dǎo)初始地理位置為北緯39.8°，東經(jīng)116.2°，高度為0，以3m/s(約6節(jié))的前向速度繞應(yīng)答器陣，行駛約2700s，理想初始航向姿態(tài)角都為0°。慣導(dǎo)采樣周期為5ms，USBL數(shù)據(jù)更新周期為2s，組合導(dǎo)航周期為2s。三應(yīng)答器位置依次為：(39.8010，-50，116.1967)，(39.8020，-50，116.1997)，(39.8028，-50，116.1966)，如圖 2所示。

圖2 三應(yīng)答器SINS/USBL緊耦合算法仿真軌跡Fig.2 Tightly integrated navigation simulation track

各傳感器誤差源和初始誤差設(shè)置如表 1。按照本節(jié)提出的三應(yīng)答器SINS/USBL緊耦合算法進(jìn)行仿真，并與傳統(tǒng)單應(yīng)答器SINS/USBL組合導(dǎo)航仿真結(jié)果進(jìn)行對(duì)比。

表1 傳感器誤差和導(dǎo)航初始誤差Tab.1 Error of the sensor and initial navigation

對(duì)比三應(yīng)答器SINS/USBL緊耦合導(dǎo)航算法和單應(yīng)答器SINS/USBL組合導(dǎo)航算法。其定位精度(1)對(duì)比如表 2，位置、速度和航向姿態(tài)誤差對(duì)比曲線如圖 3～圖 8。

表2 定位精度對(duì)比Tab.2 The comparison of position precision

圖3 北向位置誤差曲線 Fig.3 North position error

圖4 天向位置誤差曲線 Fig.4 Up position error

圖5 東向位置誤差曲線Fig.5 East position error

圖6 水平位置誤差曲線Fig.6 Level position error

圖7 速度誤差曲線對(duì)比 Fig.7 The comparison of velocity error

從表 2可以看出，三應(yīng)答器SINS/USBL緊耦合算法和單應(yīng)答器SINS/USBL組合導(dǎo)航算法相比，定位精度(1)有了很大的提高。從圖 3～圖 6位置誤差曲線圖可以看出，三應(yīng)答器SINS/USBL緊耦合算法的定位誤差更小，且更加平滑。從圖 7速度誤差曲線和圖 8航向姿態(tài)誤差曲線可以看出，三應(yīng)答器SINS/USBL緊耦合算法速度精度更高，航向姿態(tài)誤差的估計(jì)和修正能力更強(qiáng)。

圖8 航向姿態(tài)誤差曲線對(duì)比Fig.8 The comparison of attitude error

4 結(jié)論

本文針對(duì)傳統(tǒng)單應(yīng)答器SINS/USBL組合導(dǎo)航受USBL測(cè)角誤差和安裝角誤差影響較大問(wèn)題，以及現(xiàn)有USBL設(shè)備測(cè)距精度高而測(cè)角精度相對(duì)較低的普遍特點(diǎn)，提出一種三應(yīng)答器SINS/USBL緊耦合導(dǎo)航算法，避免了USBL測(cè)角誤差以及USBL到SINS安裝角誤差等對(duì)組合導(dǎo)航精度帶來(lái)的影響。仿真結(jié)果表明，和傳統(tǒng)單應(yīng)答器SINS/USBL組合導(dǎo)航算法相比，本文提出的三應(yīng)答器SINS/USBL緊耦合導(dǎo)航算法，能夠提高導(dǎo)航精度，具有可行性。

[1] 蘭華林,孫大軍,張殿倫,李想.單應(yīng)答器導(dǎo)航深海實(shí)驗(yàn)[J].海洋工程,2007,25(2):110-113.

[2] 隋海琛,田春和,韓德忠,王崇明. 水下定位系統(tǒng)誤差分析[J].水道港口,2010,31(1):69-72.

[3] LinkQuest Inc. TrackLink 1500 USBL Tracking System, User’s Guide Version 1500.7.2.

[4] 秦永元.慣性導(dǎo)航[M].北京:科學(xué)出版社,2006.

An Underwater Tightly Integrated Navigation Algorithm of Tri-responder SINS/USBL

ZHANG Ya-wen, MO Ming-gang, MA Xiao-yan, DENG Ji-quan

(Beijing Institute of Automatic Control Equipment，Beijing 100074，China)

The errors of SINS/USBL integrated navigation mainly include USBL range-measuring error, angle-measuring error, the fixation error and the position error of the responder. Besides, most USBL instruments have quite high precision in range-measuring and quite short precision in angle-measuring. For these errors and features, the paper designs an underwater tightly integrated navigation algorithm of tri-responder SINS/USBL, avoiding the angle-measuring error and the fixation error. The theory simulation proves the algorithm can improve the navigation precision. Besides, it can be more convenient because no angle-fixation calibration is needed.

Tri-responder; Strap-down inertial navigation; Ultra short base line；Tight integration

2015 - 03 - 10；

2015 - 04 - 02。

張亞文(1990 - )，男，碩士，助理工程師，主要從事慣性/水聲組合導(dǎo)航技術(shù)研究。

E-mail: zhangyawen0927@126.com

V249.32+2

A

2095-8110(2015)03-0029-05