光纖羅經(jīng)和衛(wèi)星測姿的組合導(dǎo)航算法

楊 曄，劉乃道，孟凡彬，張文杰，董金發(fā)

（天津航海儀器研究所，天津 300131）

光纖羅經(jīng)和衛(wèi)星測姿的組合導(dǎo)航算法

楊 曄，劉乃道，孟凡彬，張文杰，董金發(fā)

（天津航海儀器研究所，天津 300131）

為了提高光纖羅經(jīng)和衛(wèi)星測姿的組合導(dǎo)航精度和系統(tǒng)穩(wěn)定性，提出了一種光纖羅經(jīng)/衛(wèi)星測姿的組合導(dǎo)航算法。首先分別對光纖羅經(jīng)、衛(wèi)星測姿和組合系統(tǒng)的誤差進(jìn)行了數(shù)學(xué)建模，以姿態(tài)作為組合系統(tǒng)的觀測量，建立了組合系統(tǒng)的狀態(tài)方程和觀測方程。然后，給出了一種改進(jìn)的 Kalman濾波器，這種濾波器不但能夠保證濾波器穩(wěn)定可靠，防止濾波發(fā)散，還提高了準(zhǔn)確度，有助于提高組合導(dǎo)航精度。最后，通過靜態(tài)試驗和動態(tài)跑車試驗得出，光纖羅經(jīng)/衛(wèi)星測姿組合導(dǎo)航系統(tǒng)的航向角誤差在0.1°以內(nèi)；通過極區(qū)航行實驗得出，船舶在高緯度航行中組合導(dǎo)航系統(tǒng)航向角誤差在0.5°以內(nèi)。

光纖羅經(jīng)；衛(wèi)星測姿；組合導(dǎo)航；卡爾曼濾波

與傳統(tǒng)陀螺羅經(jīng)產(chǎn)品相比，光纖陀螺羅經(jīng)和衛(wèi)星測姿系統(tǒng)具有體積小、啟動快、精度高、壽命長、易安裝、全壽命周期成本低等多方面優(yōu)勢。其中，衛(wèi)星具有全球、全天候適用且無誤差積累的特點(diǎn)，缺點(diǎn)是會受環(huán)境因素影響，在天氣惡劣的情況下容易出現(xiàn)失鎖現(xiàn)象，造成輸出數(shù)據(jù)不連續(xù)。光纖羅經(jīng)的主要缺點(diǎn)是隨著緯度的升高，航向誤差增大[1]。

為了克服上述缺陷，尋求最優(yōu)設(shè)計，本文提出了一種新的光纖羅經(jīng)/衛(wèi)星姿態(tài)組合算法，利用光纖陀螺、衛(wèi)星及其相關(guān)的融合算法來綜合地測定航向，充分結(jié)合光纖羅經(jīng)安裝簡單、使用方便、精度高以及衛(wèi)星的全球、全天候、無誤差積累等特點(diǎn)，克服光纖羅經(jīng)和衛(wèi)星測姿各自的缺點(diǎn)，達(dá)到優(yōu)勢互補(bǔ)的目的。最后利用靜態(tài)實驗、動態(tài)跑車實驗和極區(qū)航行實驗驗證了組合算法對航向精度的提高所起的效果。

1 光纖羅經(jīng)/衛(wèi)星測姿組合導(dǎo)航系統(tǒng)建模

1.1 光纖羅經(jīng)誤差數(shù)學(xué)模型

利用慣性測量信息求解姿態(tài)信息的方法主要有方向余弦法和四元數(shù)法[2]。與方向余弦法相比，由于在相同舍位階數(shù)下四元數(shù)法計算后得到的姿態(tài)漂移值要小于方向余弦法得到的結(jié)果，并且算法簡單，計算量小，易于操作，因此，在本文的研究課題中，采用四元數(shù)法對光纖羅經(jīng)的姿態(tài)信息進(jìn)行求解。

在導(dǎo)航結(jié)算中組合導(dǎo)航狀態(tài)方程選取各導(dǎo)航參數(shù)的誤差為狀態(tài)量，因此，首先確定慣導(dǎo)系統(tǒng)的誤差方程，采用東北天地理坐標(biāo)系作為導(dǎo)航解算的基本坐標(biāo)系。在此坐標(biāo)系下某量的投影均用n表示。

① 姿態(tài)誤差方程為

推導(dǎo)得出姿態(tài)角誤差分量表達(dá)式：

