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

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

0 引 言

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

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

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

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

1 INS/GPS/CNS 聯(lián)邦濾波模型

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

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

2 基于 INS/GPS/CNS 的聯(lián)邦濾波算法

聯(lián)邦濾波算法基于卡爾曼濾波算法，狀態(tài)參數(shù)包含導航系統(tǒng)誤差（位置誤差、速度誤差和姿態(tài)誤差）和慣性器件誤差（陀螺誤差和加速度計誤差）。導航坐標系取當?shù)氐乩碜鴺讼?，XYZ軸分為指向地理坐標系的東北天方向。

2.1 系統(tǒng)狀態(tài)方程

根據(jù)文獻[9]，建立如下狀態(tài)方程：

1）速度誤差方程

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

其中：為當?shù)氐乩砭暥?；為車輛重心距地面高度；為地球半徑。

2）平臺失準角誤差方程

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

3）位置誤差方程

其中，分別為當?shù)亟浂?，緯度和高度誤差。

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

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

根據(jù)式（1）～式（7）可以建立卡爾曼濾波所需的狀態(tài)方程

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

2.2 系統(tǒng)量測方程

1）INS/GPS子濾波器1

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

2）INS/CNS子濾波器2

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

所以量測方程為

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

3）主濾波器

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

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

3 試驗結果與分析

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

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

4 結語

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

[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 組合導航聯(lián)邦濾波算法[J]. 中國慣性技術學報, 2013, 21(2): 226–230.

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

[8]劉準, 陳哲. INS/GPS/TERCOM組合制導系統(tǒng)中的信息融合方法研究[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] 嚴恭敏. 捷聯(lián)慣導系統(tǒng)動基座初始對準及其它相關問題研究[D]. 西安: 西北工業(yè)大學. 2008.