海洋重力輔助導(dǎo)航系統(tǒng)的系泊對(duì)準(zhǔn)算法

祝燕華，蔡體菁，王立輝

（東南大學(xué) 儀器科學(xué)與工程學(xué)院，南京 210096）

海洋重力輔助導(dǎo)航系統(tǒng)的系泊對(duì)準(zhǔn)算法

祝燕華，蔡體菁，王立輝

（東南大學(xué) 儀器科學(xué)與工程學(xué)院，南京 210096）

捷聯(lián)慣導(dǎo)系統(tǒng)在系泊條件下的初始對(duì)準(zhǔn)是海洋重力輔助導(dǎo)航實(shí)用化的關(guān)鍵問(wèn)題之一，本質(zhì)上屬于三軸姿態(tài)確定問(wèn)題?；趹T性凝固假設(shè)，構(gòu)建了船載捷聯(lián)慣導(dǎo)系泊對(duì)準(zhǔn)模型，研究了傳統(tǒng)三軸姿態(tài)確定雙矢量算法。對(duì)雙矢量算法進(jìn)行改進(jìn)，提出了一種基于加權(quán)矢量和三軸姿態(tài)確定的系泊對(duì)準(zhǔn)算法，該算法利用兩個(gè)觀測(cè)矢量的誤差標(biāo)準(zhǔn)差構(gòu)建加權(quán)系數(shù)，生成了精度更高的基準(zhǔn)矢量，從而提高系泊對(duì)準(zhǔn)的精度，并且該算法只需進(jìn)行一次三軸姿態(tài)解算，保持了計(jì)算量小、耗時(shí)短的特點(diǎn)。數(shù)值仿真和船載試驗(yàn)結(jié)果表明，改進(jìn)后的系泊對(duì)準(zhǔn)算法在保證初始對(duì)準(zhǔn)快速性的前提下有效提高了對(duì)準(zhǔn)精度，兩個(gè)水平姿態(tài)角誤差小于4′，航向角誤差在10′左右，滿足海洋重力輔助導(dǎo)航系統(tǒng)的應(yīng)用需求。

捷聯(lián)慣導(dǎo)；重力；系泊對(duì)準(zhǔn)；三軸姿態(tài)確定

慣性導(dǎo)航是海上運(yùn)載體最主要的導(dǎo)航方式之一，具有不依賴外界信息、隱蔽性好、抗輻射性強(qiáng)、全天候等優(yōu)點(diǎn)，是完全自主的導(dǎo)航。由于慣性傳感器的精度約束，慣導(dǎo)系統(tǒng)必須借助于其他導(dǎo)航方式的輔助才能長(zhǎng)時(shí)間使用。重力輔助導(dǎo)航是利用重力特征傳感器測(cè)量的重力異常信息或重力梯度信息，與重力場(chǎng)特征圖匹配來(lái)修正慣導(dǎo)誤差的方法，保證了導(dǎo)航的無(wú)源性、抗干擾和長(zhǎng)航時(shí)，脫離了GPS的限制，具有重要的工程應(yīng)用價(jià)值[1-2]。

捷聯(lián)慣導(dǎo)系統(tǒng)在系泊條件下的初始對(duì)準(zhǔn)是海洋重力輔助導(dǎo)航實(shí)用化的一個(gè)關(guān)鍵問(wèn)題，本質(zhì)上屬于三軸姿態(tài)確定（Triaxial attitude determination，TRIAD）。TRIAD算法是通過(guò)兩個(gè)不相交向量在兩個(gè)坐標(biāo)系中的已知坐標(biāo)來(lái)確定姿態(tài)矩陣，即雙矢量定姿。在靜基座條件下，通常利用加速度計(jì)和陀螺儀測(cè)量的地球重力加速度和地球自轉(zhuǎn)角速度作為雙矢量。而當(dāng)載體處于系泊狀態(tài)時(shí)，由于搖擺運(yùn)動(dòng)的干擾，該方法不再適用。文獻(xiàn)[3-5]引入慣性凝固假設(shè)，提出了一種基于重力加速度的對(duì)準(zhǔn)方法，隔離了載體的搖擺運(yùn)動(dòng)，進(jìn)而應(yīng)用TRIAD算法來(lái)實(shí)現(xiàn)系泊對(duì)準(zhǔn)。

