基于卡爾曼濾波的數(shù)據(jù)融合算法與應(yīng)用研究

徐恩松，陸文華，劉云飛，李寶磊，李 洋

(上海工程技術(shù)大學(xué)，上海 201620)

1 概 述

1.1 數(shù)據(jù)融合的概念

多傳感器數(shù)據(jù)融合主要是針對一個系統(tǒng)使用多種傳感器的問題而研究出來的一種信息處理方法。國外研究成果表明，信息融合的概念為：利用計算機(jī)技術(shù)對按時序獲得的若干傳感器的觀測信息在一定準(zhǔn)則下加以自動分析、綜合以完成所需的決策和估計任務(wù)而進(jìn)行的信息處理過程[1]。

信息融合處理過程的硬件基礎(chǔ)是傳感器系統(tǒng)，處理對象是多源信息，融合的核心是協(xié)調(diào)優(yōu)化和綜合處理。融合后，融合的信息效果遠(yuǎn)好于單個傳感器的信息。

信息融合的功能特點(diǎn)有:改進(jìn)多源信息的可信度和目標(biāo)的可探測性，降低推算模糊程度，提升探測性能，增強(qiáng)系統(tǒng)的自適應(yīng)性容錯調(diào)整能力，減少了信息獲取的時間，從而提高整個系統(tǒng)的信息處理性能。

1.2 信息融合理論算法

信息融合中要用到許多數(shù)學(xué)工具和方法，主要有概率論、模糊理論、小波法、神經(jīng)網(wǎng)絡(luò)等方法。信息融合作為一種智能化超高的數(shù)據(jù)綜合處理技術(shù)，集合并應(yīng)用了許多學(xué)科和新技術(shù)。數(shù)據(jù)融合廣義上的范圍包括檢測技術(shù)、信號處理與通信技術(shù)、模式識別、決策論、估計理論、最優(yōu)化理論、計算機(jī)科學(xué)、人工智能和神經(jīng)網(wǎng)絡(luò)等諸多學(xué)科[2-4]。近幾年信息融合技術(shù)的發(fā)展很快，很多應(yīng)用研究領(lǐng)域的人員已經(jīng)根據(jù)各自的具體應(yīng)用背景，提出了許多比較成熟的融合算法。在自主避障系統(tǒng)中的信息融合方法有以下幾種：

Blackman S S等提出了加權(quán)平均信息融合方法[5]；Luo R C等研究了貝葉斯估計信息融合方法[6]；Kamberova G提出統(tǒng)計決策理論信息融合方法[7]；Xu L等人采用了DempsterShafer證據(jù)理論[8]；MOORE J B等研究了卡爾曼濾波信息融合方法[9]；SASIADEK J Z等人提出了神經(jīng)網(wǎng)絡(luò)—模糊信息融合方法[10]。

文中主要研究基于卡爾曼濾波的信息融合算法，針對傳統(tǒng)集中卡爾濾波方式存在的問題，著重研究改進(jìn)的Sage-Husa濾波算法，該算法可增加濾波的精度和穩(wěn)定性能。并提出了基于改進(jìn)的Sage-Husa濾波算法的聯(lián)邦卡爾曼濾波器的設(shè)計，最后通過對特定的自主避障系統(tǒng)進(jìn)行了多傳感器信息的數(shù)據(jù)融合動態(tài)仿真，并給出了整個濾波融合的仿真結(jié)果。

2 卡爾曼濾波基本原理

設(shè)動態(tài)系統(tǒng)的方程和觀測方程分別為：

xk=Ak-1,kxk-1+Bk-1,kuk-1+wk-1

(1)

zk=Hkxk+vk

(2)

卡爾曼濾波算法的具體流程為：

預(yù)估計：

(3)

計算預(yù)估計協(xié)方差矩陣：

(4)

計算卡爾曼增益矩陣：

(5)

更新估計：

計算更新后估計協(xié)方差矩陣：

(6)

遞歸循環(huán)計算：

(7)

卡爾曼濾波的實(shí)質(zhì)是通過測量值重構(gòu)系統(tǒng)的狀態(tài)向量。它以“預(yù)測—實(shí)測—修正”的邏輯順序遞推[12]，得到系統(tǒng)的測量值以此來消除隨機(jī)干擾，再重構(gòu)出系統(tǒng)的狀態(tài)。KF計算分為時間更新過程和測量更新兩個過程，如下所示：

