基于聯(lián)邦濾波器的SINSGNSDVS水下組合導(dǎo)航研究

1、基于聯(lián)邦濾波器的SINS/GNS/DVS水下組合導(dǎo)航研究劉明雍1，周志遠(yuǎn)1，趙 濤2(1. 西北工業(yè)大學(xué) 航海學(xué)院，西安 710072;)（2. 中國(guó)兵器工業(yè)集團(tuán)第二零二研究所，咸陽(yáng) 712099)摘 要：針對(duì)現(xiàn)有的水下導(dǎo)航系統(tǒng)不能全面滿足AUV導(dǎo)航的需要，為了提高水下航行器組合導(dǎo)航系統(tǒng)長(zhǎng)時(shí)間遠(yuǎn)航程的隱蔽性，可靠性及精度，提出了一種基于聯(lián)邦濾波器的慣性導(dǎo)航系統(tǒng)、地磁導(dǎo)航、多普勒速度聲納的組合導(dǎo)航方案，并采用簡(jiǎn)化自適應(yīng)卡爾曼算法對(duì)水下航行器組合導(dǎo)航系統(tǒng)進(jìn)行誤差估計(jì)，解決了傳統(tǒng)卡爾曼濾波容易發(fā)散的問(wèn)題。仿真結(jié)果表明，在SINS/GNS/DVS組合導(dǎo)航中采用該算法，提高了定位精度，保證了濾波的快速性

2、，驗(yàn)證了該算法的可行性和正確性。關(guān) 鍵 詞： 組合導(dǎo)航；自適應(yīng)卡爾曼濾波器；地磁導(dǎo)航中圖分類號(hào)：U666. 11 文獻(xiàn)標(biāo)志碼：A Research on SINS/DNS/DVS underwater integrated navigationbased on federal filterLiu Ming-yong1，Zhou Zhi-yuan1，Zhao Tao2（1. College of Marine, Northwest Polytechnical University, Shaanxi Xian , 710072)(2. NO.202 institute of China ordna

3、nce industry group corporation, Shaanxi Xiang-yang,712099)Abstract: Considering of the fact that the existing underwater navigation system can not completely s the needs of the AUV navigation. In order to enhance the long range and deep water ability, reliability and accuracy of the underwater vehic

4、le navigation system, in this paper, we propose a filter algorithm based on the Federation of inertial navigation system consisting of Doppler speed sonar, magnetic navigation method. Meanwhile, a simplified method is adopted for adaptive Kalman Underwater Vehicle navigation system error estimates t

5、o solve the divergence problem of traditional Kalman filter. The simulation results show that the proposed mehod can imporve the positioning accuracy. Moreover, the the speediness of the filter is guaranteed. In addition, the simulation results also demonstrate the effectiveness and correctness of t

6、he algorithm. Key words: Integrated navigation; Adaptive Kalman filter; Geomagnetic Navigation0引言當(dāng)前水下組合導(dǎo)航定位普遍采用SINS/GPS/DVL方案。由于GPS信號(hào)在水下很快衰減，故航行器需定期上爬到近水面,使GPS天線露出水面,來(lái)接收GPS的定位信息，對(duì)由陀螺羅經(jīng)和多普勒速度儀組成的自主導(dǎo)航系統(tǒng)進(jìn)行位置校準(zhǔn)。然而隨著水下航行器的應(yīng)用范圍、下潛深度不斷擴(kuò)展，特別是軍事領(lǐng)域內(nèi)對(duì)其隱蔽性的要求，常規(guī)組合方案越來(lái)越難以滿足需求。作為無(wú)源導(dǎo)航技術(shù)的地磁導(dǎo)航為實(shí)現(xiàn)這一目標(biāo)提供了新的技術(shù)途徑。國(guó)內(nèi)外的研究

