李賢義,齊建宇,趙鵬飛

(北京航天自動(dòng)控制研究所，北京 100854)

?

【信息科學(xué)與控制工程】

SINS/DVL水下初始對(duì)準(zhǔn)與定位導(dǎo)航

李賢義,齊建宇,趙鵬飛

(北京航天自動(dòng)控制研究所，北京 100854)

在僅有初始位置信息的條件下，給出一種SINS/DVL組合導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)與定位導(dǎo)航的方法；以慣性坐標(biāo)系作為SINS解算的參考基準(zhǔn)并借助DVL測(cè)量的信息，用四元數(shù)卡爾曼濾波進(jìn)行初始對(duì)準(zhǔn)；計(jì)算載體在慣性凝固載體坐標(biāo)系下的位移增量再轉(zhuǎn)換至導(dǎo)航坐標(biāo)系，完成在對(duì)準(zhǔn)過(guò)程中及對(duì)準(zhǔn)完成以后的定位導(dǎo)航；處理試驗(yàn)數(shù)據(jù)表明：該方法能夠?qū)崿F(xiàn)SINS/DVL組合導(dǎo)航系統(tǒng)水下航向大失準(zhǔn)角初始對(duì)準(zhǔn)與定位導(dǎo)航，可在600 s內(nèi)完成初始對(duì)準(zhǔn)，在航行4 000 s后定位精度約為航程的0.4%。

SINS/DVL；初始對(duì)準(zhǔn)；四元數(shù)；定位導(dǎo)航

目前，常見(jiàn)捷聯(lián)慣導(dǎo)/聲多普勒速度計(jì)程儀(SINS/DVL)組合導(dǎo)航系統(tǒng)是在水面系泊或航行狀態(tài)下根據(jù)GPS等信號(hào)源提供的速度位置信息，進(jìn)行初始對(duì)準(zhǔn)，由GPS等信號(hào)源裝訂組合導(dǎo)航階段的初始位置信息，然后利用SINS和DVL測(cè)量的信息進(jìn)行組合導(dǎo)航。在特定情況下，SINS/DVL組合導(dǎo)航系統(tǒng)也需要具備在水下完成初始對(duì)準(zhǔn)的能力。水下環(huán)境復(fù)雜，信息源少，GPS信號(hào)在水中衰減很快，在水深范圍內(nèi)無(wú)法直接接收衛(wèi)星信號(hào)，無(wú)法獲得連續(xù)的位置信息。因此在僅有初始位置信息的條件下，利用SINS/DVL組合導(dǎo)航系統(tǒng)自身的測(cè)量信息，完成水下航向大失準(zhǔn)角初始對(duì)準(zhǔn)，滿(mǎn)足建立精確的姿態(tài)、速度和位置信息的要求，并兼顧考慮對(duì)準(zhǔn)時(shí)間、計(jì)算量、數(shù)據(jù)存儲(chǔ)容量的要求，是一項(xiàng)有意義的研究工作。

針對(duì)慣導(dǎo)系統(tǒng)初始對(duì)準(zhǔn)和對(duì)準(zhǔn)完成時(shí)刻位置信息精確求解， Thompson IC[1]、嚴(yán)恭敏[2]提出了逆向組合導(dǎo)航算法，正向卡爾曼濾波、正向?qū)Ш浇馑愫湍嫦蚩柭鼮V波、逆向?qū)Ш浇馑阃瑫r(shí)進(jìn)行，對(duì)歷史數(shù)據(jù)進(jìn)行逆向計(jì)算，重建航跡得到對(duì)準(zhǔn)結(jié)束時(shí)刻精確位置。李萬(wàn)里[2]提出了與之類(lèi)似的SINS/DVL組合導(dǎo)航回溯算法，并提出了有效的慣性測(cè)量數(shù)據(jù)壓縮方案，降低了數(shù)據(jù)存儲(chǔ)量，提升了運(yùn)算速度。嚴(yán)恭敏[4]還提出在車(chē)載條件下，將載體的位移增量先轉(zhuǎn)換到慣性坐標(biāo)系，再根據(jù)每次更新求得的姿態(tài)矩陣將位移增量轉(zhuǎn)換至導(dǎo)航坐標(biāo)系，具備在對(duì)準(zhǔn)完成時(shí)刻修正位置信息的能力。