式中，φx、 φy、φz為姿態(tài)誤差角， Vx、 Vy為東、北向速度，R為地球半徑，ωie為地球自轉(zhuǎn)角速率，εx、 εy、 εz為陀螺常值漂移[3]。

② 速度誤差方程為

式中， fn為加表比力，▽n為加表常值零偏。

③ 位置誤差方程為

式中，L、 h、λ分別為緯度、高度和經(jīng)度。

1.2 衛(wèi)星測姿誤差數(shù)學(xué)模型

利用衛(wèi)星測姿就是通過接收到的衛(wèi)星定位信號，特定的算法計算出天線陣列或軌跡的空間指向，進(jìn)一步處理成與傳統(tǒng)羅經(jīng)測量相當(dāng)?shù)膮?shù)，如方位角、俯仰角。其測向原理示意圖如圖1所示。

系統(tǒng)的兩幅天線A和B分別布置在與運(yùn)動載體軸線平行的連線上，接收同一時刻的衛(wèi)星導(dǎo)航信號。射頻前端和基帶處理單元與衛(wèi)導(dǎo)接收機(jī)一樣，射頻前端處理模塊通過天線接收所有可見衛(wèi)星信號，經(jīng)濾波放大后，變頻成中頻信號，最后經(jīng)模數(shù)轉(zhuǎn)換輸出數(shù)字中頻信號?；鶐?shù)字信號處理模塊對數(shù)字中頻信號進(jìn)行載波解調(diào)和偽碼解擴(kuò)，從中獲得偽距、多普勒頻移、載波相位等測量值以及解調(diào)出導(dǎo)航電文?；谳d波相位差分定位的核心技術(shù)就是整周模糊度的解算[4]。

與衛(wèi)導(dǎo)接收機(jī)不同的是，導(dǎo)航姿態(tài)解算模塊同時接收兩路基帶處理單元輸出的偽距、多普勒頻移、載波相位等測量值以及導(dǎo)航電文和整周模糊度，根據(jù)單差/雙差的相對定位模型，采用雙差法[5]（利用不同衛(wèi)星和不同天線之間的載波相位觀測量做二次差）解算出姿態(tài)量。

圖1 衛(wèi)星測姿原理示意圖Fig.1 Principle of satellite compass

假設(shè)天線A的坐標(biāo)已知并約定為主天線，天線B的坐標(biāo)未知并約定為副天線。那么，天線A和B構(gòu)成的基線矢量方向為從主天線指向副天線。首先利用雙差法得到雙差載波相位觀測方程：

式中，l、m、n為天線B指向衛(wèi)星的方向余弦參數(shù)，(Δ x,Δ y, Δ z)為概略坐標(biāo)和真實坐標(biāo)的偏差，即坐標(biāo)修正量， ▽Δ N為整周模糊度雙差值，其采用LAMDA方法[6]求解。

1.3 光纖羅經(jīng)/衛(wèi)星測姿組合系統(tǒng)的數(shù)學(xué)模型

1.3.1 組合系統(tǒng)的狀態(tài)方程

根據(jù)對組合系統(tǒng)的性能和誤差源分析，誤差方程選用15維誤差狀態(tài)向量：

式中，下標(biāo)x、y、z分別表示東北天地理坐標(biāo)系的三個方向， φx、 φy、 φz為姿態(tài)誤差角，δvx、 δ vy、δ vz為速度誤差，δφ 、δ λ、 δ h 為位置誤差；εx、 εy、 εz為陀螺常值漂移，▽x、 ▽y、 ▽z為加速度計常值零偏。其中，前9個為慣性基本誤差，后6個為慣性器件的誤差，共15個狀態(tài)量。

系統(tǒng)的誤差狀態(tài)方程：

式中：G(t)是系統(tǒng)噪聲驅(qū)動矩陣，A(t)為15×15維的系統(tǒng)陣，由分塊矩陣構(gòu)成；W(t)是噪聲驅(qū)動向量，W (t) =[wgx,wgy,wgz,wax,way,waz]中 wgx、 wgy、 wgz、 wax、 way、 waz分別為陀螺和加速度計的白噪聲。 AN9×9可由光纖羅經(jīng)誤差方程導(dǎo)出：

1.3.2 組合系統(tǒng)的量測方程

通過光纖羅經(jīng)估計得到的基線矢量[7-9]的誤差δD為

衛(wèi)星測姿系統(tǒng)中，利用LAMDA方法求得整周模糊度后，則可以得到載波相位雙差值殘差δφ為：

