融合互補濾波和卡爾曼濾波的姿態(tài)解算算法

劉 穎，楊 媛，徐金濤

(1.西安郵電大學(xué) 自動化學(xué)院，陜西 西安 710121；2.中國科學(xué)院西安光學(xué)精密機械研究所 西安中科華芯測控有限公司，陜西 西安 710119)

隨著現(xiàn)代化微電子和微加工技術(shù)的發(fā)展，由微電子機械系統(tǒng)(Micro-Electro-Mechanical System，MEMS)陀螺儀、加速度計和地磁計組成的捷聯(lián)慣性測量單元作為慣性導(dǎo)航系統(tǒng)重要的組成部分，已廣泛應(yīng)用在航空航天、軍事等領(lǐng)域。在慣性導(dǎo)航系統(tǒng)中，姿態(tài)角的測量和解算是重要的控制部分[1]，只有得到較為準(zhǔn)確的姿態(tài)信息，才能對測量對象的運動姿態(tài)進(jìn)行控制，因此，提高載體姿態(tài)解算精度一直是研究熱點。在實際對姿態(tài)進(jìn)行解算過程中，MEMS陀螺儀在進(jìn)行積分運算時會產(chǎn)生累計誤差，并且受工作溫度的影響會產(chǎn)生溫度漂移。加速度計在靜態(tài)實驗中，可以得到相對穩(wěn)定且準(zhǔn)確地數(shù)據(jù)，但是在動態(tài)試驗中，容易受到外界環(huán)境以及噪聲影響，進(jìn)而影響數(shù)據(jù)的采集。地磁計所采集的數(shù)據(jù)在實驗過程中可能會受地磁等因素的影響。因此，通常融合3個傳感器數(shù)據(jù)進(jìn)行姿態(tài)解算[2]。

目前，已有多種傳感器數(shù)據(jù)融合的姿態(tài)解算方法。較為成熟的有互補濾波技術(shù)、卡爾曼濾波技術(shù)、粒子濾波技術(shù)和神經(jīng)網(wǎng)絡(luò)技術(shù)等。其中，互補濾波原理較其他方法簡單，運算量較小，主要適用于姿態(tài)精度要求較低的慣性導(dǎo)航系統(tǒng)，能夠?qū)υ肼暫推七M(jìn)行抑制，但其截止頻率不易確定，并且如果慣性系統(tǒng)對姿態(tài)解算精度有較高要求或者慣性系統(tǒng)長期處在運動狀態(tài)，單純使用互補濾波對姿態(tài)進(jìn)行解算往往是不能滿足預(yù)期的要求。除此之外，卡爾曼濾波也被廣泛的應(yīng)用于姿態(tài)解算?？柭鼮V波算法[3]的實時性強，且結(jié)構(gòu)簡單，但是不適用于非線性系統(tǒng)。在此基礎(chǔ)上，擴展卡爾曼濾波[4]被提出，其主要工作是將狀態(tài)方程和量測方程進(jìn)行線性化處理，對于弱非線性系統(tǒng)，可以獲得較為準(zhǔn)確的結(jié)果，而對于強非線性系統(tǒng)，則會導(dǎo)致濾波器性能下降甚至濾波發(fā)散，從而產(chǎn)生誤差。文獻(xiàn)[5]提出了一種改進(jìn)的自適應(yīng)卡爾曼濾波算法，利用預(yù)報殘差的方法求解最佳遺忘因子，進(jìn)一步對在線估計的量測噪聲協(xié)方差陣和狀態(tài)估計誤差方陣采用上三角-對角(Unit Upper Triangular and Diagonal Factorization，UD)分解的形式進(jìn)行表示和更新，以此避免自適應(yīng)濾波過程中的濾波發(fā)散現(xiàn)象。

在文獻(xiàn)[5]的基礎(chǔ)上，擬提出一種基于互補濾波和改進(jìn)自適應(yīng)卡爾曼的姿態(tài)解算融合算法。利用互補濾波器融合多個傳感器數(shù)據(jù)信息以提高測量精度，并將融合得到的姿態(tài)信息作為觀測值，通過改進(jìn)的自適應(yīng)卡爾曼濾波器，彌補在動態(tài)環(huán)境下濾波過程中存在的發(fā)散問題，以期得到精確的動態(tài)情景下的姿態(tài)解算信息。