本文在文獻(xiàn)[4]的基礎(chǔ)上，給出一種SINS/DVL組合導(dǎo)航系統(tǒng)水下航向大失準(zhǔn)角初始對(duì)準(zhǔn)與定位導(dǎo)航的方法，以慣性坐標(biāo)系作為SINS解算參考基準(zhǔn)并借助DVL測(cè)量的信息，初始對(duì)準(zhǔn)和定位導(dǎo)航同時(shí)進(jìn)行，將四元數(shù)卡爾曼濾波用于姿態(tài)信息更新，按時(shí)間分段計(jì)算載體在慣性凝固載體坐標(biāo)系下的位移增量，具備了在對(duì)準(zhǔn)完成時(shí)刻獲得較為精確位置信息的能力，并可以適用于更長(zhǎng)時(shí)間的定位導(dǎo)航。

1 姿態(tài)初始對(duì)準(zhǔn)

SINS/DVL組合導(dǎo)航系統(tǒng)在水下容易受到浪涌、振動(dòng)等影響，現(xiàn)有技術(shù)無(wú)法保證載體在水下處于完全靜止?fàn)顟B(tài)，因此載體在水下保持懸停也無(wú)法根據(jù)陀螺和加速度計(jì)的輸出直接計(jì)算姿態(tài)矩陣。本文以慣性坐標(biāo)系作為SINS解算參考基準(zhǔn)并借助DVL測(cè)量的信息完成初始對(duì)準(zhǔn)，涉及的坐標(biāo)系包括：當(dāng)?shù)氐乩碜鴺?biāo)系(空間指向東北天)、慣導(dǎo)載體坐標(biāo)系(b系)、慣性凝固載體坐標(biāo)系(ib0系)、慣性凝固導(dǎo)航坐標(biāo)系(in0系)，選取當(dāng)?shù)氐乩碜鴺?biāo)系為導(dǎo)航坐標(biāo)系(n系)，具體定義見(jiàn)文獻(xiàn)[2]。

(1)

(2)

其中，c表示cos，s表示sin，Δ=λt-λ0+wiet。

(3)

(4)

(5)

(6)

(7)

(8)

2 四元數(shù)卡爾曼濾波

2.1 量測(cè)方程

將四元數(shù)矢量部分放在標(biāo)量部分的前面，以下用b、r表示b(t)、r(t)，將b、r表示成四元數(shù)的形式：

(9.1)

(9.2)

用Q表示in0到ib0的旋轉(zhuǎn)四元數(shù)：

(10)

(11)

將3×1的矩陣x映射到4×4的矩陣γ(x)、χ(x)，映射關(guān)系如下：

(12.1)

(12.2)

根據(jù)以上定義，將矩陣的元素直接代入并整理化簡(jiǎn)得到

(13.1)

(13.2)

由方程(11)、(13)，有：

(14)

(15)

令：

(16)

(17)

(18)

將方程(15)～方程(18)代入方程(14)，得到

(19)

定義如下4×3的矩陣Ξ(Q)：

(20)

將矩陣元素代入并整理化簡(jiǎn)，得到：

(21)

將方程(21)代入方程(19)，得到：

(22)

設(shè)tk時(shí)刻的四元數(shù)Q為Qk，量測(cè)陣H記為Hk，則tk時(shí)刻離散化的觀測(cè)方程為：

(23)

假設(shè)δb為零均值的高斯白噪聲，協(xié)方差陣為Rk，即：

(24.1)

(24.2)

2.2 狀態(tài)方程

Q表示in0到ib0的旋轉(zhuǎn)四元數(shù)，慣性凝固載體坐標(biāo)系ib0和慣性凝固導(dǎo)航坐標(biāo)系in0相對(duì)于慣性空間均保持不變，所以Q在整個(gè)對(duì)準(zhǔn)過(guò)程中為常值。設(shè)tk-1時(shí)刻的四元數(shù)Q為Qk-1，有

(25)

2.3 量測(cè)噪聲方程

將量測(cè)方程改寫(xiě)如下：

(26)

(27)

δb的協(xié)方差陣Rk可以選取以下兩種形式進(jìn)行建模分析：

(28.1)

(28.2)

(29)

(30)

其中Ei、e1、e2、e3定義如下：

方程(29)中：

(31)

(32)

將方程(31)(32)代入方程(29)，得到：

(33)

2.4 四元數(shù)歸一化處理

(34)

(35)

3 定位導(dǎo)航

