一種基于INS/GPS/CNS的聯邦濾波算法

杜紅松，曹永恒，郝 強，尹洪亮，楊 洺

(1. 海軍研究院，北京 100073；2. 中國船舶及海洋工程設計研究院，上海 200011；3. 哈爾濱工業大學 電氣工程及自動化學院，黑龍江 哈爾濱 150001；4. 中國艦船研究院，北京 100192)

0 引 言

艦艇、飛機、導彈等平臺常采用慣性導航系統（Inertal Navigation System，INS）與 GPS（Global Positioning System）組合的方式進行導航定位，該方法可以提供精度相對較高的速度、位置信息，但對于某些對姿態信息要求較高的情況，需尋求新的解決方案[1–3]。

天文導航系統（Celestial Navigation System，CNS）通過天體敏感器進行天體方位信息測量并經過解算輸出載體的姿態信息，具有測量精度高、誤差不隨時間積累、可靠性高的特點，是一種較常用的姿態測定方法，但該方法受環境因素影響較大，輸出導航信息不連續[4–5]。

為改善INS/GPS的姿態精度低、誤差隨時間發散的問題，同時發揮CNS的優勢，將INS/GPS與CNS進行組合是目前的研究熱點，張科和吳坤峰分別設計了2類INS/GPS/CNS組合濾波算法，并進行仿真研究，但由于條件所限，2 人未進行實物試驗[6–7]。

本文提出一種基于INS/GPS/CNS的聯邦濾波算法，根據聯邦濾波的特點，通過設計INS/CNS子濾波器、INS/GPS子濾波器和INS/GPS/CNS主濾波器，在有天文信息的情況下，進行3組合解算，在無天文的情況下，進行2組合解算，提高算法的穩定性和精度，并通過研制的INS/GPS/CNS組合測姿系統進行試驗驗證，驗證了INS/GPS/CNS組合算法姿態精度和穩定收斂性，使系統姿態精度得到了很大的提高。

1 INS/GPS/CNS 聯邦濾波模型

聯邦濾波器是Carlson提出[8]，利用信息分配原則消除各子狀態估計的相關聯性，設計簡單、容錯性強，利用較小的計算量就可以獲得全局最優或次優估計。根據信息分配策略的不同，聯邦濾波器一般分為無復位方式和有復位方式2種。其中，無復位方式各子濾波器獨立濾波，主濾波器只進行子濾波器濾波結果的簡單融合，具有容錯能力強的特點，但該方案精度取決于導航設備精度，要獲得較高的姿態精度，對INS、GPS和CNS精度要求較高，方案結構如圖1所示。有復位方式相對于無復位方式增加了主濾波器濾波結果對子濾波器的反饋校正，所以該方案精度較高，但可能會引起各濾波器之間的交叉污染，容錯性能較低，方案結構如圖2所示。

由于系統采用的導航設備精度很高，所以，本文算法采用無復位方式，以INS作為公共參考系統，GPS和CNS作為輔助系統進行方案設計。

2 基于 INS/GPS/CNS 的聯邦濾波算法

聯邦濾波算法基于卡爾曼濾波算法，狀態參數包含導航系統誤差（位置誤差、速度誤差和姿態誤差）和慣性器件誤差（陀螺誤差和加速度計誤差）。導航坐標系取當地地理坐標系，XYZ軸分為指向地理坐標系的東北天方向。

2.1 系統狀態方程

根據文獻[9]，建立如下狀態方程：

1）速度誤差方程

式中： i 為慣性坐標系；為地球坐標系；為加速度計測得的比力在導航系中的投影；為東、北、天向的速度誤差；北、天向的平臺失準角；為加速度計零偏在導航坐標系中的投影；為牽連角速度；為地球自轉角速度。有

其中：為當地地理緯度；為車輛重心距地面高度；為地球半徑。

2）平臺失準角誤差方程

其中，表示陀螺常值漂移在導航坐標系中的投影，

3）位置誤差方程

其中，分別為當地經度，緯度和高度誤差。

綜合以上分析，考慮高度通道的影響，擴充器件誤差參數為狀態變量，得到15維狀態變量如下：

其中，系三軸陀螺漂移；系加速度計零偏。

根據式（1）～式（7）可以建立卡爾曼濾波所需的狀態方程

其中：為 狀態矩陣；為 系統噪聲矩陣；為系統噪聲，具體表達式可參見文獻[9]。

2.2 系統量測方程

1）INS/GPS子濾波器1

取INS和GPS輸出的位置和速度信息之差作為觀測量，定義量測方程為：

2）INS/CNS子濾波器2