7、中對(duì)地磁導(dǎo)航的研究有很多成果。文獻(xiàn)1將測(cè)量的地磁異常場(chǎng)強(qiáng)度序列與事先存儲(chǔ)的地磁異常圖實(shí)時(shí)進(jìn)行相關(guān)匹配，確定飛機(jī)在地磁異常圖上的經(jīng)度和緯度；文獻(xiàn)2中介紹了在對(duì)飛行彈體進(jìn)行彈道控制中, 利用地磁探測(cè)滾轉(zhuǎn)姿態(tài)來(lái)實(shí)時(shí)獲取彈體的對(duì)地方位, 為彈體控制提供方位依據(jù)。但是，地磁導(dǎo)航在水下導(dǎo)航方面的應(yīng)用，所做研究很少?？紤]上述問(wèn)題，本文提出了一種SINS/GNS（地磁）/DVS組合方案，該方法以地磁場(chǎng)模型解算地磁場(chǎng)強(qiáng)度的方式來(lái)得到精確位置信息,輔以精確計(jì)時(shí)進(jìn)而獲得速度信息；以地磁系統(tǒng)獲取的速度、位置信息與慣導(dǎo)系統(tǒng)輸出的速度、位置信息的差值作為量測(cè)值,經(jīng)過(guò)卡爾曼濾波,估計(jì)導(dǎo)航系統(tǒng)的誤差,作為一個(gè)子濾波器；并與另

8、一子濾波器SINS/DVL進(jìn)行信息融合，從而構(gòu)成聯(lián)邦濾波器?？紤]到水下環(huán)境的復(fù)雜多變使導(dǎo)航系統(tǒng)特別是地磁導(dǎo)航系統(tǒng)觀測(cè)噪聲的統(tǒng)計(jì)特性無(wú)法被準(zhǔn)確獲得，采用改進(jìn)的自適應(yīng)濾波算法代替標(biāo)準(zhǔn)卡爾曼濾波，并加以驗(yàn)證。仿真結(jié)果表明，在SINS/GNS/DVS組合導(dǎo)航中采用該算法，提高了定位精度，保證了濾波的快速性，驗(yàn)證了該算法的可行性和正確性。1 基于聯(lián)邦濾波的SINS/DNS/DVS組合導(dǎo)航方案設(shè)計(jì)本文采用的基于聯(lián)邦卡爾曼濾波的水下航行器SINS/GNS/DVS組合方法如圖1所示。該組合方案包括三個(gè)濾波器，即主濾波器、子濾波器1和子濾波器2。其中，子濾波器1負(fù)責(zé)SINS和多普勒速度儀的信息融合，其量測(cè)信息主

9、要是指多普勒和SINS形成的速度信息；子濾波器2負(fù)責(zé)SINS和地磁導(dǎo)航系統(tǒng)融合，其量測(cè)信息是地磁導(dǎo)航系統(tǒng)和SINS形成的位置、速度觀測(cè)信息。主濾波器一方面對(duì)各子濾波器進(jìn)行信息綜合與分配，另一方面將系統(tǒng)狀態(tài)誤差的估計(jì)值反饋給慣導(dǎo)系統(tǒng)，以校正其累積誤差。水下航行器采用該組合方案相比集中濾波器不僅具有實(shí)時(shí)性好的特點(diǎn)，而且通過(guò)信息分配系數(shù)的選取可以具有較高的容錯(cuò)能力，便于對(duì)各傳感器實(shí)施故障診斷和隔離。由子濾波器與主濾波器合成的全局估計(jì)值及其相應(yīng)的協(xié)方差陣被放大為后再反饋到子濾波器以重置子濾波器的估計(jì)值，即 （1）圖1 SINS/DNS/DVS聯(lián)邦濾波器結(jié)構(gòu)圖2 地磁導(dǎo)航子系統(tǒng)的設(shè)計(jì)當(dāng)前的地磁導(dǎo)航技術(shù)主

10、要是指地磁匹配定位,即將預(yù)先選定的區(qū)域地磁場(chǎng)某種地磁場(chǎng)特征值, 制成參考圖并儲(chǔ)存在水下航行器上的計(jì)算機(jī)中。 當(dāng)航行器通過(guò)這些地區(qū)時(shí), 地磁傳感器實(shí)時(shí)測(cè)量地磁場(chǎng)的有關(guān)特征值, 并構(gòu)成實(shí)時(shí)圖, 實(shí)時(shí)圖與預(yù)存的參考圖在計(jì)算機(jī)中進(jìn)行相關(guān)匹配, 確定實(shí)時(shí)圖在參考圖中的最相似點(diǎn)（匹配點(diǎn)）, 從而確定出航行器的精確實(shí)時(shí)位置。圖2為地磁導(dǎo)航與捷聯(lián)慣導(dǎo)的子系統(tǒng)示意圖。 圖2 地磁導(dǎo)航子系統(tǒng)設(shè)計(jì)原理圖本文中所設(shè)計(jì)的地磁導(dǎo)航的具體實(shí)現(xiàn)包括三個(gè)步驟：第一步，利用地磁傳感器處理后所測(cè)得磁偏角和磁傾角作為匹配參數(shù)進(jìn)行圖匹配,獲取粗位置信息；第二步，由地磁場(chǎng)模型解算地磁場(chǎng)強(qiáng)度,采用逐次迭代逼進(jìn)地磁場(chǎng)強(qiáng)度測(cè)量值的方式以獲取精