(1)時間更新過程(預(yù)測)。

(a)向前推算狀態(tài)變量：

(b)向前推算誤差協(xié)方差：

(2)測量更新過程(修正)。

(a)計算卡爾曼增益：

(b)由觀測變量更新Zk估計：

(c)更新誤差協(xié)方差：

3 自主避障系統(tǒng)數(shù)據(jù)融合解決方案

對于自主避障中的組合導(dǎo)航系統(tǒng)的數(shù)據(jù)融合，通常采用的方法是分散卡爾曼濾波和集中化卡爾曼濾波。組合導(dǎo)航系統(tǒng)中所用到的參數(shù)多，采取單獨(dú)一個濾波器進(jìn)行全局?jǐn)?shù)據(jù)集中處理不能保證濾波的實(shí)時性，如果組合系統(tǒng)中的慣導(dǎo)系統(tǒng)或GPS系統(tǒng)出現(xiàn)故障，整個系統(tǒng)的輸出都會受到影響，可看出集中濾波的容錯性能差，不利于系統(tǒng)的故障診斷。相對來說，分散化濾波可以提高系統(tǒng)的容錯性能、減少計算量。它通常采用由若干個局部濾波器和一個主濾波器組成的兩級濾波器結(jié)構(gòu)，各子狀態(tài)估計的相關(guān)性利用信息原則來消除，其結(jié)構(gòu)設(shè)計靈活，各子系統(tǒng)的數(shù)據(jù)用局部濾波器處理，然后整個系統(tǒng)全局?jǐn)?shù)據(jù)的融合處理通過主濾波器進(jìn)行。本節(jié)通過利用Calson提出的聯(lián)邦濾波器[13](federated filter)，對自主避障系統(tǒng)進(jìn)行分散化濾波數(shù)據(jù)融合進(jìn)行研究。

3.1 聯(lián)邦濾波器的基本原理

聯(lián)邦濾波器是一種由多個子濾波器和一個主濾波器組成的數(shù)據(jù)融合技術(shù)。通常把慣導(dǎo)系統(tǒng)作為組合導(dǎo)航系統(tǒng)中公共參考的傳感器。一方面慣導(dǎo)系統(tǒng)的輸出直接輸入給主濾波器，另一方面各子濾波器用它作為量測值，其他導(dǎo)航系統(tǒng)對應(yīng)各子濾波器的輸出結(jié)果也進(jìn)入主濾波器進(jìn)行數(shù)據(jù)融合，達(dá)到整個慣導(dǎo)系統(tǒng)參數(shù)的最優(yōu)濾波化。

3.2 組合系統(tǒng)聯(lián)邦濾波器設(shè)計

聯(lián)邦濾波器有四種實(shí)現(xiàn)模式，依據(jù)信息分配策略的不同，四種實(shí)現(xiàn)模式為：無反饋模式、融合-反饋模式、變比例模式、零復(fù)位模式。通過分析考慮組合系統(tǒng)的容錯性能、定位精度、濾波融合算法的難易程度，文中設(shè)計的聯(lián)邦濾波器為：βm=0、系統(tǒng)無重置，系統(tǒng)主濾波器狀態(tài)估計無信息分配不進(jìn)行濾波運(yùn)算，只負(fù)責(zé)完成組合系統(tǒng)信息融合[15]。最終設(shè)計了公共參考系統(tǒng)為慣導(dǎo)系統(tǒng)，子系統(tǒng)為GPS位置子系統(tǒng)和速度子系統(tǒng)的聯(lián)邦濾波器結(jié)構(gòu)，如圖1所示。

圖1 SINS/GPS聯(lián)邦濾波器結(jié)構(gòu)

4 聯(lián)邦濾波器的數(shù)據(jù)融合算法研究

在第二節(jié)提出的聯(lián)邦卡爾曼濾波器結(jié)構(gòu)中，各子濾波器沒有考慮反饋?zhàn)饔煤椭刂媒Y(jié)構(gòu)進(jìn)行獨(dú)立濾波。這樣能夠減少主濾波器到子濾波器的反饋信息和數(shù)據(jù)計算量。但是如果沒有反饋信息的作用，會降低濾波器的估計精度和濾波的穩(wěn)定性。綜合對以上因素的分析，本節(jié)在標(biāo)準(zhǔn)聯(lián)邦濾波器的基礎(chǔ)上應(yīng)用優(yōu)化算法，以提高系統(tǒng)容錯性能和濾波的實(shí)時性。

