基于迭代卡爾曼濾波的傳感器數(shù)據(jù)融合仿真

韋 黔，陳 迪，林樹(shù)靖，仇三銘，張同興

(1.上海交通大學(xué) 電子信息與電氣工程學(xué)院 儀器科學(xué)與工程系，上海 200240；2.上海智能診療儀器工程技術(shù)研究中心，上海 200240；3.上海直川電子科技有限公司，上海 201109)

基于迭代卡爾曼濾波的傳感器數(shù)據(jù)融合仿真

韋 黔1,2，陳 迪1,2，林樹(shù)靖1,2，仇三銘1,2，張同興3

(1.上海交通大學(xué) 電子信息與電氣工程學(xué)院 儀器科學(xué)與工程系，上海 200240；2.上海智能診療儀器工程技術(shù)研究中心，上海 200240；3.上海直川電子科技有限公司，上海 201109)

在對(duì)運(yùn)動(dòng)系統(tǒng)進(jìn)行實(shí)時(shí)動(dòng)態(tài)姿態(tài)信息檢測(cè)過(guò)程中，常常需要加速度計(jì)、陀螺儀等多個(gè)慣性傳感器的協(xié)同工作，因此需要對(duì)多傳感器的輸出數(shù)據(jù)進(jìn)行融合處理。卡爾曼濾波法是其中常用的一種數(shù)據(jù)融合算法。該算法的濾波融合精度，直接影響了運(yùn)動(dòng)系統(tǒng)姿態(tài)信息的精度與實(shí)時(shí)性。在研究傳統(tǒng)的卡爾曼濾波算法的基礎(chǔ)上，仿真分析了過(guò)程噪聲協(xié)方差矩陣、測(cè)量噪聲協(xié)方差矩陣對(duì)于卡爾曼濾波算法的濾波精度、響應(yīng)時(shí)間的影響，提出了一種基于數(shù)據(jù)迭代法的卡爾曼濾波融合算法。該算法將多傳感器數(shù)據(jù)融合后的輸出再次進(jìn)行迭代運(yùn)算，實(shí)現(xiàn)了較好的融合效果。仿真實(shí)驗(yàn)結(jié)果表明，相較于傳統(tǒng)的卡爾曼濾波算法，所提出算法的復(fù)雜度低，實(shí)時(shí)性好，同時(shí)濾波精度大幅提升。

卡爾曼濾波器；傳感器數(shù)據(jù)融合；濾波精度；迭代法；仿真

0 引 言

在如設(shè)備安裝、機(jī)器人控制、測(cè)量機(jī)床儀器導(dǎo)軌的直線度、小車(chē)的位姿控制等領(lǐng)域中，都經(jīng)常需要測(cè)量?jī)A角。但是目前市面上的傾角傳感器大多價(jià)格昂貴，因此該類(lèi)型傳感器的應(yīng)用受到了一定限制。而且通常的傾角傳感器主要用于靜態(tài)測(cè)量，測(cè)量精度易受物體運(yùn)動(dòng)加速度的影響，導(dǎo)致測(cè)量誤差增加，甚至無(wú)法使用。雖然國(guó)外如XSENS等公司已有類(lèi)似產(chǎn)品上市[1]，但價(jià)格居高不下，而且精度不高，使其應(yīng)用范圍受限。雖然也有高精度的軍民兩用產(chǎn)品(如美國(guó)Crossbow的產(chǎn)品)[2]，但其高昂的價(jià)格同樣令國(guó)內(nèi)廠家無(wú)法承受，另有部分產(chǎn)品由于其高指標(biāo)而不能順利購(gòu)買(mǎi)。就國(guó)內(nèi)而言，目前還沒(méi)有類(lèi)似的研發(fā)成熟、成本低廉、精度較高的產(chǎn)品用于實(shí)際的工業(yè)與科研領(lǐng)域，因此進(jìn)行動(dòng)態(tài)傾角傳感器的自主研發(fā)與生產(chǎn)，不僅可以打破國(guó)外的壟斷，而且具有廣闊的市場(chǎng)前景與巨大的經(jīng)濟(jì)價(jià)值。

