一種改進的自適應卡爾曼濾波算法

許亞朝，何秋生，王少江，成 熊

（太原科技大學電子信息工程學院，太原030024）

?

一種改進的自適應卡爾曼濾波算法

許亞朝，何秋生，王少江，成 熊

（太原科技大學電子信息工程學院，太原030024）

摘 要：針對多傳感器數(shù)據(jù)融合時傳統(tǒng)卡爾曼濾波算法極易引起濾波發(fā)散，降低濾波精度和系統(tǒng)實時性的問題，研究一種改進的自適應濾波算法對多傳感器數(shù)據(jù)進行融合，得到更為準確的信息數(shù)據(jù)。該算法在簡化的Sage - Husa濾波基礎(chǔ)上引入濾波收斂性判據(jù)，抑制濾波發(fā)散并提高濾波精度和穩(wěn)定性。同時結(jié)合強跟蹤濾波思想調(diào)整增益矩陣，使濾波器具有強跟蹤濾波的特性，提高改進的濾波算法對不確定系統(tǒng)模型的魯棒性以及對突變狀態(tài)的濾波處理能力。將改進算法與傳統(tǒng)卡爾曼濾波算法進行仿真比較。仿真結(jié)果表明，在系統(tǒng)模型參數(shù)失配或?qū)嵶冊肼曃粗闆r下，改進的自適應濾波算法有更好的魯棒性，并且在系統(tǒng)狀態(tài)突變時仍有較好的濾波效果，明顯提高了濾波精度和實時性。

關(guān)鍵詞：卡爾曼濾波；Sage - Husa自適應濾波；強跟蹤濾波

兩輪自平衡車系統(tǒng)是高度不穩(wěn)定的非線性系統(tǒng)，在保持車體平衡過程中，控制器通過傳感器檢測到的姿態(tài)角度信號控制電機產(chǎn)生控制力矩來控制兩個車輪的速度和方向，以此控制車體平衡。因此，兩輪自平衡車的穩(wěn)定性很大程度上取決于姿態(tài)檢測的準確性，然而實際應用中加速度計動態(tài)過程采集數(shù)據(jù)誤差大，陀螺儀易受溫度和噪聲干擾導致漂移，單獨使用單一傳感器誤差較大，使測量數(shù)據(jù)不準確。目前大多采用多個傳感器測量并將測量數(shù)據(jù)進行融合的方法進行平衡車最優(yōu)姿態(tài)角的估計［1］。在傳感器數(shù)據(jù)融合中，標準卡爾曼濾波算法要求準確的系統(tǒng)模型和噪聲統(tǒng)計特性，在實際應用中很難達到，最終可能導致濾波發(fā)散。Sage-Husa自適應卡爾曼濾波算法可以實時估計噪聲統(tǒng)計特性，但會降低濾波實時性，增加算法的復雜程度，實際應用中效果并不理想。文獻［2］提出一種對Sage-Husa自適應卡爾曼濾波算法的改進方法，但是其公式復雜，計算量大，不易在實際應用中實現(xiàn)。文獻［3］提出一種簡化的Sage-Husa算法，只對對濾波影響大的測量噪聲協(xié)方差陣進行在線估計，但每個采樣周期都對進行估計，未能降低算法的復雜度，無法保證濾波的實時性。文獻［4］結(jié)合協(xié)方差匹配技術(shù)對Sage-Husa算法進行了改進，有效防止了濾波發(fā)散并提高了實時性，但由于此方法對系統(tǒng)模型不確定性的魯棒性差，并且在系統(tǒng)穩(wěn)定后對突變狀態(tài)失去跟蹤能力所以在實際應用中效果并非特別理想。

本文設計一種改進的濾波算法。對傳統(tǒng)Kalman濾波算法進行優(yōu)化改進，在簡化的Sage-Husa自適應濾波算法基礎(chǔ)上，引入濾波收斂性判據(jù)和強跟蹤濾波思想，提高濾波收斂性和穩(wěn)定性。在載有陀螺儀和加速度計慣性測量單元的兩輪自平衡小車上采集實驗數(shù)據(jù)，并在Matlab環(huán)境下對改進的濾波算法進行仿真測試。

1 基于改進的自適應卡爾曼濾波算法的數(shù)據(jù)融合

1.1 時變線性離散系統(tǒng)模型的建立

采用改進的卡爾曼濾波對加速度計和陀螺儀采集的數(shù)據(jù)進行融合，以陀螺儀測量角速度作為輸入量，加速度計測量值作為觀測量，通過濾波算法的不斷迭代準確地估計平衡車的實時姿態(tài)角度。