在實(shí)際的導(dǎo)航系統(tǒng)中，并不能準(zhǔn)確知道系統(tǒng)的噪聲方差陣Q和觀測噪聲方差陣R。從而在對不準(zhǔn)確的系統(tǒng)模型進(jìn)行濾波時，會出現(xiàn)濾波發(fā)散，導(dǎo)航系統(tǒng)在工作過程中Q、R、Φ、H參數(shù)容易發(fā)生變化。為了防止濾波發(fā)散，需要在濾波前對系統(tǒng)參數(shù)的變化進(jìn)行有效的估計，在濾波過程中及時調(diào)整濾波增益陣Kk，這便是自適應(yīng)濾波的基本思想。自適應(yīng)濾波算法有貝葉斯法、相關(guān)法、協(xié)方差匹配法等。文獻(xiàn)[16]介紹了基于Sage-Husa自適應(yīng)濾波算法和強(qiáng)跟蹤濾波算法改進(jìn)的Sage-Husa濾波算法。本小節(jié)采取該濾波算法設(shè)計出了基于改進(jìn)的Sage-Husa濾波算法的聯(lián)邦濾波算法。

4.1 Sage-Husa自適應(yīng)濾波算法

Sage-Husa自適應(yīng)濾波算法可以對觀測數(shù)據(jù)進(jìn)行遞推濾波，同時通過時變噪聲統(tǒng)計估計器實(shí)時估計和修正系統(tǒng)噪聲和觀測噪聲的統(tǒng)計特性，以此抑制濾波發(fā)散、提高濾波精度。其算法可描述為：

(8)

(9)

其中，b定義為遺忘因子，且0

4.2 強(qiáng)跟蹤卡爾曼濾波

上一節(jié)中在Sage-Husa自適應(yīng)濾波算法中增加了對系統(tǒng)噪聲統(tǒng)計特性的計算，計算量大幅增加后會降低濾波的實(shí)時性。組合導(dǎo)航系統(tǒng)中狀態(tài)變量維數(shù)高，Sage-Husa算法在對高階系統(tǒng)濾波的過程中，容易出現(xiàn)Q和R失去正定性濾波發(fā)散現(xiàn)象。針對這些問題，為了保證濾波的穩(wěn)定性，可以通過犧牲一定的精度換取濾波穩(wěn)定性，這就是強(qiáng)跟蹤卡爾曼濾波算法[17]將狀態(tài)估計誤差驗(yàn)前協(xié)方差陣乘以加權(quán)系數(shù)1，加強(qiáng)突變狀態(tài)跟蹤能力，在濾波器達(dá)到穩(wěn)態(tài)時，保持這種能力，該濾波算法對初值和噪聲統(tǒng)計特性的敏感也比較低。其算法的具體實(shí)現(xiàn)為：

(10)

其中，αi由先驗(yàn)知識確定。

4.3 改進(jìn)的Sage-Husa聯(lián)邦濾波器的設(shè)計

在對Sage-Husa自適應(yīng)濾波和強(qiáng)跟蹤卡爾曼濾波的特點(diǎn)進(jìn)行分析綜合后，考慮到濾波的精度和穩(wěn)定性，決定在收斂的條件下濾波時，采用Sage-Husa濾波算法；在濾波存在發(fā)散時，采用強(qiáng)跟蹤卡爾曼濾波。因此在濾波前需要用濾波的收斂性判斷濾波是否發(fā)散。在算法中可以利用新序列：

(11)

的性質(zhì)來構(gòu)造濾波器的收斂性判據(jù)。在式(9)中已經(jīng)包含了新序列的平方和，因此利用下面的新序列方差陣描述誤差大小的信息。

(12)

因而，可用：

(13)

作為收斂判據(jù)。其中γ≥1為可調(diào)系數(shù)，當(dāng)上式成立時，用Sage-Husa自適應(yīng)濾波，若不成立則說明濾波誤差超出預(yù)測值的γ倍，此時為濾波發(fā)散狀態(tài)，改用強(qiáng)跟蹤卡爾曼濾波。在第二節(jié)設(shè)計的標(biāo)準(zhǔn)聯(lián)邦卡爾曼濾波器結(jié)構(gòu)的基礎(chǔ)上應(yīng)用改進(jìn)的Sage-Husa算法，得到的聯(lián)邦濾波器結(jié)構(gòu)如圖2所示。