在動(dòng)態(tài)傾角傳感器的設(shè)計(jì)過(guò)程中，關(guān)鍵的問(wèn)題在于通過(guò)卡爾曼濾波器對(duì)采集到的加速度計(jì)信號(hào)與陀螺儀信號(hào)進(jìn)行數(shù)據(jù)融合，以獲得高精度、穩(wěn)定的輸出角度值[3-5]。由于目前國(guó)內(nèi)外研究者對(duì)卡爾曼濾波器相關(guān)參數(shù)的選取仍然缺乏成熟、有效的方法，因此往往因參數(shù)選取不當(dāng)，造成濾波精度較低，甚至出現(xiàn)濾波發(fā)散等情況[6]。通過(guò)Matlab軟件建立仿真模型，在分析過(guò)程噪聲協(xié)方差矩陣、測(cè)量噪聲協(xié)方差矩陣對(duì)傳統(tǒng)卡爾曼濾波算法的濾波精度、響應(yīng)時(shí)間的影響的基礎(chǔ)上，提出一種基于數(shù)據(jù)迭代法的卡爾曼濾波融合算法，將多傳感器數(shù)據(jù)融合后的輸出再次進(jìn)行迭代運(yùn)算，并與傳統(tǒng)卡爾曼濾波算法進(jìn)行仿真對(duì)比。

1 卡爾曼濾波數(shù)據(jù)融合算法

卡爾曼濾波法于1960年由匈牙利數(shù)學(xué)家卡爾曼提出[7]，經(jīng)過(guò)數(shù)十年發(fā)展，如今廣泛應(yīng)用于導(dǎo)航與定位、圖像識(shí)別、傳感器數(shù)據(jù)融合等多個(gè)領(lǐng)域[8-9]，并衍生出了多種創(chuàng)新算法。該算法主要包括兩個(gè)過(guò)程：預(yù)估與校正。首先通過(guò)預(yù)估過(guò)程，建立時(shí)間更新方程來(lái)對(duì)當(dāng)前系統(tǒng)的狀態(tài)進(jìn)行一個(gè)先驗(yàn)估計(jì)，及時(shí)向前推算當(dāng)前狀態(tài)變量和誤差協(xié)方差估計(jì)的值，以便為下一個(gè)時(shí)間狀態(tài)構(gòu)造先驗(yàn)估計(jì)值；然后校正過(guò)程負(fù)責(zé)反饋，利用測(cè)量更新方程在預(yù)估過(guò)程的先驗(yàn)估計(jì)值及當(dāng)前測(cè)量變量的基礎(chǔ)上，建立起對(duì)當(dāng)前狀態(tài)改進(jìn)的后驗(yàn)估計(jì)。這個(gè)過(guò)程稱(chēng)之為預(yù)估-校正過(guò)程，對(duì)應(yīng)的估計(jì)算法稱(chēng)為預(yù)估-校正算法[10-14]。將系統(tǒng)的傾角真值與陀螺儀的常值偏差b作為狀態(tài)向量，用于加速度計(jì)與陀螺儀數(shù)據(jù)融合的離散卡爾曼濾波方程如下所示：

(1)

(2)

(3)

(4)

(5)

其中，A為系統(tǒng)狀態(tài)變換矩陣；H為系統(tǒng)觀測(cè)模型矩陣；p(k)為系統(tǒng)估計(jì)誤差協(xié)方差矩陣；K(k)為卡爾曼增益或混合因數(shù)；R為系統(tǒng)測(cè)量誤差協(xié)方差矩陣；ωgyro為陀螺儀的輸出值；φacce為加速度計(jì)的輸出值；T為采樣時(shí)間；Q為系統(tǒng)過(guò)程噪聲協(xié)方差矩陣，其具體形式為：

(6)

其中，qacce與qgyro分別為加速度計(jì)與陀螺儀的測(cè)量誤差。

通過(guò)上述五個(gè)卡爾曼濾波方程的反復(fù)迭代計(jì)算，可以將加速度計(jì)輸出的角度值與陀螺儀輸出的角速度值進(jìn)行融合，找到系統(tǒng)的最佳輸出，得到一個(gè)高精度的物體姿態(tài)角度值。

2 仿真實(shí)驗(yàn)

為了驗(yàn)證濾波算法取不同參數(shù)對(duì)于卡爾曼濾波特性的影響，通過(guò)數(shù)據(jù)采集裝置采集了動(dòng)態(tài)傾角傳感器在施加外部擾動(dòng)情況下的輸出角度值并利用Matlab軟件進(jìn)行仿真實(shí)驗(yàn)。

2.1數(shù)據(jù)采集裝置結(jié)構(gòu)

數(shù)據(jù)采集裝置由傾角傳感器、串行接口與電子計(jì)算機(jī)組成。其中傾角傳感器由一個(gè)加速度計(jì)ADXL203、兩個(gè)正交放置的陀螺儀ENC-03R、電源模塊以及STM32單片機(jī)模塊組成。加速度計(jì)測(cè)量得到的物體角度值與陀螺儀測(cè)量得到的物體角速度值經(jīng)過(guò)串行接口進(jìn)入電子計(jì)算機(jī)進(jìn)行保存，然后通過(guò)Matlab軟件進(jìn)行仿真。

