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

杜紅松，曹永恒，郝 強(qiáng)，尹洪亮，楊 洺

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

0 引 言

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

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

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

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

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

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

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

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

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

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

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

1）速度誤差方程

式中： i 為慣性坐標(biāo)系；為地球坐標(biāo)系；為加速度計(jì)測(cè)得的比力在導(dǎo)航系中的投影；為東、北、天向的速度誤差；北、天向的平臺(tái)失準(zhǔn)角；為加速度計(jì)零偏在導(dǎo)航坐標(biāo)系中的投影；為牽連角速度；為地球自轉(zhuǎn)角速度。有

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

2）平臺(tái)失準(zhǔn)角誤差方程

其中，表示陀螺常值漂移在導(dǎo)航坐標(biāo)系中的投影，

3）位置誤差方程

其中，分別為當(dāng)?shù)亟?jīng)度，緯度和高度誤差。

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

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

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

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

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

1）INS/GPS子濾波器1

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

2）INS/CNS子濾波器2

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

所以量測(cè)方程為

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

3）主濾波器

在無CNS信息時(shí)，主濾波器采用子濾波器1的狀態(tài)更新結(jié)果，在有CNS信息時(shí)，主濾波器采用兩子濾波器的的濾波結(jié)果對(duì)INS信息進(jìn)行更新，構(gòu)建量測(cè)方程如下：

其中：為量測(cè)矩陣為量測(cè)噪聲。

3 試驗(yàn)結(jié)果與分析

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

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

4 結(jié)語(yǔ)

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

[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]王常虹, 劉睿, 李葆華. 地磁天文自主導(dǎo)航算法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2010, 18(4): 429–433.

[6]張科, 劉海鵬, 李恒年, 等. SINS/GPS/CNS 組合導(dǎo)航聯(lián)邦濾波算法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2013, 21(2): 226–230.

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

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