5 仿真實(shí)驗(yàn)與結(jié)果分析

在實(shí)際的慣性導(dǎo)航系統(tǒng)和GPS系統(tǒng)中，兩者擁有不同的采樣率，各自的采樣率周期不成整數(shù)倍，需要考慮到系統(tǒng)數(shù)據(jù)同步的問題。在本仿真實(shí)驗(yàn)中假設(shè)慣導(dǎo)系統(tǒng)數(shù)據(jù)更新采樣周期為0.01 s，GPS測量輸出周期為1 s，假設(shè)GPS在采樣周期內(nèi)數(shù)據(jù)保持不變，程序設(shè)計中不考慮數(shù)據(jù)同步問題。整個濾波仿真參數(shù)如下：

地球自轉(zhuǎn)角速度：7.292 115 8E-5 rad/s；

陀螺儀測量均方差：σg=0.001；

圖2 Sage-Husa聯(lián)邦濾波結(jié)構(gòu)

加速度測量均方差：σa=0.001；

GPS位置均方差：σp=25；

GPS速度均方差：σv=0.15；

慣導(dǎo)初始位置誤差：δφ=30''，δλ=30''，δh=15 m；

慣導(dǎo)初始速度誤差：δvx=0.1 m/s，δvy=0.1 m/s，δvz=0.1 m/s；

慣導(dǎo)初始姿態(tài)誤差：δψ=300''，δθ=300''，δγ=300''；

GPS初始位置誤差：δφ=1'' ，δλ=1''，δh=25 m；

GPS初始速度誤差：δvx=0.15 m/s，δvy=0.15 m/s，δvz= 0.15 m/s；

濾波初值：

X0= [30″,30″,15,0.1,0.1,0.1,300″,300″,300″, 0.001° ,0.001° ,0.001° ,0.001,0.001,0.001]；

P0=diag([30″,30″,15,0.1,0.1,0.1,300″,300″,300,0.001, 0.001,0.001,0.005,0.005,0.005]2)；

Q=diag([0.0012;0.0012;0.0012;0.0012;0.0012;0.0012])；

RV=diag([0.152;0.152;0.152])；

RP=diag([1″, 1″, 25]2)。

在上述濾波仿真程序中，及時對慣導(dǎo)系統(tǒng)的位置參數(shù)和速度參數(shù)進(jìn)行了修正。在濾波開始初期，利用估計的誤差狀態(tài)對慣導(dǎo)系統(tǒng)進(jìn)行反饋校正和輸出校正，當(dāng)濾波穩(wěn)定后繼續(xù)進(jìn)行輸出校正，同時實(shí)施周期性的反饋校正，以便達(dá)到較好的濾波精度和穩(wěn)定性。仿真結(jié)果見圖3～圖5。

圖3 聯(lián)邦濾波與標(biāo)準(zhǔn)濾波緯度誤差

圖4 聯(lián)邦濾波與標(biāo)準(zhǔn)濾波經(jīng)度誤差

圖5 聯(lián)邦濾波與標(biāo)準(zhǔn)濾波高度誤差

通過緯度、經(jīng)度、高度三個方向的仿真結(jié)果可看出，采用改進(jìn)的聯(lián)邦卡爾曼濾波數(shù)據(jù)融合算法比標(biāo)準(zhǔn)卡爾曼濾波算法的導(dǎo)航系統(tǒng)的定位誤差更小，組合導(dǎo)航系統(tǒng)采用GPS測量值可以及時修正慣導(dǎo)系統(tǒng)的測量誤差，可以避免純慣導(dǎo)系統(tǒng)的發(fā)散問題，提高了單一導(dǎo)航系統(tǒng)的定位精度。

6 結(jié)束語

重點(diǎn)研究了無人機(jī)自主避障系統(tǒng)中多傳感器信息融合時采用的卡爾曼濾波算法問題。在分析了傳統(tǒng)卡爾曼濾波在數(shù)據(jù)融合存在的問題以后，提出了分散化聯(lián)邦卡爾曼濾波的方式，采用改進(jìn)Sage-Husa聯(lián)邦濾波方法對組合導(dǎo)航進(jìn)行數(shù)據(jù)信息融合以此提高濾波精度和濾波穩(wěn)定性。最后進(jìn)行了數(shù)據(jù)融合MATLAB仿真實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果表明比傳統(tǒng)濾波效果明顯提高。