2.2仿真實(shí)驗(yàn)過(guò)程

(1)調(diào)試數(shù)據(jù)采集裝置；

(2)將傾角傳感器水平放置于實(shí)驗(yàn)臺(tái)，開(kāi)始供電，一段時(shí)間后開(kāi)始采樣。在采樣過(guò)程中，對(duì)傳感器施加兩段小幅擾動(dòng)，同時(shí)在兩段擾動(dòng)中間讓傳感器有一個(gè)小幅的角度變化(10°)；

(3)結(jié)束采樣，分別保存采集到的物體角度與角度值數(shù)據(jù)，送入Matlab，進(jìn)行卡爾曼濾波數(shù)據(jù)仿真實(shí)驗(yàn)；

(4)改變卡爾曼濾波器參數(shù)，重復(fù)上述仿真過(guò)程，對(duì)結(jié)果進(jìn)行對(duì)比。

3 實(shí)驗(yàn)結(jié)果

設(shè)置采樣間隔為0.02 s，采樣時(shí)間為6.68 s，在開(kāi)始數(shù)據(jù)采集后的1.7～2.4 s之間施加一段擾動(dòng)，然后從2.9 s開(kāi)始給予系統(tǒng)一個(gè)角度變化，直到3.4 s后恢復(fù)穩(wěn)定，最后從4.4 s開(kāi)始施加第二段擾動(dòng)。將系統(tǒng)角度值突變后的濾波響應(yīng)時(shí)間t(從采樣時(shí)間2.9 s處角度值開(kāi)始突變到角度值回到穩(wěn)態(tài)值的90%的時(shí)間)、超調(diào)量(角度值跳變后的第一個(gè)波峰減去穩(wěn)態(tài)值)與第二段擾動(dòng)進(jìn)行濾波后的輸出角度誤差平均幅值作為評(píng)價(jià)濾波效果的指標(biāo)。采集到的加速度計(jì)輸出如圖1所示。

圖1 加速度計(jì)輸出角度采樣值

加速度計(jì)的測(cè)量值指標(biāo)如表1所示。

表1 加速度計(jì)測(cè)量數(shù)據(jù)采集指標(biāo)

3.1Q陣對(duì)于濾波效果的影響

假設(shè)對(duì)于加速度計(jì)輸出與陀螺儀輸出的信任度相等，即qacce=qgyro，則Q陣可量化為一個(gè)常數(shù)a與二階單位陣I的乘積，即：Q=a*I。保持其他參數(shù)不變，取R=10-3，變化測(cè)量噪聲協(xié)方差矩陣Q的初值，對(duì)采樣數(shù)據(jù)進(jìn)行卡爾曼濾波，得到的不同濾波效果如圖2所示。

圖2 不同Q取值下的卡爾曼濾波效果

由圖2可見(jiàn)，隨著過(guò)程噪聲協(xié)方差矩陣Q逐漸減小，濾波后的誤差平均幅值逐漸降低，但是Q進(jìn)一步減小到10-4以后，濾波后的角度值會(huì)產(chǎn)生超調(diào)，且隨著Q的進(jìn)一步減小，超調(diào)會(huì)有一定的增大。與此同時(shí)，系統(tǒng)對(duì)角度值突變的跟蹤速度變慢了，需要一定時(shí)間才能跟蹤出角度的變化，即響應(yīng)時(shí)間越來(lái)越長(zhǎng)。

3.2R陣對(duì)于濾波效果的影響

為了進(jìn)一步研究R的取值對(duì)于濾波效果的影響，綜合考慮響應(yīng)時(shí)間、超調(diào)量與誤差平均幅值的影響，取Q=10-3I，保持其他參數(shù)不變，改變R的初值，不同取值情況下的濾波效果如圖3所示。

從圖3可以看出，R取值較大時(shí)濾波效果較好，對(duì)誤差的抑制效果較大。但是會(huì)產(chǎn)生超調(diào)，且響應(yīng)較慢。如果繼續(xù)減小R，對(duì)于誤差的抑制作用將越來(lái)越低，但無(wú)超調(diào)，跟蹤速度較快。

圖3 不同R取值下的卡爾曼濾波效果圖

4 引入迭代法的卡爾曼濾波仿真