本文構(gòu)建了捷聯(lián)慣導(dǎo)系泊對(duì)準(zhǔn)的模型，研究了基于傳統(tǒng)TRIAD的系泊對(duì)準(zhǔn)算法。為了提高系泊對(duì)準(zhǔn)的精度，對(duì)TRIAD算法進(jìn)行了改進(jìn)，提出了一種基于加權(quán)矢量和的TRIAD算法，該算法利用兩個(gè)觀測(cè)矢量的誤差標(biāo)準(zhǔn)差構(gòu)建加權(quán)系數(shù)，構(gòu)造新的基準(zhǔn)矢量，從而提高基準(zhǔn)矢量的精度。通過(guò)數(shù)值仿真和船載試驗(yàn)對(duì)改進(jìn)算法進(jìn)行了驗(yàn)證，結(jié)果表明，基于加權(quán)矢量和的系泊對(duì)準(zhǔn)算法在保證計(jì)算快速性的前提下有效提高了對(duì)準(zhǔn)精度。

1 捷聯(lián)慣導(dǎo)系泊對(duì)準(zhǔn)模型的構(gòu)建

1.1 坐標(biāo)系描述

1）地心慣性坐標(biāo)系（i）：原點(diǎn)位于地球中心，oxi、oyi軸在赤道平面內(nèi)，oxi軸指向春分點(diǎn)，ozi軸指向地球極軸；

2）地球坐標(biāo)系（e）：原點(diǎn)位于地球中心，oxe軸通過(guò)零子午線，oze軸指向地球極軸；

3）導(dǎo)航坐標(biāo)系（n）：原點(diǎn)位于載體重心，oxn軸指向東（E），oyn軸指向北（N），ozn軸指向天頂（U）；

4）載體坐標(biāo)系（b）：原點(diǎn)位于載體重心，oxb軸指向載體橫軸方向，oyb軸指向載體縱軸方向，ozb軸指向載體豎軸方向；

5）載體慣性坐標(biāo)系（b0）：在初始對(duì)準(zhǔn)的起始時(shí)刻將載體坐標(biāo)系b經(jīng)慣性凝固后的坐標(biāo)系。

1.2 系泊對(duì)準(zhǔn)模型

根據(jù)姿態(tài)矩陣的鏈?zhǔn)椒▌t，任意時(shí)刻載體的姿態(tài)矩陣可由下式確定[3-6]：

2 雙矢量定姿算法

2.1 基于TRIAD的系泊對(duì)準(zhǔn)算法

已知參考坐標(biāo)系下兩條非平行單位矢量為 W1和W2，它們?cè)诒倔w坐標(biāo)系下對(duì)應(yīng)的單位矢量為V1和V2。以（W1, V1）為基準(zhǔn)矢量，分別在參考坐標(biāo)系和本體坐標(biāo)系中構(gòu)建兩組符合右手準(zhǔn)則的正交坐標(biāo)系，兩組正交坐標(biāo)系中各坐標(biāo)軸的單位矢量分別為[7-11]

由上述分析可知，TRIAD算法采用的是雙矢量定姿，那么，確定系泊對(duì)準(zhǔn)中的常值矩陣的關(guān)鍵在于尋求合適的雙矢量。

系泊條件下，捷聯(lián)慣導(dǎo)系統(tǒng)加速度計(jì)輸出的比力為

則該比力在載體慣性坐標(biāo)系內(nèi)的投影可表示為

為了消除載體干擾加速度的影響，在時(shí)間段[t0,t]內(nèi)對(duì)式(7)進(jìn)行積分可得：

由于干擾加速度近似呈周期性變化，經(jīng)過(guò)一個(gè)周期的積分近似為零，即

結(jié)合式(8)和式(9)可得：

式(10)中，慣性坐標(biāo)系下的重力加速度 gi與時(shí)間相關(guān)，可表示為

故

由式(10)可知，選取不同的積分時(shí)刻t1和可分別得到i系和b0系下的兩組矢量則可依據(jù) TRIAD算法求出常值矩陣進(jìn)而通過(guò)式(1)獲得姿態(tài)矩陣完成捷聯(lián)慣導(dǎo)系統(tǒng)的系泊對(duì)準(zhǔn)。

2.2 基于加權(quán)矢量和TRIAD的系泊對(duì)準(zhǔn)算法

根據(jù) TRIAD算法的原理，提高基準(zhǔn)矢量的精度便可提高姿態(tài)矩陣解算的精度。

假設(shè)一個(gè)由兩個(gè)傳感器組成的系統(tǒng)，給定參考坐標(biāo)系下兩矢量W1和W2。第1個(gè)傳感器觀測(cè)參考坐標(biāo)系下的 W1，得到觀測(cè)矢量為 V1，測(cè)量誤差的標(biāo)準(zhǔn)差為σ1；第2個(gè)傳感器觀測(cè)參考坐標(biāo)系下的W2，得到觀測(cè)矢量為V2，測(cè)量誤差的標(biāo)準(zhǔn)差為σ2。

定義兩個(gè)觀測(cè)矢量的加權(quán)系數(shù)為

顯然，最優(yōu)估計(jì)誤差的方差為

3 數(shù)值仿真研究