首先建立系統(tǒng)的狀態(tài)方程和測量方程，設線性離散系統(tǒng)數(shù)學模型如下：

狀態(tài)方程：

測量方程：

式（1），（2）中，為k時刻系統(tǒng)的狀態(tài)向量，U（k）是k時刻輸入信號，Z（k）為k時刻的測量向量，A，B，H分別為系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣、輸入控制矩陣和測量矩陣，v（k）和W（k）分別是測量噪聲和過程噪聲。

在兩輪平衡車姿態(tài)檢測系統(tǒng)中，車體的角度和角速度成導數(shù)關(guān)系，將車體角度θ作為狀態(tài)變量，將陀螺儀的常值偏差作為另一狀態(tài)變量。陀螺儀傾角測量方程：

式（3）中，θ（k）為車體角度，ω（k）是陀螺儀測得的角速度，v′（k）是陀螺儀測量噪聲，b（k）是陀螺儀常值偏差，dt是測量周期。由式（1）～式（3）可得：

1.2 基于Sage-Husa改進的自適應濾波算法的設計

基于Sage-Husa改進的自適應卡爾曼濾波算法為：

①狀態(tài)預測：

②新息序列更新：

③狀態(tài)預測協(xié)方差更新：

通過次優(yōu)漸消因子λ（k），實時調(diào)整狀態(tài)預測協(xié)方差陣和增益陣［5］，對歷史數(shù)據(jù)進行漸消，降低舊數(shù)據(jù)對當前濾波值的影響程度，使其具備強跟蹤濾波的優(yōu)良特性。

④測量噪聲協(xié)方差估計：

在Sage-Husa濾波算法中，測量噪聲協(xié)方差估計為：

式（9）中存在負因子，可能會使R（k）的估計值逐漸失去正定性，使濾波增益大于1，最終導致濾波發(fā)散。為預防測量噪聲估計失去正定性，修正為式（8）進行改進，保證當前估計值R（k）正定就可以保證濾波增益值小于1，抑制了濾波發(fā)散的可能性。

⑤濾波增益矩陣更新：

⑥姿態(tài)角狀態(tài)估計方程：

⑦狀態(tài)估計協(xié)方差：由式（10）和（12）可看出濾波增益和系統(tǒng)狀態(tài)估計協(xié)方差與測量噪聲協(xié)方差R（k）有關(guān)，R（k）對濾波影響更大，而Q（k）對濾波影響較小。所以本文基于簡化的Sage-Husa算法，僅對測量噪聲協(xié)方差R（k）進行估計，以減小計算量，提高實時性。

⑧濾波收斂性判據(jù)：

在濾波過程中，由濾波收斂性判據(jù)判斷當前濾波是否收斂，若滿足條件說明當前濾波收斂，不需要更新R（k），令；若不滿足則說明此時系統(tǒng)模型已不適應當前狀態(tài)，則需要由式（8）估計R（k）的新值。dk=（1 0＜b＜1是遺忘因子。

⑨系統(tǒng)噪聲估計：

系統(tǒng)噪聲主要由陀螺儀內(nèi)部機理決定，噪聲參數(shù)相對穩(wěn)定。只需如式（14）所示估計系統(tǒng)的噪聲協(xié)方差，式（14）中q-acce和q-gyro是加速度計和陀螺儀測量值的協(xié)方差，表示濾波器對傳感器的信任程度，本文通過實驗不斷修正其取值大小，以取得較好的濾波效果。

?自適應漸消因子：

式（17）中β≥1是為使狀態(tài)估計值更平滑引入的一個弱化因子。式（19）中ρ為遺忘因子［6］。

當模型不確定時Kalman濾波的魯棒性較差，使濾波過程中狀態(tài)估計不準。而且，在系統(tǒng)達到平穩(wěn)狀態(tài)時，對突變狀態(tài)的濾波效果無法滿足要求［7］。系統(tǒng)穩(wěn)定時，增益矩陣K（k）趨于極小值，若系統(tǒng)狀態(tài)發(fā)生突變，預報殘差V（k）增大，由式（10）可知，增益矩陣K（k）不會隨V（k）增大而增大，此時濾波器不能對突變狀態(tài)進行跟蹤處理。為克服這一缺陷，通過實時調(diào)整狀態(tài)預測協(xié)方差和相應的增益陣，使濾波器具有強跟蹤濾波的特性，對簡化的Sage-Husa算法進行改進。

強跟蹤濾波器在線選擇一個適當?shù)臑V波增益陣使：

即不同時刻的殘差序列正交。正交性原理表示在模型參數(shù)失配情況下，需在線調(diào)整增益矩陣K（k），使預報殘差時刻具有類似于高斯白噪聲的性質(zhì)［8-9］。V0為殘差的協(xié)方差。

