SINS/DVL水下初始對準與定位導(dǎo)航

李賢義,齊建宇,趙鵬飛

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

?

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

SINS/DVL水下初始對準與定位導(dǎo)航

李賢義,齊建宇,趙鵬飛

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

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

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

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

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

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

1 姿態(tài)初始對準

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

(1)

(2)

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

(3)

(4)

(5)

(6)

(7)

(8)

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

2.1 量測方程

將四元數(shù)矢量部分放在標量部分的前面，以下用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ù)以上定義，將矩陣的元素直接代入并整理化簡得到

(13.1)

(13.2)

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

(14)

(15)

令：

(16)

(17)

(18)

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

(19)

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

(20)

將矩陣元素代入并整理化簡，得到：

(21)

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

(22)

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

(23)

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

(24.1)

(24.2)

2.2 狀態(tài)方程

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

(25)

2.3 量測噪聲方程

將量測方程改寫如下：

(26)

(27)

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

(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)航坐標系下的速度計算公式如下：

(36)

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

(37)

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

(38)

(39.1)

(39.2)

在對準完成以后，載體按照以下方式進行位置更新：

(40)

4 試驗驗證

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

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

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

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

圖2 速度曲線

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

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

5 結(jié)論

本文給出了一種SINS/DVL組合導(dǎo)航系統(tǒng)水下航向大失準角初始對準與定位導(dǎo)航的方法。在僅給定初始位置信息的情況下，即可實現(xiàn)SINS/DVL組合導(dǎo)航系統(tǒng)連續(xù)提供姿態(tài)、速度、位置信息。處理試驗數(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] 嚴恭敏,嚴衛(wèi)生，徐德民.逆向?qū)Ш剿惴捌湓诮萋?lián)羅經(jīng)動基座初始對準中的應(yīng)用[C] //第27屆中國控制會議論文集.北京：北京航空航天大學(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] 嚴恭敏，翁俊，白亮，等.基于慣性參考系的動基座初始對準與定位導(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].國防科技大學(xué)學(xué)報，2013，35(6)：46-52.

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

[9] 項鳳濤.捷聯(lián)系統(tǒng)四元素姿態(tài)解算的精細積分法[J].四川兵工學(xué)報，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.

(責任編輯 楊繼森)

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 作者簡介：李賢義 (1990—)，男，碩士，主要從事慣性導(dǎo)航研究。

10.11809/scbgxb2017.04.029

李賢義,齊建宇,趙鵬飛.SINS/DVL水下初始對準與定位導(dǎo)航[J].兵器裝備工程學(xué)報，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