自適應(yīng)聯(lián)邦濾波器在SINS

1、自適應(yīng)聯(lián)邦濾波器在SINS/DNS/DVS組合導(dǎo)航中的新應(yīng)用劉明雍，周志遠(yuǎn)，趙卓(西北工業(yè)大學(xué) 航海學(xué)院，西安 710072)摘要：為了提高水下航行器組合導(dǎo)航系統(tǒng)長(zhǎng)時(shí)間遠(yuǎn)航程的隱蔽性，可靠性及精度，采用了一種基于聯(lián)邦濾波器的慣性導(dǎo)航系統(tǒng)、地磁導(dǎo)航、多普勒速度聲納的組合導(dǎo)航方案，并利用自適應(yīng)卡爾曼濾波對(duì)水下航行器組合導(dǎo)航系統(tǒng)進(jìn)行誤差估計(jì),通過(guò)實(shí)時(shí)估計(jì)和修正系統(tǒng)噪聲以及觀測(cè)噪聲的統(tǒng)計(jì)特性達(dá)到降低模型誤差、抑制濾波發(fā)散的目的。在Matlab 環(huán)境下的仿真證實(shí)了該方案可以達(dá)到較高精度。關(guān) 鍵 詞 組合導(dǎo)航；自適應(yīng)卡爾曼濾波器；地磁導(dǎo)航中圖分類號(hào)：TP273 文獻(xiàn)標(biāo)識(shí)碼：A0 引言當(dāng)前在水下航行器組合導(dǎo)

2、航定位方面較為普遍采用的是SINS/DVS/GPS方案,由于GPS信號(hào)在水下很快衰減，所以航行器在水下執(zhí)行任務(wù)時(shí)無(wú)法利用其信息，故航行器需定期上爬到近水面,通過(guò)控制GPS天線伸縮裝置使GPS天線露出水面,并使GPS天線與水面保持一定距離,利用GPS接收機(jī)的定位信息對(duì)由陀螺羅經(jīng)和多普勒速度儀組成的自主導(dǎo)航系統(tǒng)進(jìn)行位置校準(zhǔn)。然而隨著當(dāng)前水下航行器的應(yīng)用范圍、航行距離、下潛深度不斷擴(kuò)展，尤其在是軍事領(lǐng)域內(nèi)對(duì)其隱蔽性的要求，上述組合方案越來(lái)越難以滿足需求。地磁導(dǎo)航的出現(xiàn)為實(shí)現(xiàn)這一目標(biāo)提供了新的技術(shù)途徑。20世紀(jì)80年代初瑞典的Lund學(xué)院對(duì)船只的地磁導(dǎo)航進(jìn)行了實(shí)驗(yàn)驗(yàn)證1，實(shí)驗(yàn)中將地磁強(qiáng)度的測(cè)量數(shù)據(jù)與地

3、磁圖進(jìn)行人工比對(duì)，確定船只的位置；同時(shí)根據(jù)距離已知的兩個(gè)磁傳感器的輸出時(shí)差，確定船只的地速。2006年，F(xiàn)Goldenberg針對(duì)飛機(jī)的地磁導(dǎo)航系統(tǒng)進(jìn)行了研究2，將測(cè)量的地磁異常場(chǎng)強(qiáng)度序列與事先存儲(chǔ)的地磁異常圖實(shí)時(shí)進(jìn)行相關(guān)匹配，確定飛機(jī)在地磁異常圖上的經(jīng)度和緯度。國(guó)內(nèi), 近期有很多研究機(jī)構(gòu)包括高校掀起了一股地磁導(dǎo)航研究的熱潮。在文獻(xiàn)3中介紹了在對(duì)飛行彈體進(jìn)行彈道控制中, 利用地磁探測(cè)滾轉(zhuǎn)姿態(tài)來(lái)實(shí)時(shí)獲取彈體的對(duì)地方位, 為彈體控制提供方位依據(jù)。本文提出一種SINS/DVS/DNS（地磁）組合方案，以地磁場(chǎng)模型解算地磁場(chǎng)強(qiáng)度的方式來(lái)得到精確位置信息,輔以精確計(jì)時(shí)進(jìn)而獲得速度信息。以地磁系統(tǒng)獲取的速