給定參考坐標(biāo)系下兩矢量W1和W2，以及參考坐標(biāo)系和本體坐標(biāo)系之間的姿態(tài)矩陣真值A(chǔ)，假設(shè)：

若傳感器沒(méi)有測(cè)量誤差，則本體坐標(biāo)系中對(duì)應(yīng)的兩個(gè)矢量應(yīng)為

而實(shí)際傳感器均存在測(cè)量誤差，假設(shè)其服從零均值高斯分布，仿真分為如下兩種情況：

1）兩傳感器等精度情況

圖1 等精度情況下姿態(tài)角誤差比較Fig.1 Comparison on attitude errors when with equal accuracy

2）兩傳感器非等精度情況

由圖1和圖2可知，加權(quán)矢量和TRIAD算法明顯提高了姿態(tài)解算的精度。仿真中選取若干不同的參考矢量和姿態(tài)矩陣真值，以及改變兩觀測(cè)傳感器的精度，能得到多組仿真結(jié)果。精度比較情況與上述相仿，限于篇幅不再列出。

加權(quán)矢量和TRIAD算法較之傳統(tǒng)的TRIAD算法同樣只需進(jìn)行一次 TRIAD解算，保持了計(jì)算量小、耗時(shí)短的特點(diǎn)，非常適用于進(jìn)行系泊對(duì)準(zhǔn)。

圖2 非等精度情況下姿態(tài)角誤差比較Fig.2 Attitude error comparison under condition of unequal accuracy

4 船載試驗(yàn)結(jié)果

為了驗(yàn)證基于加權(quán)矢量和TRIAD系泊對(duì)準(zhǔn)算法的工程實(shí)用性，于2015年6月在山東青島進(jìn)行了4次激光捷聯(lián)慣導(dǎo)系統(tǒng)船載試驗(yàn)。其中：激光陀螺儀精度為0.01 (°)/h，加速度計(jì)精度為1×10-5g；系泊對(duì)準(zhǔn)時(shí)間設(shè)置為120 s，其中，t1=60 s，t2=120 s。試驗(yàn)中配置高精度的慣導(dǎo)系統(tǒng)作為對(duì)比基準(zhǔn)，系統(tǒng)在船體內(nèi)的安裝情況如圖3所示。表1給出四次船載試驗(yàn)的系泊對(duì)準(zhǔn)結(jié)果。

由表1可知，基于加權(quán)矢量和的系泊對(duì)準(zhǔn)算法能在載體處于搖擺基座情況下實(shí)現(xiàn)系統(tǒng)的初始對(duì)準(zhǔn)，兩個(gè)水平姿態(tài)角誤差小于4′，航向角誤差在10′左右，達(dá)到了初始對(duì)準(zhǔn)的要求。

圖3 船載試驗(yàn)系統(tǒng)安裝情況Fig.3 System installation in shipboard experiment

表1 激光捷聯(lián)慣導(dǎo)系統(tǒng)系泊對(duì)準(zhǔn)試驗(yàn)結(jié)果Tab.1 Result of mooring alignment experiment of laser SINS

5 結(jié) 論

系泊條件下的初始對(duì)準(zhǔn)是海上運(yùn)載體捷聯(lián)慣導(dǎo)系統(tǒng)的關(guān)鍵技術(shù)之一，本文基于慣性凝固假設(shè)，構(gòu)建了系泊對(duì)準(zhǔn)模型，隔離了載體的搖擺運(yùn)動(dòng)，提出了一種基于加權(quán)矢量和的 TRIAD算法實(shí)現(xiàn)初始三軸姿態(tài)解算。該算法的優(yōu)點(diǎn)在于利用了觀測(cè)矢量的誤差標(biāo)準(zhǔn)差構(gòu)建了加權(quán)系數(shù)，生成了精度更高的基準(zhǔn)矢量，提高了系泊對(duì)準(zhǔn)的精度。由于該算法只需完成一次TRIAD解算，仍然具有計(jì)算量小、耗時(shí)短的特點(diǎn)，保證了系泊對(duì)準(zhǔn)的快速性。

綜上，基于加權(quán)矢量和的TRIAD算法能在保證計(jì)算快速性的前提下有效提高對(duì)準(zhǔn)精度，滿足海洋重力輔助導(dǎo)航系統(tǒng)的應(yīng)用需求，具有一定的理論和工程應(yīng)用價(jià)值。

（References）：

[1] Rice H, Mendelsohn L, Aarons R, et al. Next generation marine precision navigation system[C]//IEEE 2000 Position Location and Navigation Symposium. 2000: 200-206.

[2] Cai Ti-jing. Study on novel gravity passive navigation system[J]. Journal of Southeast University (Eng. Ed.), 2006, 22(1): 53-58.