將式（10）帶入（20）中得到：

即：

將式（7）帶入（22）中，簡化得：

由（23）有（17），（18）可求解λ（k）.

2 仿真分析

仿真數(shù)據(jù)在載有陀螺儀和加速度計慣性測量單元的兩輪自平衡小車上采集的。將實驗測量數(shù)據(jù)導入到Matlab中對改進的濾波算法進行仿真測試，設置參數(shù)初始值如下表。

表1 濾波算法初始參數(shù)設置Tab.1 Filter initial parameters settings

圖1是車體直立靜止狀態(tài)下應用改進后的自適應濾波算法得到的姿態(tài)角度輸出曲線。車體直立靜止時，加速度計測得的零位傾角值存在很大干擾噪聲。應用改進的濾波方法后輸出傾角曲線更平滑，濾除了原始輸出數(shù)據(jù)的干擾噪聲，使傾角誤差明顯減小，控制在0.5°之內(nèi)。濾波處理之后的數(shù)據(jù)經(jīng)控制單元解算輸出，控制電機輸出轉(zhuǎn)矩，使車體穩(wěn)定性更好。經(jīng)濾波處理之后的輸出傾角比原始輸出穩(wěn)定而有效，可滿足靜止狀態(tài)下車體姿態(tài)檢測的精度，說明了改進后的濾波方法在應用于平衡車姿態(tài)檢測的可行性。

圖2是在姿態(tài)檢測過程中人為加入突變數(shù)據(jù)，模擬小車在行走過程中由于噪聲干擾引起的抖動。由圖可以看出在2 s和4 s處的數(shù)據(jù)發(fā)生突變，加速度計測量數(shù)據(jù)發(fā)生較大變化，而經(jīng)濾波處理的輸出曲線很好的濾除由于突變數(shù)據(jù)引起的輸出震蕩，更好地保證了平衡車在行駛過程的穩(wěn)定性。

圖1 車身直立靜止時應用改進濾波算法后的傾角輸出曲線Fig.1 The improved filtering algorithm output with the car in vertically stationary

圖2 動態(tài)條件下改進的濾波算法傾角輸出曲線Fig.2 The improved filtering algorithm output under the dynamic condition

圖3是采用卡爾曼濾波方法對測量數(shù)據(jù)處理后的傾角曲線。標準卡爾曼濾波要求準確的系統(tǒng)模型和噪聲統(tǒng)計特性，當系統(tǒng)模型不準確或者噪聲統(tǒng)計特性發(fā)生改變時，濾波效果就會變差。在實際情況下模型和噪聲統(tǒng)計特性并不能保證非常精確，可能導致濾波發(fā)散，這種方法在實際應用中局限性很大。當改變測量噪聲的協(xié)方差，濾波后的輸出傾角誤差較大，并可能引起濾波發(fā)散，如圖4所示。

如圖5所示，Sage-Husa濾波算法的濾波效果較好，但是Sage-Husa濾波對初值選取要求高，易引起濾波發(fā)散。并且由于其需要不斷估計過程噪聲和測量噪聲的協(xié)方差陣，增加了計算量，容易積累過程誤差，在實際應用中長時間運行會降低姿態(tài)角度檢測的精度和實時性。當測量噪聲和系統(tǒng)噪聲協(xié)方差初值選取不當，或?qū)嶋H運行中對時變噪聲的估計R（k）取負時，而使濾波增益大于1，導致濾波發(fā)散，如圖6所示，濾波發(fā)散使姿態(tài)角度輸出遠離真實值。

圖3 系統(tǒng)模型和噪聲統(tǒng)計特性已知時卡爾曼濾波傾角輸出Fig.3 Kalman filter output with system model and the known noise statistical characteristics

圖4 系統(tǒng)模型和噪聲統(tǒng)計特性變化后卡爾曼濾波傾角輸出Fig.4 Kalman filter output with system model and the changing noise statistical properties

圖5 Sage-Husa濾波收斂時傾角輸出Fig.5 Sage-Husa filtering output with convergence

圖6 Sage-Husa濾波發(fā)散時傾角輸出Fig.6 Sage-Husa filtering output with divergence

如圖7，系統(tǒng)運行過程中，在2至3 s內(nèi)突然改變加速度計測量傾角值，模擬實際情況中由路況影響使車體角度突然改變。改進后的濾波算法在車身角度突然改變時能比較準確的實時跟蹤車體實際角度，而其他方法瞬間失去跟蹤能力，導致姿態(tài)角度估計不準，最終使控制器得不到正確的傾角信息而使車身不穩(wěn)甚至傾倒。