式中， ▽R?ik為第i顆衛(wèi)星到光纖羅經(jīng)估計的主天線距離與第j顆衛(wèi)星到光纖羅經(jīng)估計的主天線距離之間的差， ζφ為觀測噪聲向量。

結(jié)合以上分析，得到組合系統(tǒng)的觀測方程：

1.3.3 光纖羅經(jīng)/衛(wèi)星測姿系統(tǒng)組合測姿系統(tǒng)的卡爾

曼濾波器設(shè)計

狀態(tài)預(yù)測方程[10]：

狀態(tài)估計方程：

濾波增益方程：

估計均方誤差方程：

一步預(yù)測均方誤差方程：

2 改進(jìn)的卡爾曼濾波

光纖陀螺在慣性系統(tǒng)應(yīng)用中存在以下幾類誤差：陀螺零偏、標(biāo)度因數(shù)誤差、頻響寬度與延遲時間、光纖陀螺敏感軸失調(diào)角誤差等。這些誤差對測姿精度有一定的影響，同時這些誤差造成傳統(tǒng)的卡爾曼濾波無法準(zhǔn)確建模，存在濾波發(fā)散，系統(tǒng)不穩(wěn)定問題。

為了克服傳統(tǒng)的Kalman濾波的發(fā)散性，本文提出了一種改進(jìn)的卡爾曼濾波方法。在該方法中，速度加姿態(tài)組合模式需要對衛(wèi)星測姿系統(tǒng)輸出速度信息桿臂效應(yīng)補(bǔ)償，在衛(wèi)星測姿系統(tǒng)輸出處補(bǔ)償桿臂速度至光纖羅經(jīng)位置。具體算法如下：

按照前述算法模型，設(shè)計 Kalman濾波器。將桿臂效應(yīng)[11]補(bǔ)償后衛(wèi)星測姿速度信息及輸出三維姿態(tài)信息與光纖羅經(jīng)速度信息、三維姿態(tài)信息作差，作為Kalman濾波器量測信息，通過濾波估計出姿態(tài)誤差角φ和光纖羅經(jīng)與衛(wèi)星測姿系統(tǒng)的坐標(biāo)失調(diào)角λ。

“速度+姿態(tài)”組合模式對時間延遲比較敏感，光纖羅經(jīng)與衛(wèi)星測姿通過時統(tǒng)信息進(jìn)行同步，應(yīng)使時間延遲量盡量小，并且需要通過時統(tǒng)信息計算出時間延遲量▽T。對準(zhǔn)過程中用對齊后的t-▽T時刻光纖羅經(jīng)與衛(wèi)星測姿信息進(jìn)行匹配，估計出t-▽T時刻姿態(tài)誤差角 φ( t -▽ T)和光纖羅經(jīng)與衛(wèi)星測姿的坐標(biāo)失調(diào)角λ（ t-▽ T）。通過以上改進(jìn)的卡爾曼濾波器不但能夠保證濾波器穩(wěn)定可靠，防止濾波發(fā)散[12]，同時提高了準(zhǔn)確度，有助于提高組合導(dǎo)航精度。

3 實驗結(jié)果與驗證

在通過仿真實驗的前提下，分別進(jìn)行了靜態(tài)試驗、動態(tài)試驗，特別是在高緯度航行實驗中表現(xiàn)出高精度和高穩(wěn)定性的效果。

3.1 靜態(tài)試驗

在靜態(tài)情況下，GPS基線矢量長度為3 m，測試時間為 3～4 h。從中選取了一段組合系統(tǒng)觀測時間為7000～17000 s的歷元信息，用于光纖羅經(jīng)導(dǎo)航信息仿真與解算。利用Matlab進(jìn)行圖形化處理，得到如圖2所示靜態(tài)情況下的航向角誤差曲線圖。

圖2 靜態(tài)情況下的航向角誤差Fig.2 Heading error in static case

從圖 2可以看出，光纖羅經(jīng)穩(wěn)定航向角誤差為0.2°，衛(wèi)星測姿穩(wěn)定航向角誤差為 0.4°，組合系統(tǒng)穩(wěn)定航向角誤差小于0.1°。

3.2 動態(tài)試驗

動態(tài)跑車實驗時，將高精度的PHINS導(dǎo)航系統(tǒng)輸出數(shù)據(jù)作為姿態(tài)角基準(zhǔn)數(shù)據(jù)，對單光纖羅經(jīng)、單衛(wèi)星測姿和組合系統(tǒng)解算后的結(jié)果進(jìn)行比較分析，利用Matlab進(jìn)行圖形化處理，得到如圖3所示動態(tài)情況下的航向角誤差曲線圖。