1 載體姿態(tài)描述與更新

(1)

式中：θ、ψ和φ分別表示繞坐標(biāo)軸yb轉(zhuǎn)過的角度、繞坐標(biāo)軸zb轉(zhuǎn)過的角度以及繞坐標(biāo)軸xb轉(zhuǎn)過的角度；q0、q1、q2和q3表示當(dāng)前時刻的四元數(shù)參數(shù)值。

姿態(tài)更新理論是基于MEMS陀螺儀、加速度計以及電子羅盤組件的測量值對姿態(tài)矩陣進(jìn)行實時更新，采用四元數(shù)法進(jìn)行更新姿態(tài)數(shù)據(jù)。在具體的實踐中，四元數(shù)可描述剛體對于某個坐標(biāo)系的定點運動，表達(dá)式中標(biāo)量部分表示剛體轉(zhuǎn)動角余弦值的一半，矢量部分分別表示剛體轉(zhuǎn)動角度、旋轉(zhuǎn)軸以及轉(zhuǎn)動方向，即

(2)

式中：θ為轉(zhuǎn)動角度；旋轉(zhuǎn)軸的旋轉(zhuǎn)方向為uR，R表示坐標(biāo)系。將求解得到的四元數(shù)代入式(1)可以得到

(3)

2 互補濾波

在俯仰角和橫滾角的測量上，加速度計的靜態(tài)響應(yīng)較好，但是在動態(tài)環(huán)境下會存在不可消除的動態(tài)噪聲，從而降低姿態(tài)解算精度。一般利用磁力計求解航向角，但同樣低頻段響應(yīng)較好，高頻段響應(yīng)存在明顯缺陷。對于MEMS陀螺儀來說，在動態(tài)環(huán)境下仍有較好的性能，但在處理陀螺數(shù)據(jù)時需要進(jìn)行積分運算，會產(chǎn)生累積誤差，故需要設(shè)計一個低通濾波器解決短時間內(nèi)快速變化的噪聲，保留有效信號。同時，還需要一個高通濾波器對累積誤差進(jìn)行處理。因此，結(jié)合兩種濾波器的特點設(shè)計互補濾波器以提高解算精度[7]。利用加速度計和MEMS陀螺儀在各頻率段的特性，設(shè)計由一個低通濾波器和一個高通濾波器組成的互補濾波器，頻率互補，其原理如圖1所示。

圖1 互補濾波原理

圖1中，低通濾波器和高通濾波器的傳遞函數(shù)滿足如下關(guān)系[8]

(4)

式中：G1(S)和G2(S)分別表示低通濾波器和高通濾波器的傳遞函數(shù)；S表示在頻域分析中以虛指數(shù)為基本信號；K1表示低通濾波器調(diào)整權(quán)值；K2表示高通濾波器調(diào)整權(quán)值。

加速度計和電子羅盤得到的姿態(tài)信息包含高頻噪聲，而MEMS陀螺儀的輸出值包含低頻噪聲。傳感器1是由加速度計和磁力計組成得到的姿態(tài)信息，包括航向角、俯仰角和橫滾角，用C1(S)表示，傳感器2是MEMS陀螺儀測量的姿態(tài)角，用C2(S)表示，則真實的姿態(tài)信息C3(S)與C1(S)和C2(S)的關(guān)系可表示為

(5)

式中，ε1和ε2表示噪聲對姿態(tài)信息的影響。

(6)

事實上，互補濾波器中K1和K2的確定直接影響了最終姿態(tài)角的確定。當(dāng)K1和K2越小，加速度計和磁力計測量姿態(tài)信息過程中的高頻噪聲就能很好地抑制，但是不能有效抑制陀螺儀在計算過程中所產(chǎn)生的積分誤差。當(dāng)K1和K2過大時，對陀螺儀產(chǎn)生的累積誤差有很好的抑制效果，但姿態(tài)角隨著加速度計和磁力計的解算速度加快噪聲也會越來越大。

利用差分法得到時域姿態(tài)解算公式為

(7)

3 改進(jìn)自適應(yīng)卡爾曼濾波

