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

(上海工程技術大學，上海 201620)

1 概 述

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

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

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

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

1.2 信息融合理論算法

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

Blackman S S等提出了加權平均信息融合方法[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)絡—模糊信息融合方法[10]。

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

2 卡爾曼濾波基本原理

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

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

(1)

zk=Hkxk+vk

(2)

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

預估計：

(3)

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

(4)

計算卡爾曼增益矩陣：

(5)

更新估計：

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

(6)

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

(7)

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

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

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

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

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

(a)計算卡爾曼增益：

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

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

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

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

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

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

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

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

圖1 SINS/GPS聯(lián)邦濾波器結構

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

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

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

4.1 Sage-Husa自適應濾波算法

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

(8)

(9)

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

4.2 強跟蹤卡爾曼濾波

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

(10)

其中，αi由先驗知識確定。

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

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

(11)

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

(12)

因而，可用：

(13)

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

5 仿真實驗與結果分析

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

地球自轉角速度：7.292 115 8E-5 rad/s；

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

圖2 Sage-Husa聯(lián)邦濾波結構

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

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

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

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

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

慣導初始姿態(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)。

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

圖3 聯(lián)邦濾波與標準濾波緯度誤差

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

圖5 聯(lián)邦濾波與標準濾波高度誤差

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

6 結束語

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