對于姿態組合子濾波器，由于INS輸出為機體系下姿態（用表示），CNS為慣性系下姿態（用表示），所以在進行濾波前，需將兩坐標系進行轉換，將姿態信息轉化到導航系下：其中：為機體系（b系）轉化到導航系（n系）的姿態轉移矩陣；為慣性系（i系）轉化到導航系（n系）的姿態轉移矩陣。

所以量測方程為

其中：為量測矩陣C為姿態角誤差與平臺誤差角之間的轉移矩陣，具體表達形式可參考文獻[10]；為量測噪聲。

3）主濾波器

在無CNS信息時，主濾波器采用子濾波器1的狀態更新結果，在有CNS信息時，主濾波器采用兩子濾波器的的濾波結果對INS信息進行更新，構建量測方程如下：

其中：為量測矩陣為量測噪聲。

3 試驗結果與分析

為驗證本文所提算法的有效性，在實驗室條件下，利用INS/GPS/CNS三組合試驗裝置進行試驗，試驗裝置由光纖慣導、GPS接收機和天文相機組成。本文采用靜態試驗的方法進行系統及算法精度驗證，同時采用INS/GPS兩組合算法的平臺誤差角結果作為對照，試驗時間5 000 s，試驗結果如圖3和圖4所示。

從試驗曲線可以看出：本文所提基于INS/GPS/CNS的聯邦濾波算法平臺誤差角誤差達到1′以內；相較于傳統的INS/GPS兩組合算法，平臺誤差角精度整體提高了一個數量級，同時算法收斂，未出現兩組合算法平臺誤差角隨時間發散的現象；由于CNS系統依賴受環境因素影響較大，而本試驗是在無天文信號期間試驗裝置開機，待天文信號出現后記錄數據，所以在圖3的前1 000 s出現曲線收斂現象，而且收斂速度較快，證明CNS對姿態信息的校正能力很強，同時，試驗期間存在間斷無天文信號的現象，在此期間濾波器切換為子濾波器1，而曲線未出現發散和誤差變大的現象，證明本算法在無天文期間也可以對INS/GPS兩組合算法進行很好的調節。通過上述分析，證明了基于INS/GPS/CNS的聯邦濾波算法的可行性、高精度性和可靠性。

4 結語

本文利用INS，GPS，CNS的特點，采用聯邦濾波的思想提出了一種新的濾波器，實現了導航系統的高度融合及輸出導航信息的完整性；并設計了基于INS/GPS/CNS的聯邦濾波算法，該算法具有狀態切換靈活、計算量小、容錯性高的特點；實驗室條件下，利用INS/GPS/CNS三組合試驗裝置進行了靜態試驗，試驗結果表明，該算法平臺誤差角可達1′以內，遠遠高出傳統的INS/GPS兩組合算法精度，同時算法收斂性好、狀態切換快、可靠性強。因此，該算法具有重要的工程應用意義。

[1]GUL F, FANG J, GAHO A A. GPS/SINS navigation data fusion using quaternion model and unscented Kalman filter[C]//2006 IEEE International Conference on Mechatronics and Automation Control. 2006: 1854–1859.

[2]FENWICK A J. Algorithms for position fixing using pulse arrival time[J]. Radar Sonar Navigation, 1999, 146(4).

[3]JWO D J, HSIEH M Y, LAI S Y. GPS navigation processing using the quaternion-based divided difference filter [J]. GPS Solutions, 2010, 14(3): 217–228.

[4]WU Xiao-juan, WANG Xin-long. A SINS/CNS deep integrated navigation method based on mathematical horizon reference[J]. Aircraft Engineering and Aerospace Technology,2011, 83(1): 26–34.

[5]王常虹, 劉睿, 李葆華. 地磁天文自主導航算法[J]. 中國慣性技術學報, 2010, 18(4): 429–433.

[6]張科, 劉海鵬, 李恒年, 等. SINS/GPS/CNS 組合導航聯邦濾波算法[J]. 中國慣性技術學報, 2013, 21(2): 226–230.

[7]吳坤峰, 楊峰, 梁彥, 等. INS/GPS/CNS 組合導航系統仿真[J]. 火力與指揮控制, 2010, 35(7): 13–19.

[8]劉準, 陳哲. INS/GPS/TERCOM組合制導系統中的信息融合方法研究[J]. 宇航學報, 2001, 22(3): 26–31.

[9]ZHANG Tong, CHEN Kang, FU Wenxing. Optimal twoiteration sculling compensation mathematical framework for SINS velocity updating [J]. Journal of Systems Engineering and Electronics, 2014, 25(6): 1065–1071.

[10] 嚴恭敏. 捷聯慣導系統動基座初始對準及其它相關問題研究[D]. 西安: 西北工業大學. 2008.