11、確的位置信息,同時(shí)對(duì)2個(gè)點(diǎn)之間進(jìn)行精確計(jì)時(shí),進(jìn)而解算出載體的即時(shí)速度并由此獲得速度信息；第三步，以地磁匹配系統(tǒng)獲取的速度、位置信息與慣導(dǎo)系統(tǒng)輸出的位置、速度信息的差值作為量測(cè)值,經(jīng)過(guò)卡爾曼濾波估計(jì)誤差，作為聯(lián)邦濾波器的一個(gè)子濾波器,再與慣導(dǎo)主濾器進(jìn)行數(shù)據(jù)融合。3組合導(dǎo)航自適應(yīng)卡爾曼算法 對(duì)于水下航行器組合導(dǎo)航系統(tǒng)，由于海況的復(fù)雜性，系統(tǒng)量測(cè)噪聲統(tǒng)計(jì)特性隨實(shí)際工作環(huán)境而改變，初始的先驗(yàn)值并不能代表實(shí)際工作時(shí)的噪聲情況。雖然經(jīng)過(guò)對(duì)捷聯(lián)式慣性導(dǎo)航系統(tǒng)和地磁匹配系統(tǒng)的大量反復(fù)試驗(yàn)，可以獲得試驗(yàn)時(shí)系統(tǒng)噪聲的統(tǒng)計(jì)特性，但是實(shí)際工作時(shí)量測(cè)噪聲的統(tǒng)計(jì)特性仍然是未知的。針對(duì)此問(wèn)題，采用簡(jiǎn)化的Sage-Husa自

12、適應(yīng)濾波方法，解決了傳統(tǒng)卡爾曼濾波容易發(fā)散的問(wèn)題。設(shè)系統(tǒng)的狀態(tài)模型和量測(cè)模型方程為 （2）和為互不相關(guān)的白噪聲序列，其方差和均值分別為，標(biāo)準(zhǔn)卡爾曼算法描述如下： （3） （4） （5） （6）（7） （8）Sage-Husa自適應(yīng)濾波是在標(biāo)準(zhǔn)卡爾曼濾波的基礎(chǔ)上，同時(shí)估計(jì)過(guò)程噪聲和量測(cè)噪聲的統(tǒng)計(jì)特性。事實(shí)上，組合導(dǎo)航系統(tǒng)中的系統(tǒng)噪聲一般具有穩(wěn)定性，所以本文僅對(duì)量測(cè)噪聲進(jìn)行估計(jì)。（9） （10）其中，取0.95，式（3）至式（10）即為簡(jiǎn)化Sage-Husa自適應(yīng)濾波算法。4 自適應(yīng)聯(lián)邦濾波模型建立41系統(tǒng)狀態(tài)方程導(dǎo)航坐標(biāo)系取當(dāng)?shù)氐牡乩碜鴺?biāo)系，其中X Y Z 分別指向東北天建立狀態(tài)方程如下 （11

13、）對(duì)于慣導(dǎo)系統(tǒng)，作為參考系統(tǒng)的慣導(dǎo)系統(tǒng),取姿態(tài)誤差角（）、速度誤差（）位置誤差（）陀螺常值漂移（）陀螺一階馬爾科夫過(guò)程（） 加速度計(jì)零位漂移（）作為系統(tǒng)的狀態(tài)變量,其中：和的推導(dǎo)過(guò)程及具體內(nèi)容略。對(duì)于多普勒子系統(tǒng)，取對(duì)于地磁子系統(tǒng)，取。INS/DVS子系統(tǒng)和INS/DNS子系統(tǒng)狀態(tài)方程分別為：42 SINS/DVS量測(cè)方程的建立 43 SINS/DNS量測(cè)方程的建立由于利用三軸捷聯(lián)磁場(chǎng)計(jì)測(cè)量地磁場(chǎng)獲得地磁場(chǎng)在載體坐標(biāo)系3 個(gè)坐標(biāo)軸方向的分量分別為以磁偏角和磁傾角匹配獲取的粗略位置的經(jīng)緯度作為循環(huán)計(jì)算的經(jīng)緯度初始值，以高度計(jì)測(cè)量的高度作為循環(huán)計(jì)算的高度初始值 ,逐次代入地磁場(chǎng)模型解算地磁場(chǎng)強(qiáng)度，

