融合互補濾波和卡爾曼濾波的姿態解算算法

劉 穎，楊 媛，徐金濤

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

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

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

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

1 載體姿態描述與更新

(1)

式中：θ、ψ和φ分別表示繞坐標軸yb轉過的角度、繞坐標軸zb轉過的角度以及繞坐標軸xb轉過的角度；q0、q1、q2和q3表示當前時刻的四元數參數值。

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

(2)

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

(3)

2 互補濾波

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

圖1 互補濾波原理

圖1中，低通濾波器和高通濾波器的傳遞函數滿足如下關系[8]

(4)

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

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

(5)

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

(6)

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

利用差分法得到時域姿態解算公式為

(7)

3 改進自適應卡爾曼濾波

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

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

狀態預測表示為

(8)

預測誤差表示為

(9)

濾波增益表示為

(10)

新息估計表示為

(11)

狀態估計表示為

Xk=Xk|k-1+Kkεk

(12)

誤差估計表示為

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

(13)

(14)

(15)

(16)

(17)

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

(18)

(19)

又因為有以下關系

(20)

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

(21)

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

由于Pk、Pk|k-1容易出現負定的情況，因此先對Pk、Pk|k-1進行UD分解，得到

(22)

令矩陣

則式(22)可以表示為

(23)

(24)

(25)

利用上述方法對Pk-1和Pk進行UD分解，得Pk|k-1的UD分解表達式為

(26)

(27)

再進行UD分解，得到關系式

(28)

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

代入式(28)，并對V進行UD分解，得到關系式

(29)

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

(30)

4 融合算法

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

圖2 融合算法姿態解算過程

5 實驗及結果分析

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

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

圖3 橫滾角

圖4 俯仰角

圖5 航向角

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

圖6 橫滾角誤差曲線

圖7 俯仰角誤差曲線

圖8 航向角誤差曲線

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

6 結語

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