載體在導(dǎo)航坐標(biāo)系下的速度計(jì)算公式如下：

(36)

本文通過(guò)求解載體在ib0系下的位移增量實(shí)現(xiàn)對(duì)準(zhǔn)過(guò)程中及對(duì)準(zhǔn)完成以后的定位導(dǎo)航。在SINS/DVL組合導(dǎo)航系統(tǒng)對(duì)準(zhǔn)時(shí)間T內(nèi)，載體自身運(yùn)動(dòng)在ib0系下的位移增量是：

(37)

利用分部積分公式，載體在導(dǎo)航坐標(biāo)系n下的位移增量如下：

(38)

(39.1)

(39.2)

在對(duì)準(zhǔn)完成以后，載體按照以下方式進(jìn)行位置更新：

(40)

4 試驗(yàn)驗(yàn)證

本文采用船載試驗(yàn)的實(shí)測(cè)數(shù)據(jù)進(jìn)行算法的性能分析和驗(yàn)證。主要試驗(yàn)設(shè)備的指標(biāo)如下：激光陀螺零偏穩(wěn)定性0.01°/h(1σ)，加速度計(jì)零偏穩(wěn)定性1×10-4g0(1σ)，更新頻率100 Hz；DVL的測(cè)速精度0.5%V，更新頻率1 Hz；對(duì)比用的GPS接收機(jī)，位置更新頻率1 Hz。

把組合導(dǎo)航系統(tǒng)本身內(nèi)置算法稱(chēng)為算法1，試驗(yàn)過(guò)程按算法1要求進(jìn)行，在對(duì)準(zhǔn)階段試驗(yàn)船漂浮在長(zhǎng)江中央，對(duì)準(zhǔn)結(jié)束后試驗(yàn)船緩慢加速至約8 knot航行。本文所用算法稱(chēng)為算法2，算法2起始時(shí)刻選為算法1的對(duì)準(zhǔn)完成時(shí)刻。為評(píng)價(jià)算法2提供的姿態(tài)、速度、位置的準(zhǔn)確性，以算法1提供的姿態(tài)、速度、GPS提供的位置作為參考基準(zhǔn)。

圖1，圖2分別是兩種算法提供的姿態(tài)角曲線，速度曲線。由圖1，圖2可知，算法1在圖1中0時(shí)刻提供的航向角為308.18°，算法2在初始姿態(tài)角大小安全未知的情況下，假設(shè)初始航向角、俯仰角、橫滾角均為0，在對(duì)準(zhǔn)時(shí)間T=600 s后提供的姿態(tài)角、速度便能夠很好的跟蹤算法1提供的姿態(tài)角、速度，該初始對(duì)準(zhǔn)算法采用線性濾波，對(duì)初始航向角大小沒(méi)有要求，不需要復(fù)雜的線性化和非線性建模與濾波計(jì)算，在系統(tǒng)獲得初值位置信息后即可啟動(dòng)，不需要專(zhuān)門(mén)的系泊對(duì)準(zhǔn)時(shí)間，增強(qiáng)了系統(tǒng)的快速響應(yīng)能力。

圖1 姿態(tài)角曲線

圖2 速度曲線

圖3是以GPS提供的位置信息為基準(zhǔn)，算法2的位置誤差、里程及定位精度曲線，在600 s之前隨著姿態(tài)估計(jì)精度的提高，位置誤差和定位精度逐步趨于收斂；在600 s以后，位置誤差隨里程的增加呈增加的趨勢(shì)，定位精度基本維持在0.4%航程以?xún)?nèi)，說(shuō)明本文所給出的定位導(dǎo)航方法，在水下無(wú)GPS等信號(hào)源提供連續(xù)位置信息情況下，具備在對(duì)準(zhǔn)完成時(shí)刻獲得較為精確位置信息的能力，相比文獻(xiàn)[4]，可以適用于更長(zhǎng)時(shí)間的定位導(dǎo)航，與航跡重建補(bǔ)償算法、回溯導(dǎo)航算法和逆向?qū)Ш剿惴ㄏ啾?，不需要存?chǔ)大量的數(shù)據(jù)和回溯計(jì)算，適合于工程應(yīng)用。

圖3 位置誤差、里程及定位精度

5 結(jié)論