實驗跑車數(shù)據(jù)如圖3所示。由圖3中光纖羅經(jīng)、衛(wèi)星測姿與組合系統(tǒng)的對比可知，組合系統(tǒng)相對于單光纖羅經(jīng)、單衛(wèi)星測姿，航向誤差明顯降低，精度得到的極大的提高，光纖羅經(jīng)航向角誤差為0.4°，衛(wèi)星測姿誤差為0.5°，組合系統(tǒng)航向角誤差為0.1°。

圖3 動態(tài)情況下航向角誤差Fig.3 Heading error in dynamic case

3.3 極區(qū)航行實驗

光纖羅經(jīng)系統(tǒng)隨中遠(yuǎn)集團(tuán)永盛輪成功往返北極航線，歷時55 d，航程20 000 nm，到達(dá)北緯78°05′的北極航道最高點(diǎn)。

按照實驗系統(tǒng)所工作地點(diǎn)緯度，將本次試驗分為以下兩個航行區(qū)域：

① 在低緯度區(qū)域（φ≤60°N）和高緯度區(qū)域（60°N＜φ≤77.5°N）范圍內(nèi)，系統(tǒng)采用羅經(jīng)算法；

② 在高緯度區(qū)域（77.5°N＜φ＜90°N）范圍內(nèi)，系統(tǒng)采用極區(qū)算法（地理坐標(biāo)系與格網(wǎng)坐標(biāo)系的切換）。

考慮到衛(wèi)星測姿艏向誤差不發(fā)散，本次試驗將該設(shè)備作為基準(zhǔn)設(shè)備。人工讀取并記錄組合系統(tǒng)、光纖羅經(jīng)、電羅經(jīng)在各緯度的艏向，并與衛(wèi)星測姿系統(tǒng)的數(shù)據(jù)進(jìn)行對比。鑒于航行時間比較長，截取其中具有代表性的一段航程進(jìn)行分析，如表1所示。

從隨船記錄的與衛(wèi)星測姿導(dǎo)航數(shù)據(jù)分析比對，組合系統(tǒng)不論在低緯度還是在高緯度航行，系統(tǒng)工作穩(wěn)定。試驗結(jié)果表明，光纖羅經(jīng)誤差變化范圍的最大值小于1°，組合羅經(jīng)系統(tǒng)航向誤差變化范圍的最大值小于0.5°，電羅經(jīng)在高位度區(qū)域誤差超過6°。由此可知，在高緯度航行中精度光纖羅經(jīng)/衛(wèi)星測姿組合系統(tǒng)明顯優(yōu)于傳統(tǒng)電羅經(jīng)。

表1 極區(qū)航行艏向數(shù)據(jù)Tab.1 Heading data in polar navigation

4 結(jié) 論

本文采用光纖羅經(jīng)和衛(wèi)星測姿的組合方案，設(shè)計的基于Kalman濾波器的速度、姿態(tài)組合算法。在衛(wèi)星測姿的基線長度為3 m的條件下，靜態(tài)情況航向角精度為0.05°；在動態(tài)情況下，根據(jù)與高精度的PHINS導(dǎo)航系統(tǒng)對比，航向角優(yōu)于0.1°；在高緯度航行實驗中航向誤差明顯優(yōu)于電羅經(jīng)。實驗結(jié)果表明，該算法能夠明顯提高衛(wèi)星測姿、光纖羅經(jīng)的姿態(tài)精度，達(dá)到的優(yōu)勢互補(bǔ)的目的。

（Reference）

[1] 張科, 劉海鵬, 李恒年, 錢山. SINS/GPS/CNS組合導(dǎo)航聯(lián)邦濾波算法[J]. 中國慣性技術(shù)學(xué)報, 2013, 21(2): 226-230. Zhang Ke, Liu Hai-peng, Li Heng-nian, Qian Shan. SINS/ GPS/CNS integrated navigation federal filtering algorithm[J]. Journal of Chinese Inertial Technology, 2013, 21(2): 226-230.

[2] 屈薔, 劉建業(yè), 熊智. 基于加性四元數(shù)誤差方程的慣性/天文姿態(tài)組合算法[J]. 中國慣性技術(shù)學(xué)報, 2011, 19(6): 316-319. Qu Qiang, Liu Jian-Ye, Xiong Zhi.Inertial/Celestial attitude integrated algorithm based on additive quaternion [J]. Journal of Chinese Inertial Technology, 2011, 19(6): 316-319.