通過(guò)對(duì)Q和R的討論，發(fā)現(xiàn)無(wú)法得到一個(gè)能使卡爾曼濾波器獲得最優(yōu)濾波效果的Q、R取值，使其同時(shí)滿(mǎn)足響應(yīng)快、濾波精度高的要求。實(shí)際上，對(duì)于Q、R參數(shù)的最優(yōu)化選取一直是卡爾曼濾波領(lǐng)域一個(gè)亟待解決的問(wèn)題。為了克服這一問(wèn)題，通過(guò)在卡爾曼濾波器中引入迭代法來(lái)進(jìn)行濾波。選擇一個(gè)固定的Q、R取值(Q=10-3I，R=10-3)，采用已濾波后的輸出信號(hào)作為新的輸入信號(hào)再次進(jìn)行濾波，得到一個(gè)新的輸出信號(hào)，將這樣的一次再濾波過(guò)程稱(chēng)為一次迭代濾波過(guò)程，經(jīng)過(guò)一次迭代濾波后的輸出信號(hào)稱(chēng)為一次迭代濾波信號(hào)。同理，經(jīng)過(guò)多次迭代濾波后的輸出信號(hào)稱(chēng)為多次迭代濾波輸出信號(hào)。通過(guò)Matlab仿真，可將不同迭代次數(shù)下原始信號(hào)經(jīng)卡爾曼濾波后的濾波效果在圖4與圖5中加以比較。

圖4 不同迭代次數(shù)下的卡爾曼濾波效果圖

從不同迭代次數(shù)下的濾波效果可以看出，采用迭代法卡爾曼濾波進(jìn)行數(shù)據(jù)融合，與未迭代相比濾波效果有所提升，而且隨著迭代次數(shù)的增加，濾波后的誤差平均幅值與未迭代相比有大幅減少，但也注意到，當(dāng)?shù)螖?shù)過(guò)多時(shí)，誤差平均幅值的減小將不再明顯。與此同時(shí)，系統(tǒng)會(huì)開(kāi)始產(chǎn)生超調(diào)，同時(shí)超調(diào)量會(huì)隨著迭代次數(shù)的增加越來(lái)越大，同時(shí)響應(yīng)越來(lái)越慢。迭代3次情況下實(shí)時(shí)性好，濾波精度高，超調(diào)也較低。

圖5 不同迭代次數(shù)下的卡爾曼濾波效果指標(biāo)對(duì)比

5 結(jié)束語(yǔ)

相關(guān)參數(shù)的選取問(wèn)題一直是影響傳統(tǒng)卡爾曼濾波算法濾波效果的關(guān)鍵因素，而目前還沒(méi)有成熟、有效的取值方法,通過(guò)采用Matlab軟件對(duì)采集到的傾角傳感器輸出數(shù)據(jù)進(jìn)行仿真并具體量化分析比較Q、R兩個(gè)關(guān)鍵參數(shù)的不同取值對(duì)卡爾曼濾波性能所造成的影響，在此基礎(chǔ)上進(jìn)行分析比對(duì)，找到了一個(gè)較優(yōu)的取值組合。為了進(jìn)一步提高濾波效果，在傳統(tǒng)卡爾曼濾波算法的基礎(chǔ)上，提出了迭代法卡爾曼濾波，將一次卡爾曼濾波后的輸出數(shù)據(jù)再次引入系統(tǒng)進(jìn)行迭代融合濾波。

仿真結(jié)果表明，在恰當(dāng)?shù)牡螖?shù)條件下(3次)，相較于傳統(tǒng)的卡爾曼濾波法，該算法可大幅提高系統(tǒng)的濾波精度，使低濾波后誤差的平均幅值，同時(shí)保證了系統(tǒng)的實(shí)時(shí)性，具有一定的工程應(yīng)用價(jià)值。

[1] Giansanti D,Maccioni G,Macellari V.The development and test of a device for the Reconstruction of 3-D position and orientation by means of a kinematic sensor assembly with rate gyroscopes and accelerometers[J].IEEE Transactions on Biomedical Engineering,2005,52(7):1271-1277.

[2] Tiwari A,Ballal P,Lewis F L.Energy-efficient wireless sensor network design and implementation for condition-based maintenance[J].ACM Transactions on Sensor Networks,2007,3(1):1.

[3] 馮智勇,曾 瀚,張 力,等.基于陀螺儀及加速度計(jì)信號(hào)融合的姿態(tài)角度測(cè)量[J].西南師范大學(xué)學(xué)報(bào):自然科學(xué)版,2011,36(4):137-141.

[4] 鄒 波，張 華，姜 軍.多傳感信息融合的改進(jìn)擴(kuò)展卡爾曼濾波定姿[J].計(jì)算機(jī)應(yīng)用研究，2014，31(4):1035-1038.