經(jīng)典的卡爾曼濾波僅適用于已知噪聲的先驗統(tǒng)計特性的線性控制系統(tǒng)，即系統(tǒng)階數(shù)已知的情況。然而，在實際應(yīng)用中，濾波的先驗性值往往很難確定或者是隨時間不斷變化，對于運動模型的系統(tǒng)噪聲Q和測量噪聲R的獲取十分困難，具有時變性和不確定性，故在復(fù)雜情況下經(jīng)典卡爾曼濾波不能發(fā)揮其作用[9]。因此，能夠自動控制的改進(jìn)自適應(yīng)卡爾曼濾波算法可改善先驗信息不足、模型不夠精確等問題。

Sage-Husa自適應(yīng)卡爾曼濾波算法的實質(zhì)是當(dāng)系統(tǒng)的噪聲協(xié)方差矩陣Q和觀測噪聲協(xié)方差陣R不確定時，濾波器自身可利用測量數(shù)據(jù)所得出的信息不斷地對系統(tǒng)噪聲特性以及濾波器的增益矩陣進(jìn)行估計和修正，也就是在離散卡爾曼的基礎(chǔ)上，計算k時刻的系統(tǒng)干擾均值qk、系統(tǒng)干擾協(xié)方差矩陣Qk、量測噪聲均值rk和量測噪聲協(xié)方差矩陣Rk，并進(jìn)行調(diào)整，從而減少濾波過程中的估計誤差，進(jìn)一步提高濾波精度[10]。

狀態(tài)預(yù)測表示為

(8)

預(yù)測誤差表示為

(9)

濾波增益表示為

(10)

新息估計表示為

(11)

狀態(tài)估計表示為

Xk=Xk|k-1+Kkεk

(12)

誤差估計表示為

Pk=(I-KkHk)Pk|k-1

(13)

(14)

(15)

(16)

(17)

在Sage-Husa自適應(yīng)卡爾曼濾波算法中，bk-1的選擇關(guān)系到濾波是否發(fā)散，會直接影響最終濾波的精度。為此，需要制定一個檢驗標(biāo)準(zhǔn)判斷濾波是否發(fā)散[11]，即

(18)

(19)

又因為有以下關(guān)系

(20)

將式(20)代入式(19)，可得最佳遺忘因子為

(21)

遺忘因子可對濾波器的記憶長度進(jìn)行有效的限制，使新獲得數(shù)據(jù)在估計中發(fā)揮作用，從而加強新觀測數(shù)據(jù)在實時估計中的作用，同時加速對陳舊數(shù)據(jù)的遺忘，使其在實時估計中發(fā)揮較少的作用[12]。UD分解Sage-Husa自適應(yīng)濾波是一種可以提高濾波數(shù)值穩(wěn)定性的有效分解濾波的方法。由于UD分解Sage-Husa自適應(yīng)濾波在實際操作過程中運算量更少，故可在實際場合進(jìn)行應(yīng)用[13]。

由于Pk、Pk|k-1容易出現(xiàn)負(fù)定的情況，因此先對Pk、Pk|k-1進(jìn)行UD分解，得到

(22)

令矩陣

則式(22)可以表示為

(23)

(24)

(25)

利用上述方法對Pk-1和Pk進(jìn)行UD分解，得Pk|k-1的UD分解表達(dá)式為

(26)

(27)

再進(jìn)行UD分解，得到關(guān)系式

(28)

通過式(28)得到Pk的UD分解，令

代入式(28)，并對V進(jìn)行UD分解，得到關(guān)系式

(29)

從而得到了Pk的UD分解更新表達(dá)式為

(30)

4 融合算法

通過四元數(shù)法可以求解并表示空間中的載體姿態(tài)，如橫滾角、航向角及橫滾角信息等，但仍然會存在制造工藝、環(huán)境溫度以及積分過程中存在的累計積分誤差等問題。為了提高姿態(tài)解算的精度，將互補濾波和改進(jìn)自適應(yīng)卡爾曼姿態(tài)解算算法進(jìn)行融合，進(jìn)一步提高姿態(tài)解算精度。在自適應(yīng)卡爾曼濾的遞推步驟中，利用UD分解避免了計算過程中出現(xiàn)負(fù)定的情況，抑制了濾波發(fā)散。在實際的應(yīng)用中，利用改進(jìn)自適應(yīng)卡爾曼濾波進(jìn)行姿態(tài)解算更新前需要輸入觀測值。在對觀測值進(jìn)行測量時，利用互補濾波器進(jìn)行數(shù)據(jù)融合，避免加速度計和電子羅盤在動態(tài)、高頻率情況下的噪聲影響，將最終得到姿態(tài)角帶入四元數(shù)微分方程和更加精確的四元數(shù)，從而得到更加精確的姿態(tài)角[15]?；诨パa濾波和改進(jìn)自適應(yīng)卡爾曼的姿態(tài)解算融合算法進(jìn)行姿態(tài)解算過程如圖2所示。

