光纖羅經和衛星測姿的組合導航算法

楊 曄，劉乃道，孟凡彬，張文杰，董金發

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

光纖羅經和衛星測姿的組合導航算法

楊 曄，劉乃道，孟凡彬，張文杰，董金發

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

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

光纖羅經；衛星測姿；組合導航；卡爾曼濾波

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

為了克服上述缺陷，尋求最優設計，本文提出了一種新的光纖羅經/衛星姿態組合算法，利用光纖陀螺、衛星及其相關的融合算法來綜合地測定航向，充分結合光纖羅經安裝簡單、使用方便、精度高以及衛星的全球、全天候、無誤差積累等特點，克服光纖羅經和衛星測姿各自的缺點，達到優勢互補的目的。最后利用靜態實驗、動態跑車實驗和極區航行實驗驗證了組合算法對航向精度的提高所起的效果。

1 光纖羅經/衛星測姿組合導航系統建模

1.1 光纖羅經誤差數學模型

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

在導航結算中組合導航狀態方程選取各導航參數的誤差為狀態量，因此，首先確定慣導系統的誤差方程，采用東北天地理坐標系作為導航解算的基本坐標系。在此坐標系下某量的投影均用n表示。

① 姿態誤差方程為

推導得出姿態角誤差分量表達式：

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

② 速度誤差方程為

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

③ 位置誤差方程為

式中，L、 h、λ分別為緯度、高度和經度。

1.2 衛星測姿誤差數學模型

利用衛星測姿就是通過接收到的衛星定位信號，特定的算法計算出天線陣列或軌跡的空間指向，進一步處理成與傳統羅經測量相當的參數，如方位角、俯仰角。其測向原理示意圖如圖1所示。

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

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

圖1 衛星測姿原理示意圖Fig.1 Principle of satellite compass

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

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

1.3 光纖羅經/衛星測姿組合系統的數學模型

1.3.1 組合系統的狀態方程

根據對組合系統的性能和誤差源分析，誤差方程選用15維誤差狀態向量：

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

系統的誤差狀態方程：

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

1.3.2 組合系統的量測方程

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

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

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

結合以上分析，得到組合系統的觀測方程：

1.3.3 光纖羅經/衛星測姿系統組合測姿系統的卡爾

曼濾波器設計

狀態預測方程[10]：

狀態估計方程：

濾波增益方程：

估計均方誤差方程：

一步預測均方誤差方程：

2 改進的卡爾曼濾波

光纖陀螺在慣性系統應用中存在以下幾類誤差：陀螺零偏、標度因數誤差、頻響寬度與延遲時間、光纖陀螺敏感軸失調角誤差等。這些誤差對測姿精度有一定的影響，同時這些誤差造成傳統的卡爾曼濾波無法準確建模，存在濾波發散，系統不穩定問題。

為了克服傳統的Kalman濾波的發散性，本文提出了一種改進的卡爾曼濾波方法。在該方法中，速度加姿態組合模式需要對衛星測姿系統輸出速度信息桿臂效應補償，在衛星測姿系統輸出處補償桿臂速度至光纖羅經位置。具體算法如下：

按照前述算法模型，設計 Kalman濾波器。將桿臂效應[11]補償后衛星測姿速度信息及輸出三維姿態信息與光纖羅經速度信息、三維姿態信息作差，作為Kalman濾波器量測信息，通過濾波估計出姿態誤差角φ和光纖羅經與衛星測姿系統的坐標失調角λ。

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

3 實驗結果與驗證

在通過仿真實驗的前提下，分別進行了靜態試驗、動態試驗，特別是在高緯度航行實驗中表現出高精度和高穩定性的效果。

3.1 靜態試驗

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

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

從圖 2可以看出，光纖羅經穩定航向角誤差為0.2°，衛星測姿穩定航向角誤差為 0.4°，組合系統穩定航向角誤差小于0.1°。

3.2 動態試驗

動態跑車實驗時，將高精度的PHINS導航系統輸出數據作為姿態角基準數據，對單光纖羅經、單衛星測姿和組合系統解算后的結果進行比較分析，利用Matlab進行圖形化處理，得到如圖3所示動態情況下的航向角誤差曲線圖。

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

圖3 動態情況下航向角誤差Fig.3 Heading error in dynamic case

3.3 極區航行實驗

光纖羅經系統隨中遠集團永盛輪成功往返北極航線，歷時55 d，航程20 000 nm，到達北緯78°05′的北極航道最高點。

按照實驗系統所工作地點緯度，將本次試驗分為以下兩個航行區域：

① 在低緯度區域（φ≤60°N）和高緯度區域（60°N＜φ≤77.5°N）范圍內，系統采用羅經算法；

② 在高緯度區域（77.5°N＜φ＜90°N）范圍內，系統采用極區算法（地理坐標系與格網坐標系的切換）。

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

從隨船記錄的與衛星測姿導航數據分析比對，組合系統不論在低緯度還是在高緯度航行，系統工作穩定。試驗結果表明，光纖羅經誤差變化范圍的最大值小于1°，組合羅經系統航向誤差變化范圍的最大值小于0.5°，電羅經在高位度區域誤差超過6°。由此可知，在高緯度航行中精度光纖羅經/衛星測姿組合系統明顯優于傳統電羅經。

表1 極區航行艏向數據Tab.1 Heading data in polar navigation

4 結 論

本文采用光纖羅經和衛星測姿的組合方案，設計的基于Kalman濾波器的速度、姿態組合算法。在衛星測姿的基線長度為3 m的條件下，靜態情況航向角精度為0.05°；在動態情況下，根據與高精度的PHINS導航系統對比，航向角優于0.1°；在高緯度航行實驗中航向誤差明顯優于電羅經。實驗結果表明，該算法能夠明顯提高衛星測姿、光纖羅經的姿態精度，達到的優勢互補的目的。

（Reference）

[1] 張科, 劉海鵬, 李恒年, 錢山. SINS/GPS/CNS組合導航聯邦濾波算法[J]. 中國慣性技術學報, 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] 屈薔, 劉建業, 熊智. 基于加性四元數誤差方程的慣性/天文姿態組合算法[J]. 中國慣性技術學報, 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] 戴東凱, 王省書, 戰德軍, 等. 基于單軸旋轉INS/GPS組合姿態誤差觀測的垂線偏差測量方法[J]. 中國慣性技術學報，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] 胡佩達, 高鐘毓, 張嶸, 韓豐田. 基于三軸搖擺臺的高精度姿態試驗系統[J]. 中國慣性技術學報, 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] 袁群哲, 周紅進. ADCP與光纖陀螺羅經組合導航[J].中國慣性技術學報, 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）；天津市海洋經濟創新發展區域示范項目（CXSF2014-3）

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

1005-6734(2015)06-0746-05

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