4、度、位置信息與慣導(dǎo)系統(tǒng)輸出的速度、位置信息的差值作為量測(cè)值,經(jīng)過(guò)卡爾曼濾波,估計(jì)導(dǎo)航系統(tǒng)的誤差,作為聯(lián)邦濾波器的一個(gè)子系統(tǒng)；同樣將多普勒速度儀與慣導(dǎo)相結(jié)合構(gòu)成聯(lián)邦濾波器的另一個(gè)子系統(tǒng)。通過(guò)信息融合技術(shù)可以達(dá)到系統(tǒng)的最優(yōu)綜合。1 INS/DNS/DVS組合導(dǎo)航方案以慣導(dǎo)系統(tǒng)作為參考系統(tǒng)，選取聯(lián)邦濾波器的的兩個(gè)子系統(tǒng)分別為DNS和DVS進(jìn)行方案設(shè)計(jì)和聯(lián)邦濾波。 1. 1地磁導(dǎo)航子系統(tǒng)的設(shè)計(jì)當(dāng)前的地磁導(dǎo)航技術(shù)主要是指地磁匹配定位,即將預(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í)

5、時(shí)圖與預(yù)存的參考圖在計(jì)算機(jī)中進(jìn)行相關(guān)匹配, 確定實(shí)時(shí)圖在參考圖中的最相似點(diǎn)（匹配點(diǎn)）, 從而確定出航行器的精確實(shí)時(shí)位置。本文中地磁導(dǎo)航的具體實(shí)現(xiàn)包括三個(gè)步驟：第一步，利用地磁傳感器處理后所測(cè)得磁偏角和磁傾角作為匹配參數(shù)進(jìn)行圖匹配,獲取粗位置信息；第二步，由地磁場(chǎng)模型解算地磁場(chǎng)強(qiáng)度,采用逐次迭代逼進(jìn)地磁場(chǎng)強(qiáng)度測(cè)量值的方式以獲取精確的位置信息,同時(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è)子濾波器。12組合導(dǎo)航系統(tǒng)聯(lián)邦濾波器設(shè)計(jì)聯(lián)邦濾波

6、器如圖1所示，圖中公共參考系統(tǒng)為慣導(dǎo)系統(tǒng)，它的輸出一方面直接給主濾波器，另一方面它可以輸給各子濾波器作為量測(cè)值。各子系統(tǒng)的輸出只給相應(yīng)的子濾波器。各子濾波器的局部估計(jì)及其協(xié)方差陣送入主濾波器和主濾波器的估計(jì)值一起進(jìn)行融合以得到全局最優(yōu)估計(jì)。由子濾波器與主濾波器合成的全局估計(jì)值及其相應(yīng)的協(xié)方差陣被放大為后再反饋到子濾波器以重置子濾波器的估計(jì)值，即 （1） 13系統(tǒng)狀態(tài)方程和量測(cè)方程的建立131系統(tǒng)狀態(tài)方程 (11)其中，為系統(tǒng)噪聲方差陣的修正系數(shù)。由于量側(cè)野值主要影響到量測(cè)噪聲，所以對(duì)量測(cè)噪聲方差陣的調(diào)整精度顯得非常重要?？紤]到每圖1 聯(lián)邦濾波器的結(jié)構(gòu)導(dǎo)航坐標(biāo)系取當(dāng)?shù)氐牡乩碜鴺?biāo)系，其中X Y Z

7、 分別指向東北天建立狀態(tài)方程如下 （2） 對(duì)于慣導(dǎo)系統(tǒng)，作為參考系統(tǒng)的慣導(dǎo)系統(tǒng),取姿態(tài)誤差角（）、速度誤差（）位置誤差（）陀螺常值漂移（）陀螺一階馬爾科夫過(guò)程（） 加速度計(jì)零位漂移（）作為系統(tǒng)的狀態(tài)變量,其中：3 BP神經(jīng)網(wǎng)絡(luò)對(duì)加權(quán)系數(shù)的調(diào)節(jié)算法3.1 BP神經(jīng)網(wǎng)絡(luò)輸入信號(hào)的定義根據(jù)卡爾曼濾波器中的新息的統(tǒng)計(jì)特性可以判斷濾波器性能的高低。假定k表示一段時(shí)間內(nèi)的統(tǒng)計(jì)數(shù)，則時(shí)刻新息的估計(jì)方差定義為5： (14)定義新息估計(jì)方差的變化率為： (15)其中為采樣頻率，T為計(jì)算某時(shí)刻新息估計(jì)方差的變化率時(shí)所使用的采樣點(diǎn)的個(gè)數(shù)，一般T取10左右的整數(shù)。將(14)式可以寫成遞推形式： (16)為了保持濾波始