[3] 戴東凱, 王省書, 戰(zhàn)德軍, 等. 基于單軸旋轉(zhuǎn)INS/GPS組合姿態(tài)誤差觀測的垂線偏差測量方法[J]. 中國慣性技術(shù)學(xué)報，2015, 23(4): 172-178. Dai Dong-kai, Wang Xing-shu, Zhan De-jun, et al. Measurement of dynamic vertical deflections by observing attitude errors of single-axis rotation INS/GPS system[J]. Journal of Chinese Inertial Technology, 2015, 23(4): 172-178.

[4] Ramanandan A, Chen A, Farrell J A. Inertial navigation aiding by stationary updates[J]//IEEE Transacation on Intelligent Transportation Systems, 2012, 13(1): 235-248.

[5] Dai D K, Wang X S, Zhan D J, et al. An improved method for dynamic measurement of deflections of the vertical based on the maintenance of attitude reference[J]. Sensors, 2014, 14(9): 16322-16342.

[6] 胡佩達(dá), 高鐘毓, 張嶸, 韓豐田. 基于三軸搖擺臺的高精度姿態(tài)試驗系統(tǒng)[J]. 中國慣性技術(shù)學(xué)報, 2013, 21(4): 271-274. Hu Pei-da, Gao Zhong-yu, Zhang Rong, Han Feng-tian. High accuracy attitude testing system based on three-axis test table[J]. Journal of Chinese Inertial Technology, 2013, 21(4): 271-274.

[7] Titterton D H, Weston J L. Strapdown inertial navigation technology[M]. London: Peter Pergrinus Ltd, 1997: 384-391.

[8] Johnson G, Waid J, Primm M, et al. Ship attitude accuracy trade study for aircraft approach and landing operations[C]//IEEE Position Location and Navigation Symposium, 2012: 783-790.

[9] 袁群哲, 周紅進(jìn). ADCP與光纖陀螺羅經(jīng)組合導(dǎo)航[J].中國慣性技術(shù)學(xué)報, 2007, 15(11): 713-720. Yuan Qun-zhe, Zhou Hong-jin. FOG compass/acoustic Dopper current profile integrated navigation[J]. Journal of Chinese Inertial Technology, 2007, 15(11): 713-720.

[10] Dini D H, Mandic D P, Julier S J. A widely linear complex unscented Kalman filter[J]//IEEE Signal Processing Letters, 2011, 18(11): 623-626.

[11] Grejner-Brzezinska D A, Yi, Toth Y C K. Precision GPS/ INS navigation in urban canyons: Dealing with GPS losses of lock[C]//Proc IAG 2001 Scientific Assembly. Budapest, Hungary, 2001.

[12] Yang Yun-chun, Farrell J A. Two antennas GPS -aided INS for attitude determination[J]//IEEE Transactions on Control Systems Technology, 2003(11): 905-917.

Integrated navigation algorithm of FOG-compass and satellite-attitude

YANG Ye, LIU Nai-dao, MENG Fan-bin, ZHANG Wen-jie, Dong Jin-fa
(Tianjin Navigation Instrument Research Institute, Tianjin 300131, China)

In order to improve the precision and stability of an integrated FOG compass/satellite-attitude navigation, an integrated navigation algorithm is proposed based on FOG-compass/satellite-attitude. Firstly, the error models of an integrated FOG-compass/satellite-attitude system are mathematically established, and the state equation and the observation equation of the integrated system are derived by taking the attitude as the system’s observation. Then, an improved Kalman filter is proposed and used to ensure the stability and reliability of the filter, and improve the accuracy of the integrated navigation. The static test and dynamic in-car test both show that the heading error of the integrated system is less than 0.1°. The test for ship sailing at high latitude shows that this heading error is less than 0.5°.

FOG compass; satellite-attitude; integrated navigation; Kalman filter

U666.1

：A

2015-09-10；

：2015-11-25

天津市科技興海項目（KJXH2013-09，KJXH2014-10）；天津市海洋經(jīng)濟(jì)創(chuàng)新發(fā)展區(qū)域示范項目（CXSF2014-3）

楊曄（1968—），男，研究員，碩士生導(dǎo)師，從事導(dǎo)航、制導(dǎo)與控制方面研究工作。E-mail: liuyuyangye@eyou.com

1005-6734(2015)06-0746-05

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