圖2 融合算法姿態(tài)解算過程

5 實驗及結(jié)果分析

采用型號為HX-INS-12的MEMS慣性測量單元進(jìn)行跑車實驗?；鶞?zhǔn)采用一個高精度激光慣導(dǎo)系統(tǒng)，姿態(tài)角數(shù)據(jù)更新周期為30 ms，航向角精度0.04°，姿態(tài)精度0.01°，對準(zhǔn)時間不超過6 min，MEMS陀螺儀的零偏值為10°/h，加速度計的零偏值為15 mg。將高精度激光慣導(dǎo)系統(tǒng)和電子羅盤固定在車輛中，連接好天線等其他設(shè)備。車輛開動后，進(jìn)行40 min的數(shù)據(jù)采集，得到三軸陀螺的數(shù)據(jù)、三軸加速度傳感器的數(shù)據(jù)以及磁力計的數(shù)據(jù)，將數(shù)據(jù)通過互補濾波器得到互補濾波的觀測值，最后通過改進(jìn)自適應(yīng)卡爾曼濾波得到最終精確的偏航角、俯仰角和橫滾角。

分別利用互補濾波算法、改進(jìn)自適應(yīng)卡爾曼濾波和所提融合算法對采集到的數(shù)據(jù)進(jìn)行姿態(tài)解算。在600 s內(nèi)，利用上述3種算法得到的橫滾角、俯仰角和航向角分別如圖3、圖4和圖5所示。

圖3 橫滾角

圖4 俯仰角

圖5 航向角

對比分析3種算法解算得到的橫滾角、俯仰角以及航向角與實際橫滾角、俯仰角和航向角，誤差對比曲線分別如圖6、圖7和圖8所示。

圖6 橫滾角誤差曲線

圖7 俯仰角誤差曲線

圖8 航向角誤差曲線

從圖6—圖8中可以看出，利用所提融合算法、互補濾波算法以及改進(jìn)自適應(yīng)卡爾曼濾波進(jìn)行姿態(tài)解算，得到的橫滾角誤差分別為2.2°、2.7°和3.5°，俯仰角誤差分別為1.8°、2.5°和3.7°，航向角誤差分別為1.7°、2.7°、3.4°。較互補濾波算法和改進(jìn)自適應(yīng)卡爾曼濾波算法，所提融合算法在橫滾角誤差上分別降低了18.5%和37.1%，在俯仰角誤差上分別降低了28.0%和51.4%，在航向角誤差上分別降低了37.0%和50.0%。

6 結(jié)語

基于互補濾波和改進(jìn)自適應(yīng)卡爾曼的姿態(tài)解算融合算法，通過低通濾波器和高通濾波器相結(jié)合的方法，對短時間內(nèi)快速變化的噪聲進(jìn)行過濾，并對累積誤差進(jìn)行處理。改進(jìn)自適應(yīng)卡爾曼濾波算法是在傳統(tǒng)的Sage-Husa自適應(yīng)卡爾曼濾波的基礎(chǔ)之上與UD分解進(jìn)行結(jié)合，保障了量測噪聲、預(yù)測誤差協(xié)方差陣以及狀態(tài)估計協(xié)方差矩陣的正定性，對濾波發(fā)散具有明顯的抑制作用。實驗結(jié)果表明，互補濾波算法和改進(jìn)自適應(yīng)卡爾曼濾波算法的姿態(tài)角解算精度低，且收斂速度較慢，在動態(tài)實驗中還容易出現(xiàn)發(fā)散的情況，而所提融合算法結(jié)構(gòu)較為簡單，收斂速度快，能夠有效抑制濾波發(fā)散，保證較高的姿態(tài)解算精度。