8、終最優(yōu)，如何根據(jù)新息統(tǒng)計(jì)特性的變化來(lái)定量的衡量野值對(duì)濾波器的影響程度，以達(dá)到適當(dāng)調(diào)節(jié)和的目的是本文研究的一個(gè)難點(diǎn)。考慮到：1）若量測(cè)中無(wú)野值，則為零。2）若時(shí)刻出現(xiàn)了方差恒為的連續(xù)野值，則當(dāng)時(shí)，；當(dāng)時(shí)，幾乎不變。3）野值的方差越大，越大。因此，的大小與野值方差的大小存在某種關(guān)系，本文利用神經(jīng)網(wǎng)絡(luò)來(lái)建立與野值方差的映射關(guān)系，以達(dá)到正確修正濾波器的目的，即選取作為網(wǎng)絡(luò)的輸入信號(hào)，網(wǎng)絡(luò)輸出和調(diào)節(jié)權(quán)值系數(shù)。3.2 BP神經(jīng)網(wǎng)絡(luò)的結(jié)構(gòu)及訓(xùn)練方法設(shè)計(jì)考慮到新息是多維的，若集中用一個(gè)BP網(wǎng)絡(luò)來(lái)處理，使得網(wǎng)絡(luò)的學(xué)習(xí)訓(xùn)練難度增大，本文采用對(duì)每一個(gè)新息分量分散處理。由于新息分量中噪聲的統(tǒng)計(jì)特性可能不一樣，所以不同

9、的分量對(duì)應(yīng)的網(wǎng)絡(luò)的訓(xùn)練樣本也可能不一樣。若量測(cè)分量中噪聲不同，則需單獨(dú)對(duì)每一個(gè)分量上的神經(jīng)網(wǎng)絡(luò)進(jìn)行訓(xùn)練；若量測(cè)分量中噪聲相同，則所有的分量可以共用一個(gè)神經(jīng)網(wǎng)絡(luò)，任取一個(gè)具有代表性的分量對(duì)神經(jīng)網(wǎng)絡(luò)進(jìn)行訓(xùn)練即可。但是，不管哪一種情況，BP神經(jīng)網(wǎng)絡(luò)的結(jié)構(gòu)是相同的，如圖2所示。圖2 BP神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)圖在圖2的三層網(wǎng)絡(luò)中，輸入層神經(jīng)元個(gè)數(shù)為1，輸入向量為；隱層的神經(jīng)元個(gè)數(shù)n一般由訓(xùn)練的樣本容量和訓(xùn)練的精度決定，隱層輸出向量為；輸入層到隱層之間的權(quán)值矩陣為，閾值為，隱層到輸出層之間的權(quán)值矩陣為，閾值為，神經(jīng)網(wǎng)絡(luò)的輸出為系統(tǒng)噪聲方差的修正系數(shù)和量測(cè)噪聲方差的修正系數(shù)。本文所提出的濾波算法的精度由神經(jīng)網(wǎng)絡(luò)對(duì)新

10、息序列的監(jiān)測(cè)精度所決定，而神經(jīng)網(wǎng)絡(luò)對(duì)新息的監(jiān)測(cè)精度又取決于BP神經(jīng)網(wǎng)絡(luò)的訓(xùn)練程度。所以對(duì)神經(jīng)網(wǎng)絡(luò)的訓(xùn)練是本文的一個(gè)重點(diǎn)和難點(diǎn)，可以用如下方法獲取神經(jīng)網(wǎng)絡(luò)的訓(xùn)練樣本：1）分析被野值污染的量測(cè)新息的統(tǒng)計(jì)特性，確定新息估計(jì)方差及其變化率的上界和下界。用實(shí)際采樣的量測(cè)值（含有未知野值噪聲）進(jìn)行仿真，在濾波的過(guò)程中利用(15)(16)式計(jì)算出新息的估計(jì)方差和估計(jì)方差的變化率。并確定它們的上界和下界。分別設(shè)為、和、。2）從量測(cè)新息的估計(jì)方差和變化率中獲取訓(xùn)練樣本。首先模擬出一個(gè)方差為的隨機(jī)噪聲作為野值加入到事先未被污染得量測(cè)序列中，并用標(biāo)準(zhǔn)Kalman濾波進(jìn)行仿真，根據(jù)上面的分析結(jié)果知，此時(shí)存在一個(gè)幾乎不

