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

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

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

?

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

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

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

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

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

0 引言

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

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

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

(1)

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

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

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

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

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

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絕對定位誤差相比，三應(yīng)答器USBL絕對定位誤差因素相對減小，主要包括應(yīng)答器初始位置誤差和USBL實時測距誤差。應(yīng)答器初始位置誤差一般經(jīng)過布放時的標(biāo)定測量便可以達(dá)到較高的精度。本算法將USBL測距精度誤差擴展為系統(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緊耦合算法觀測方程

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

(10)

對式(10)求偏微分可得

(11)

則有

(12)

其中

(13)

化簡得

(14)

其中

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

(15)

(16)

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

(17)

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

(18)

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

(19)

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

Z=HX+v

(20)

3 仿真與分析

以某淺水區(qū)域探測為應(yīng)用背景，設(shè)計仿真條件為：慣導(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)行對比。

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

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

表2 定位精度對比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 速度誤差曲線對比 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)誤差的估計和修正能力更強。

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

4 結(jié)論

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

[1] 蘭華林,孫大軍,張殿倫,李想.單應(yīng)答器導(dǎo)航深海實驗[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