三應答器SINS/USBL緊耦合導航算法

張亞文，莫明崗，馬小艷，鄧繼權

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

?

三應答器SINS/USBL緊耦合導航算法

張亞文，莫明崗，馬小艷，鄧繼權

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

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

三應答器；捷聯慣性導航；超短基線；緊耦合

0 引言

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

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

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

(1)

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

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

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

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

本文提出的三應答器SINS/USBL緊耦合導航算法將避免USBL測角誤差以及系統誤差中的兩坐標系間的安裝角誤差等誤差項對組合導航精度的影響，并對USBL測距誤差進行在線估計補償。

2 緊耦合導航算法濾波模型及濾波方程

2.1 三應答器SINS緊耦合導航狀態模型

三應答器SINS/USBL緊耦合導航算法誤差狀態模型包括SINS的誤差狀態模型和USBL的誤差狀態模型。

2.1.1SINS誤差狀態模型

(2)

捷聯慣性導航誤差狀態方程為

(3)

2.1.2USBL誤差狀態模型

和單應答器USBL絕對定位誤差相比，三應答器USBL絕對定位誤差因素相對減小，主要包括應答器初始位置誤差和USBL實時測距誤差。應答器初始位置誤差一般經過布放時的標定測量便可以達到較高的精度。本算法將USBL測距精度誤差擴展為系統狀態，即

(4)

(5)

其中

(6)

2.1.3 緊耦合導航系統誤差狀態模型

結合2.1.1和2.1.2可得，18維的三應答器SINS/USBL緊耦合導航算法誤差狀態量為：

(7)

SINS/USBL組合導航的卡爾曼濾波狀態方程為

(8)

其中：

(9)

2.2 三應答器SINS/USBL緊耦合算法觀測方程

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

(10)

對式(10)求偏微分可得

(11)

則有

(12)

其中

(13)

化簡得

(14)

其中

聯立式(12)和式(14)可得

(15)

(16)

綜上可得，三應答器SINS/USBL緊耦合導航算法的觀測量為

(17)

三應答器SINS/USBL緊耦合導航算法觀測方程為

(18)

三應答器SINS/USBL緊耦合導航算法觀測矩陣為

(19)

三應答器SINS/USBL緊耦合導航算法量測方程為

Z=HX+v

(20)

3 仿真與分析

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

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

各傳感器誤差源和初始誤差設置如表 1。按照本節提出的三應答器SINS/USBL緊耦合算法進行仿真，并與傳統單應答器SINS/USBL組合導航仿真結果進行對比。

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

對比三應答器SINS/USBL緊耦合導航算法和單應答器SINS/USBL組合導航算法。其定位精度(1)對比如表 2，位置、速度和航向姿態誤差對比曲線如圖 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可以看出，三應答器SINS/USBL緊耦合算法和單應答器SINS/USBL組合導航算法相比，定位精度(1)有了很大的提高。從圖 3～圖 6位置誤差曲線圖可以看出，三應答器SINS/USBL緊耦合算法的定位誤差更小，且更加平滑。從圖 7速度誤差曲線和圖 8航向姿態誤差曲線可以看出，三應答器SINS/USBL緊耦合算法速度精度更高，航向姿態誤差的估計和修正能力更強。

圖8 航向姿態誤差曲線對比Fig.8 The comparison of attitude error

4 結論

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

[1] 蘭華林,孫大軍,張殿倫,李想.單應答器導航深海實驗[J].海洋工程,2007,25(2):110-113.

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

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

[4] 秦永元.慣性導航[M].北京:科學出版社,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 - )，男，碩士，助理工程師，主要從事慣性/水聲組合導航技術研究。

E-mail: zhangyawen0927@126.com

V249.32+2

A

2095-8110(2015)03-0029-05