[3] 秦永元, 朱新穎, 趙長(zhǎng)山, 等. 艦載機(jī)捷聯(lián)慣導(dǎo)自對(duì)準(zhǔn)方案設(shè)計(jì)與仿真[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2008, 16(1): 28-33. Qin Yong-yuan, Zhu Xin-ying, Zhao Chang-shan, et al. Design and simulation on SINS self-alignment for carrier born aircraft[J]. Journal of Chinese Inertial Technology, 2008, 16(1): 28-33.

[4] 孫偉, 孫楓, 楊琳. 搖擺基座下捷聯(lián)系統(tǒng)快速初始對(duì)準(zhǔn)研究[J]. 系統(tǒng)仿真學(xué)報(bào), 2013, 25(3): 495-498. Sun Wei, Sun Feng, Yang Lin. Research on rapid alignment of SINS with swaying motion[J]. Journal of System Simulation, 2013, 25(3): 495-498.

[5] 楊曄, 毋興濤, 楊建林, 等. 方位捷聯(lián)平臺(tái)重力儀分布式 Kalman濾波初始對(duì)準(zhǔn)算法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2014, 22(2): 191-194. Yang Ye, Wu Xing-tao, Yang Jian-lin, et al. Distributed Kalman filter initial alignment algorithm for azimuth strapdown platform gravimeter[J]. Journal of Chinese Inertial Technology, 2014, 22(2): 191-194.

[6] Acharya A, Sadhu S, Ghoshal T K. Improved self-alignment scheme for SINS using augmented measurement[J]. Aerospace Science and Technology, 2011, 15(2): 125-128.

[7] 江潔, 王英雷, 張廣軍. 一種改進(jìn)的基于雙矢量觀測(cè)的姿態(tài)確定算法[J]. 北京航空航天大學(xué)學(xué)報(bào), 2012, 38(8): 992-995. Jiang Jie, Wang Ying-lei, Zhang Guang-jun. Improved determination algorithm using two vector measurements [J]. Journal of Beijing University of Aeronautics and Astronautics, 2012, 38(8): 992-995.

[8] 劉曉輝, 黨亞民, 王潛心, 等. 雙基線姿態(tài)確定三種算法的比較分析[J]. 大地測(cè)量與地球動(dòng)力學(xué), 2013, 33(1): 123-127. Liu Xiao-hui, Dang Ya-min, Wang Qian-xin, et al. Analysis of three algorithm of attitude determination based on two baselines[J]. Journal of geodesy and geodynamics, 2013, 33(1): 123-127.

[9] Shuster M D. The optimization of TRIAD[J]. The Journal of the Astronautical Sciences, 2007, 55(2): 245-257.

[10] Bar-Itzhack I Y, Harman R R. Optimized TRIAD algorithm for attitude determination[J]. Journal of Guidance, Control and Dynamics, 1997, 20(1): 208-211.

[11] Celik O, Hajiyev C. A comparison of attitude determination methods for small satellites[C]//Proceedings of 6th International Conference on Recent Advances in Space Technologies. 2013: 261-264.

Mooring alignment algorithm for marine gravity aided navigation system

ZHU Yan-hua, CAI Ti-jing, WANG Li-hui

(Department of Instrument Science & Engineering, Southeast University, Nanjing 210096, China)

The initial alignment of SINS in mooring conditions is a key problem to the practicability of marine gravity aided navigation system, which is essentially a three-axis attitude determination (TRIAD) problem. This paper built up a shipboard SINS mooring alignment model based on inertial freezing hypothesis, and improved the two-vector attitude determination algorithm based on the study of traditional one, which is an mooring alignment algorithm based on weighted vector and TRIAD. In the new algorithm, the standard deviation of two observation vectors is utilized to construct the weighting coefficients and generate a higher precision reference vector. Thereby, the accuracy of mooring alignment is improved. The algorithm requires just one times of TRIAD calculation, so the system keeps the advantages of small computation amount and short time-consuming. Numerical simulation and shipboard experiment results show that the weighted vector TRIAD algorithm effectively improves the alignment accuracy without decreasing the rapidity of the initial alignment. The horizontal attitude error is less than 4′, and the heading angle error is about 10′, which meet the application requirements of marine gravity aided navigation system.

strapdown inertial navigation; gravity; mooring alignment; triaxial attitude determination

U666.1

A

1005-6734(2016)02-0160-04

10.13695/j.cnki.12-1222/o3.2016.02.004

2015-09-30；

2016-04-12

國(guó)家自然科學(xué)基金資助項(xiàng)目（61503077）；江蘇省自然科學(xué)基金資助項(xiàng)目（BK20130628）

祝燕華（1980—），女，副教授，從事慣性導(dǎo)航和組合導(dǎo)航技術(shù)研究。E-mail: zhuyh@seu.edu.cn