14、當(dāng)最小時(shí),最后一次代入的經(jīng)緯度和高度即為地磁導(dǎo)航系統(tǒng)輸出的位置，輔以2個(gè)點(diǎn)精確計(jì)時(shí)則可得到速度。 5 仿真 根據(jù)提出的自適應(yīng)聯(lián)邦濾波算法，對(duì)水下航行器組合導(dǎo)航進(jìn)行了仿真。初始仿真條件為:陀螺漂移為0. 1°/ h ,載體位置北緯34.5°,東經(jīng)105.3°高度10m ,初始東向速度為50m/s, 北向速度為50 m/s，天向速度為零。陀螺漂移白噪聲漂移均方根0. 1°/ h ,加速度計(jì)零偏0. 001 g ,隨機(jī)均方誤差為0. 001 g ,速度誤差為0. 1 m/。在進(jìn)行自適應(yīng)濾波時(shí)，主濾波器初始分配系數(shù)取0.5，子濾波器均取0.25。仿真時(shí)間300秒

15、。 圖 3 速度誤差曲線圖 圖 4 位置誤差曲線圖仿真結(jié)果表明，在300秒的仿真時(shí)間里，其中東向速度、北向速度誤差在初始階段抖動(dòng)是比較劇烈的，主要原因是卡爾曼濾波的初始P陣取的較大，濾波器不能保持無(wú)偏造成的。在40秒左右之后誤差增長(zhǎng)趨于穩(wěn)定，是因?yàn)槌跏贾礟對(duì)濾波的影響逐漸減弱，且自適應(yīng)算法對(duì)量測(cè)噪聲R的在線修正。緯度、經(jīng)度誤差則基本保持在導(dǎo)航精度內(nèi)，誤差保持在5米范圍之內(nèi)。 6結(jié)論：本文以AUV組合導(dǎo)航系統(tǒng)為應(yīng)用背景,針對(duì)水下導(dǎo)航對(duì)組合導(dǎo)航特殊要求, 引入地磁導(dǎo)航，設(shè)計(jì)了SINS/GNS/DVS組合導(dǎo)航系統(tǒng)。為了克服地磁匹配系統(tǒng)和慣導(dǎo)系統(tǒng)測(cè)量方差為時(shí)變的影響，采用自適應(yīng)卡爾曼聯(lián)邦濾波器進(jìn)行誤差

16、估計(jì)和數(shù)據(jù)融合，通過(guò)實(shí)時(shí)估計(jì)和修正觀測(cè)噪聲的統(tǒng)計(jì)特性達(dá)到降低模型誤差、抑制濾波發(fā)散的目的。仿真表明,改進(jìn)的自適應(yīng)算法能夠同時(shí)保證信息融合的精度和穩(wěn)定性,濾波性能得到了改善. 通過(guò)在MATLAB環(huán)境下仿真，本文所提出的方案得到了實(shí)現(xiàn)，也驗(yàn)證了改進(jìn)的自適應(yīng)信息融合算法的優(yōu)良特性和較高的實(shí)際應(yīng)用價(jià)值。目前地磁組合導(dǎo)航技術(shù)在導(dǎo)航、制導(dǎo)領(lǐng)域具有很大的潛力。它可以作為GPS的備份系統(tǒng), 在GPS 失效的情況下校正慣導(dǎo)的積累誤差,可以用于姿態(tài)方位的測(cè)量,以及遠(yuǎn)程車輛的監(jiān)控和無(wú)人機(jī)的導(dǎo)航。隨著地磁傳感器技術(shù)的提高,地磁測(cè)量精度和速度都會(huì)得到提高，地磁導(dǎo)航系統(tǒng)的應(yīng)用范圍還將進(jìn)一步被擴(kuò)展。參考文獻(xiàn)1Goldenb

17、erg F. Geomagnetic Navigation beyond Magnetic CompassC. PLANS 2006, San Diego, California. 684-694.2Zheng Yufeng, Edward A Essock, Bruce C Hansen. Advanced discrete wavelet transform fusion algorithm and its optimization by using the metric of image quality indexJ .Optical Engineering, 2005, 44 (3) : 1-12.3魯平,趙龍，陳哲.改進(jìn)的Sage-Husa 自適