11、變的與對(duì)應(yīng)，所以這個(gè)對(duì)應(yīng)的網(wǎng)絡(luò)的期望輸出就為，這樣便得到一組樣本值，使在區(qū)域里取值，并重復(fù)上面的操作，便可得到足夠的神經(jīng)網(wǎng)絡(luò)的訓(xùn)練樣本。4 仿真分析以在二維平面上做勻速直線運(yùn)動(dòng)的運(yùn)載體為例，對(duì)其位置進(jìn)行觀測(cè)。其運(yùn)動(dòng)模型為： 觀測(cè)向量為，分別為二維平面上的坐標(biāo)。狀態(tài)向量為：，分別為沿軸向的速度分量。離散化后的系統(tǒng)矩陣，觀測(cè)矩陣，其中為采樣時(shí)間，為零均值的高斯白噪聲，假設(shè)。仿真2分鐘，采樣周期，在40s到60s之間加入方差為80的正態(tài)噪聲野值。由于仿真模型中沒有系統(tǒng)噪聲，所以圖2的神經(jīng)網(wǎng)絡(luò)的輸出可以只取一個(gè)，即量測(cè)噪聲方差的修正系數(shù)，選擇隱層神經(jīng)元個(gè)數(shù)為12，訓(xùn)練誤差SSE=0.02時(shí)的網(wǎng)絡(luò)權(quán)值陣

12、和閾值如下：圖3和圖4分別是野值存在下的標(biāo)準(zhǔn)kalman濾波算法和本文提出的算法比較。從圖中可以看出，當(dāng)在40s加入野值時(shí)，標(biāo)準(zhǔn)kalman濾波器的誤差顯然越來(lái)越大，而本文提出的修正后的kalman濾波器能夠有效地抑制成片野值對(duì)濾波效果的影響，且濾波器的穩(wěn)定性和精度也有所提高。圖3 x方向位置誤差圖4 y方向位置誤差5 結(jié)論通過(guò)對(duì)kalman濾波過(guò)程中的新息序列統(tǒng)計(jì)特性的分析，給出了一種利用BP神經(jīng)網(wǎng)絡(luò)修正的卡爾曼濾波算法。當(dāng)野值出現(xiàn)時(shí)，模型中系統(tǒng)噪聲和量測(cè)噪聲的方差陣已經(jīng)發(fā)生了變化，如果還用原來(lái)的方差陣參與運(yùn)算，這便是導(dǎo)致卡爾曼濾波器濾波效果變差甚至發(fā)散的主要原因。新息估計(jì)方差的變化率一方面

13、反映了與野值方差的大小關(guān)系，另一方面也反映了野值消失的時(shí)刻，所以，本文利用BP網(wǎng)絡(luò)對(duì)新息估計(jì)方差的變化率進(jìn)行實(shí)時(shí)監(jiān)測(cè)，并根據(jù)其變化規(guī)律，計(jì)算出一組加權(quán)系數(shù)對(duì)噪聲的方差陣做實(shí)時(shí)修正，從而有效的抑制了成片野值對(duì)濾波器的影響，提高了濾波器的精度和穩(wěn)定性，同時(shí)對(duì)單個(gè)離散野值也有良好的濾波效果，而且工程上實(shí)現(xiàn)方便，適用面較廣。參考文獻(xiàn)：1 Jo-Anne Ting, D'Souza, A.; Schaal, S. Automatic Outlier Detection: A Bayesian ApproachC.Robotics and Automation, 2007 IEEE Interna

14、tional Conference on 10-14 April 2007 Page(s):2489 2494.2 XIE L, SOH C, SOUZA C E. Robust Kalman filtering for uncertain discrete-time systems J. IEEE Trans on Auto Control, 1994, 39 (6) : 13101314.3 盧迪，姚郁等. 一種抗野值的Kalman濾波器J. 系統(tǒng)仿真學(xué)報(bào)，2004，16（5）102710294 柏菁，劉建業(yè)等. 模糊自適應(yīng)卡爾曼濾波技術(shù)研究J.信息與控制學(xué)報(bào)，2002，31（3）1931

15、975 J Z Sasiadek，Q Wang，M B Zeremba. Fuzzy Adaptive Kalman Filtering For INS/GPS Data Fusion. Proceedings of the 15th IEEE International Symposium on IntelligentC. Control，Rio，Patras，GREECE，July，2000，1719，181186.BP Neural Network Adaptive Kalman Filtering When Existing OutliersAbstract: This paper p

16、roposes a novel anti-outlier method based on BP neural network adaptive Kalman filtering to reduce the influence of outliers. Meanwhile, the affection of outliers for Kalman filter is analyzed quantificationally. The change rate of innovation sequence estimation covariance is monitored and computed

17、in real time by BP neural network. A group of weighted coefficient is used to amend the system noise and measurement noise on line. In the full paper, we explain in much detail the BP neural network adaptive algorithm proposed by us; here we just list the topics discussed in our detailed explanation

18、: (1) analyze the influence of outlier for Kalman filter; (2) the modification algorithm for standard Kalman filter;(3) the adjusting algorithm of weighted coefficient matrix based on BP neural network, moreover, the framework and training method of BP neural network are also designed. Finally, we give a numerical simulation