本文給出了一種SINS/DVL組合導(dǎo)航系統(tǒng)水下航向大失準(zhǔn)角初始對(duì)準(zhǔn)與定位導(dǎo)航的方法。在僅給定初始位置信息的情況下，即可實(shí)現(xiàn)SINS/DVL組合導(dǎo)航系統(tǒng)連續(xù)提供姿態(tài)、速度、位置信息。處理試驗(yàn)數(shù)據(jù)表明：本文所給方法有效可行。

[1] THOMPSON I C，KENNETH J，MORGAN S.Rapid Self-alignment of a Strapdown Inertial System through Real-time Reprocessing[P].United States Patent:US7 739045B2，2010-6-15.

[2] 嚴(yán)恭敏,嚴(yán)衛(wèi)生，徐德民.逆向?qū)Ш剿惴捌湓诮萋?lián)羅經(jīng)動(dòng)基座初始對(duì)準(zhǔn)中的應(yīng)用[C] //第27屆中國(guó)控制會(huì)議論文集.北京：北京航空航天大學(xué)出版社，2008:724-729.

[3] LI Wanli,WU Wenqi,WANG Jinling.A Novel Backtracking Navigation Scheme for Automoumous Underwater Vehicles[J].Measurerment,2013(9)：22.

[4] 嚴(yán)恭敏，翁俊，白亮，等.基于慣性參考系的動(dòng)基座初始對(duì)準(zhǔn)與定位導(dǎo)航[J].系統(tǒng)工程與電子技術(shù)，2011，33(3):618-621.

[5] CHOUKROUN D,BAR-ITZHACK I Y,OSHMAN Y.A novel quaternion Kalman filter[R].TAE Report 930,Faculty of Aerospace Engineering,Technion-Israel Institude of Technology,Haifa,Israel,2004.

[6] BAR-ITZHACK I Y,OSHMAN J.Attitude Determination from Vector Observations:Quaternion Estimation[J].IEEE Trans On AES,1985,21(1):128-135.

[7] 張力軍，張士峰，楊華波.基于 MEKF 的航天器姿態(tài)確定算法[J].國(guó)防科技大學(xué)學(xué)報(bào)，2013，35(6)：46-52.

[8] 尹劍.捷聯(lián)姿態(tài)解算中方向余弦與四元數(shù)法分析比較[J].四川兵工學(xué)報(bào)，2015，6(9):106-110.

[9] 項(xiàng)鳳濤.捷聯(lián)系統(tǒng)四元素姿態(tài)解算的精細(xì)積分法[J].四川兵工學(xué)報(bào)，2010,31(5):103-106.

[10]CHOUKROUN D,BAR-ITZHACK I Y,OSHMAN Y.Novel quaternion Kalman filter[J].IEEE Transactions on Aerospace and Electronic Systems,2006,42(1):174-190.

(責(zé)任編輯 楊繼森)

Initial Underwater Alignment and PositionDetermination for SINS/DVL

LI Xian-yi, QI Jian-yu, ZHAO Peng-fei

(Beijing Aerospace Automatic Control Institute， Beijing 100854， China)

The paper presented a method of initial underwater alignment and position determination for SINS/DVL integrated navigation system under the condition of the only initial position information. By taking inertial frame as computing reference and with the adding of DVL, a Kalman filter for estimating the quaternion was established to realize the initial alignment; Moreover, a real-time position determination ability was accessed by adding the distance of the carrier travelled in the navigation frame, which is transformed from that in the inertial frame, avoiding recording mass of data and “playback” calculation. Finally, some tests were carried out to prove the correctness and effectiveness of the presented method of initial underwater alignment with large course misalignment angle and position determination for SINS/DVL integrated navigation system, and the results show that it can achieve the initial alignment in 600 seconds and the position accuracy is about 0.4% of the distance travelled in 4 000 seconds.

SINS/DVL；initial alignment；quaternion；position determination

2016-11-22；

2016-12-25 作者簡(jiǎn)介：李賢義 (1990—)，男，碩士，主要從事慣性導(dǎo)航研究。

10.11809/scbgxb2017.04.029

李賢義,齊建宇,趙鵬飛.SINS/DVL水下初始對(duì)準(zhǔn)與定位導(dǎo)航[J].兵器裝備工程學(xué)報(bào)，2017(4):132-136.

format:LI Xian-yi, QI Jian-yu, ZHAO Peng-fei.Initial Underwater Alignment and Position Determination for SINS/DVL[J].Journal of Ordnance Equipment Engineering,2017(4):132-136.

V249.31

A

2096-2304(2017)04-0132-05