綜上仿真結(jié)果表明，改進的濾波算法對加速度計的零位偏差有較好的修正能力，能夠明顯抑制動態(tài)過程中的干擾噪聲。改進濾波方法具有較強收斂性，并且在系統(tǒng)模型和噪聲統(tǒng)計特性不精確情況下具有較強魯棒性，同時在車身角度發(fā)生突變時能比較快速準確的實時跟蹤車體實際角度。

圖7 突變狀態(tài)下改進的濾波算法與其他濾波算法輸出比較Fig.7 Output comparison between improved filtering algorithm and others in mutation condition

3 結(jié)論

本文針對傳統(tǒng)卡爾曼濾波算法在對多傳感器數(shù)據(jù)融合過程中的局限性，改進了一種自適應強跟蹤卡爾曼濾波算法。文中分析并比較了卡爾曼濾波算法和Sage-Husa濾波算法的不足和相同條件下改進濾波方法的優(yōu)越性。仿真結(jié)果表明，改進的濾波算法能有效的修正加速度計零位偏差，并能明顯抑制動態(tài)過程中的噪聲干擾實現(xiàn)平滑濾波，在狀態(tài)突變時能快速準確的實時跟蹤車體實際角度，比傳統(tǒng)卡爾曼類濾波方法有較強的穩(wěn)定性和魯棒性。

參考文獻：

［1］ 楊凌霄，李曉陽.基于卡爾曼濾波的兩輪自平衡車姿態(tài)檢測方法［J］.計算機仿真，2014（31）：406-409.

［2］ 周晶，黃顯高.一種改進的Sage-Husa自適應濾波算法［J］.儀器儀表用戶，2009（16）：71-73.

［3］ 沈云峰，朱海，莫軍.簡化的Sage-Husa自適應濾波算法在組合導航中的應用及仿真［J］.青島大學學報，2001（16）：44-47.

［4］ 魯平，趙龍，陳哲.改進的Sage-Husa自適應濾波及其應用［J］.系統(tǒng)仿真學報，2007，19（15）：3503-3505.

［5］ 宋康寧，叢爽，鄧科，等.自適應強跟蹤卡爾曼濾波在陀螺穩(wěn)定平臺中的應用［J］.中國科學技術(shù)大學學報，2015（7）：401-406.

［6］ 張欣，白越，趙常均，等.多旋翼姿態(tài)解算中的改進自適應擴展Kalman算法［J］.光學精密工程，2014，22（12）：3384-3390.

［7］ 郭澤，繆玲娟，趙洪松.一種改進的強跟蹤UKF算法及其在SINS大方位失準角初始對準中的應用［J］.航空學報，2014，35（1）：203-214.

［8］ ZHANG Z T，ZHANG J S.A novel strong tracking finite-difference extended Kalman filter for nonlinear eye tracking［J］.Science in China，2009，52（4）：688-694.

［9］ 范文兵，劉春風，張素貞.一種強跟蹤擴展卡爾曼濾波器的改進算法［J］.控制與決策，2006，21（1）：73-76.

（School of Electronic and Information Engineering，Taiyuan University of Science & Technology，Taiyuan 030024，China）

An Improved Adaptive Kalman Filtering Algorithm

XU Ya-zhao，HE Qiu-sheng，WANG Shao-jiang，CHENG Xiong

Key words：kalman filter，sage-husa adaptive filter，strong tracking filter

Abstract：Traditional Kalman filtering algorithm easily leads filter to diverge and reduces the filtering accuracy and system real-time performance when multi-sensor is mixed with data.This paper puts forward an improved adaptive filtering algorithm for multi-sensor mixing data，which gets more accurate information data.The algorithm restrains filtering divergence and improves filtering accuracy and stability with introducing filtering convergence criterion to the simplified Sage-Husa filter.And the algorithm adjusts gain matrix with strong tracking filter，thus making the filter have strong tracking performance and improving the filter′s robustness for uncertain system model and processing capacity for mutation status.Simulation comparison between the improved algorithm and the traditional Kalman filtering algorithm shows that the improved adaptive filtering algorithm has better robustness and filtering effectiveness with system model parameters mismatched and the real variable noise unknown.And it also shows that the filtering accuracy and real-time performance have been improved obviously.

中圖分類號：TP212.9

文獻標志碼：A

doi：10.3969/ j.issn.1673 -2057.2016.03.001

文章編號：1673 -2057（2016）03 -0163 -06

收稿日期：2015-08-22

基金項目：山西省自然科學基金（2013011035-2）

作者簡介：許亞朝（1990 -），男，碩士研究生，主要研究方向為智能控制理論研究。