[5] 高同躍,龔振邦,羅 均,等.基于加速度計(jì)和角速率陀螺的超小無(wú)人直升機(jī)姿態(tài)控制系統(tǒng)[J].飛行器測(cè)控學(xué)報(bào),2007,26(1):70-73.

[6] Kownacki C.Optimization approach to adapt Kalman filters for the real-time application of accelerometer and gyroscope signal’s filtering[J].Digital Signal Processing,2011,21(1):131-140.

[7] Kalman R E.A new approach to linear filtering and prediction problems[J].Journal of Fluids Engineering,1960,82(1):35-45.

[8] 鐘偉才,劉 靜,劉 芳,等.二階卡爾曼濾波分布估計(jì)算法[J].計(jì)算機(jī)學(xué)報(bào),2004,27(9):1272-1277.

[9] 夏偉才,曾致遠(yuǎn).一種基于卡爾曼濾波的背景更新算法[J].計(jì)算機(jī)技術(shù)與發(fā)展,2007,17(10):134-136.

[10] 梁丁文,袁 磊,蔡之華,等.基于無(wú)跡卡爾曼濾波傳感器信息融合的車(chē)輛導(dǎo)航算法[J].計(jì)算機(jī)應(yīng)用,2013,33(12):3444-3448.

[11] 周戰(zhàn)馨,高亞楠,陳家斌,等.基于無(wú)軌跡卡爾曼濾波的大失準(zhǔn)角INS初始對(duì)準(zhǔn)[J].系統(tǒng)仿真學(xué)報(bào),2006,18(1):173-175.

[12] 董健康,安 東.自適應(yīng)卡爾曼濾波在組合導(dǎo)航中的應(yīng)用研究[J].計(jì)算機(jī)技術(shù)與發(fā)展,2011,21(10):183-185.

[13] Gan Q,Harris C J.Comparison of two measurement fusion methods for Kalman filter based multisensor data fusion[J].IEEE Transactions on Aerospace and Electronic Systems,2001,37(1):273-279.

[14] 卞月根,張 偉.基于非線性卡爾曼濾波的車(chē)輛定位優(yōu)化算法[J].計(jì)算機(jī)技術(shù)與發(fā)展,2015,25(8):80-83.

Simulation of Multisensor Data Fusion Based on Iterative Kalman Filter

WEI Qian1,2，CHEN Di1,2，LIN Shu-jing1,2，QIU San-ming1,2，ZHANG Tong-xing3

(1.Department of Instrument Science and Engineering,School of Electronic Information and Electrical Engineering,Shanghai Jiaotong University,Shanghai 200240,China; 2.Shanghai Engineering Research Center for Intelligent Diagnosis and Treatment Instrument,Shanghai 200240,China; 3.Shanghai Zhichuan Electronic Tech Co,Ltd.,Shanghai 201109,China)

It is needed the cooperation for inertial sensors like accelerometers and gyroscopes in real-time and dynamic attitude information detection for mobile system,so the output data of multisensors is required to fuse and process.The Kalman filter,as a data fusion algorithm,is usually adopted and its filtering accuracy would have a direct influence on the accuracy and real-time performance of the attitude information of mobile system.On the basis of research on the traditional Kalman filter,simulation and analysis of the impact of process and measurement noise covariance matrix on the accuracy and response time of Kalman filtering algorithm,a novel Kalman filter based on a data iterative algorithm is presented,which carries on the iterative operation again for the output after fusion of multisensors data,realizing the good effect.Simulation shows that it has a good real-time performance and low complexity and promotes filtering accuracy significantly compared with traditional Kalman filter.

Kalman filter;sensory data fusion;filtering precision;iterative algorithm;simulation

2016-09-06

：2016-12-14 < class="emphasis_bold">網(wǎng)絡(luò)出版時(shí)間

時(shí)間：2017-07-05

科技部重點(diǎn)研發(fā)項(xiàng)目(2016YFC0100902)；上海市閔行區(qū)產(chǎn)學(xué)研項(xiàng)目(2014MH163)

韋 黔(1991-)，男(布依族)，碩士研究生，研究方向?yàn)镸EMS傳感器；陳 迪，教授，博士生導(dǎo)師，研究方向?yàn)榉枪鐼EMS微加工技術(shù)、準(zhǔn)LIGA技術(shù)、Bio-MEMS和RF-MEMS。

http://kns.cnki.net/kcms/detail/61.1450.TP.20170705.1651.052.html

TP301.6

:A

:1673-629X(2017)09-0137-04

10.3969/j.issn.1673-629X.2017.09.030