李賢義,齊建宇,趙鵬飛

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

?

【信息科學與控制工程】

SINS/DVL水下初始對準與定位導航

李賢義,齊建宇,趙鵬飛

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

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

SINS/DVL；初始對準；四元數；定位導航

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

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

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

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

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

(1)

(2)

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

(3)

(4)

(5)

(6)

(7)

(8)

2 四元數卡爾曼濾波

2.1 量測方程

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

(9.1)

(9.2)

用Q表示in0到ib0的旋轉四元數：

(10)

(11)

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

(12.1)

(12.2)

根據以上定義，將矩陣的元素直接代入并整理化簡得到

(13.1)

(13.2)

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

(14)

(15)

令：

(16)

(17)

(18)

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

(19)

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

(20)

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

(21)

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

(22)

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

(23)

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

(24.1)

(24.2)

2.2 狀態(tài)方程

Q表示in0到ib0的旋轉四元數，慣性凝固載體坐標系ib0和慣性凝固導航坐標系in0相對于慣性空間均保持不變，所以Q在整個對準過程中為常值。設tk-1時刻的四元數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 四元數歸一化處理

(34)

(35)

3 定位導航

載體在導航坐標系下的速度計算公式如下：

(36)

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

(37)

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

(38)

(39.1)

(39.2)

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

(40)

4 試驗驗證

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

把組合導航系統(tǒng)本身內置算法稱為算法1，試驗過程按算法1要求進行，在對準階段試驗船漂浮在長江中央，對準結束后試驗船緩慢加速至約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)角大小安全未知的情況下，假設初始航向角、俯仰角、橫滾角均為0，在對準時間T=600 s后提供的姿態(tài)角、速度便能夠很好的跟蹤算法1提供的姿態(tài)角、速度，該初始對準算法采用線性濾波，對初始航向角大小沒有要求，不需要復雜的線性化和非線性建模與濾波計算，在系統(tǒng)獲得初值位置信息后即可啟動，不需要專門的系泊對準時間，增強了系統(tǒng)的快速響應能力。

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

圖2 速度曲線

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

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

5 結論

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

[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)生，徐德民.逆向導航算法及其在捷聯(lián)羅經動基座初始對準中的應用[C] //第27屆中國控制會議論文集.北京：北京航空航天大學出版社，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] 嚴恭敏，翁俊，白亮，等.基于慣性參考系的動基座初始對準與定位導航[J].系統(tǒng)工程與電子技術，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].國防科技大學學報，2013，35(6)：46-52.

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

[9] 項鳳濤.捷聯(lián)系統(tǒng)四元素姿態(tài)解算的精細積分法[J].四川兵工學報，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—)，男，碩士，主要從事慣性導航研究。

10.11809/scbgxb2017.04.029

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