基于卡爾曼濾波和互補濾波的AHRS系統研究

蔡陽 胡杰

摘要：AHRS航姿參考系統中通常需要融合MEMS傳感器數據來進行姿態解算，由于MEMS傳感器自身的一些缺陷導致在姿態解算中會出現較為嚴重的誤差。AHRS中常見對加速度計、陀螺儀和磁力計進行卡爾曼濾波、互補濾波的方法，由于使用單一的濾波算法時會出現誤差，導致姿態角解算精度不高。本文采用卡爾曼濾波融合互補濾波的濾波算法，通過卡爾曼濾波對加速度計和陀螺儀起抑制漂移作用，進而得到最優估計姿態角，減小傳感器引起的誤差，再由估計值和磁力計經過互補濾波濾除噪聲，提高姿態角的解算精度。仿真實驗表明：融合濾波算法可以抑制漂移和濾除噪聲，在靜態和動態條件下，都有良好表現。

關鍵詞：AHRS;MEMS;姿態解算;卡爾曼濾波;互補濾波

中國分類號：TP301? 文獻標識碼：A

文章編號：1009-3044（2021）10-0230-03

Abstract： AHRS heading and attitude reference system usually needs to fuse MEMS sensor data for attitude calculation. Due to some defects of MEMS sensor itself， there will be more serious errors in attitude calculation. Kalman filtering and complementary filtering methods for accelerometers， gyroscopes， and magnetometers are common in AHRS. Due to errors when a single filtering algorithm is used， the accuracy of the attitude angle calculation is not high. In this paper， the Kalman filter fusion complementary filter filter algorithm is used to suppress drift of the accelerometer and gyroscope through Kalman filter， and then obtain the optimal estimated attitude angle， reduce the error caused by the sensor， and then pass the estimated value and the magnetometer. Complementary filtering filters out noise and improves the accuracy of attitude angle calculation. Simulation experiments show that the fusion filtering algorithm can suppress drift and filter noise， and it performs well under static and dynamic conditions.

Keywords： AHRS; MEMS;attitude calculation; Kalman filter;complementary filter

航姿參考系統AHRS（Attitude and Heading Reference System）由MEMS（Micro-Electro Mechanical System）慣性傳感器三軸陀螺儀、三軸加速度計和磁力計的數據融合來進行姿態解算[1]。在進行姿態解算過程中，由于MEMS器件固有的一些缺陷，陀螺儀測量角度時使用積分，會存在積分誤差，而且當陀螺儀靜止不動時，也會產生漂移，加速度計會受到重力和振動的影響，磁力計會受到地理環境和自身環境干擾，導致進行姿態角解算時精度較低。

為了能夠準確測量姿態角，提高精度，通常會將不同的傳感器進行數據融合，并采用卡爾曼濾波算法和互補濾波算法以及以此為基礎的衍生算法。文獻[2]提出基于卡爾曼濾波的進行融合算法解算姿態角，采用時間序列建模，根據量測方程和狀態方程對數據進行濾波處理，可是誤差會在量測方程線性是引入;文獻[3]提出基于互補濾波對加速度計和陀螺儀進行濾波處理，并且采用量子粒子濾波為濾波算法作參數尋優;文獻[4]提出廣義互補濾波在AHRS中的姿態估計，通過分析乘性誤差，將加入運動加速度的補償，來提高姿態角解算精度;文獻[5]中提出改進型顯性互補濾波，采用巴特沃斯低通濾波和設置緩存區，進而減少加速度計姿態角解算的影響。

本文采用兩種濾波方式分別為卡爾曼濾波、互補濾波，結合兩者特性將其融合。先通過卡爾曼濾波抑制原始數據中的漂移等因素，對陀螺儀，加速計的輸出數據進行融合，從而得到的姿態角，作為系統狀態的最優估算。再利用互補濾波特性，將kalman濾波得到的姿態角與磁力計數據進行融合，并做姿態角的解算，能夠提高姿態角解算精度。

AHRS結合MEMS傳感器各自優點，通過融合濾波算法，從而能夠得出精度較高的航姿姿態角。

1 互補濾波

1.1 互補濾波原理

針對MEMS慣性傳感器件中，加速計由于受重力影響和瞬時誤差加大，但是具有良好的低頻特性，而陀螺儀因計算角度過程中，不斷積分或積累誤差，然而有良好的高頻特性。

互補濾波針對加速計低頻特性，濾去高頻信號和陀螺儀高頻特性，濾除低頻信號，通過陀螺儀的數據進行積分得出姿態角的最優估計值，再對加速計數據得出姿態來進行校正姿態角[6]。將陀螺儀、加速計的原始數據融合并結合磁力計輸出，從而在高、低頻段都能夠得到準確的姿態角，互補濾波工作原理如圖1：

1.2 四元數

四元數基本表示形式為：

當姿態角發生變化時，相當于載體坐標系分別繞地理坐標系X，Y，Z進行旋轉，得到

四元數的載體坐標系b旋轉到地理坐標系n的矩陣為[Cnb]為：

由公式（1）（2）（3）可以得出，歐拉角與四元數之間的關系為：

2 卡爾曼濾波

在AHRS系統中通常采用卡爾曼濾波，對原始傳感器數據進行建模，通過過程的預測更新，進而對下一個時刻的姿態角進行預測[9]。

狀態方程（5）和測量方程（6）的建立，其中：

狀態方程：

本文中以陀螺儀的角速度ω和靜態漂移?作為輸入量，來建立量測方程：

公式中：[θk]為k時刻傳感器角度、ωk-1為k-1時刻的陀螺輸出角速度、?k-1為k-1時陀螺儀靜態的漂移、[Wk-1]為量測的噪聲，[dt]是采樣的時間[8]。

公式中：ω為陀螺儀輸出;T為采樣時間;[Zk]為加速計量測角度;[Vk]為系統測量的噪聲。

3 卡爾曼濾波融合互補濾波

卡爾曼濾波通過線性狀態方程和遞歸運算，結合輸入輸出觀測數據，估算最優角度，可以解決單一使用互補濾波時，傳感器帶的漂移問題。互補濾波由于運算量不大，并且相對簡單，可以過濾噪聲，通過卡爾曼濾波預估的角度值，結合磁力計和四元數，可得出準確的姿態角[10-11]。設計圖如圖2所示.

4 仿真實驗與分析

本實驗采用MPU9255MEMS九軸傳感器原始數據，對融合濾波進行驗證，將傳感器采集的數據加速計和陀螺數據經過卡爾曼濾波后，傳遞給互補濾波器結合磁力計輸出姿態角。

在仿真軟件MATLAB中，對融合算法進行編程設計，并對MEMS傳感器數據進行融合濾波分析對比濾波效果。

實驗結果圖3，圖4中的輸出表明，單一的卡爾曼濾波或者單一的互補濾波在輸出姿態角時仍存在不穩定的因素，得到的角度存在波動。相比較而言融合濾波輸出姿態角精度更高，在圖形中波形更為平穩。

在動態測量過程中，融合濾波輸出姿態角波動較小，圖形收斂速度較快;在靜態濾波處理過程中，融合濾波能有效抑制漂移，過濾噪聲。相比單一濾波算法有一定的提升，同樣能夠提高姿態角的測量精度，這對于AHRS系統的穩定性，也有相應的提升。

5 結語

本文采用對AHRS中的MEMS器件進行卡爾曼濾波與互補濾波的融合濾波，通過卡爾曼濾波抑制漂移和對互補濾波過濾噪聲進行融合，可以提高姿態角解算精度，從而為AHRS系統姿態解算提升了性能。

參考文獻：

[1] 劉浩，楊薇秀，焦勝海，等.基于AHRS算法的小型無人機導航精度改進方法[J].中國電子科學研究院學報，2020，15（5）：461-469.

[2] 劉浩宇，李奇.卡爾曼濾波算法在AHRS姿態角解算中的應用[J].工業控制計算機，2018，31（6）：69-71.

[3] 史露強，何怡剛，羅旗舞，等.基于傳感器數據融合的傾斜角度測量方法研究[J].儀器儀表學報，2017，38（7）：1683-1689.

[4] 王勇軍，李智，李翔.小型無人飛行器機動過程中航姿互補濾波算法研究[J].電子測量與儀器學報，2020，34（7）：141-150.

[5] 付雷，章政，余義.基于改進型顯性互補濾波的MEMS姿態解算[J].自動化與儀表，2018，33（11）：7-12.

[6] 李二闖，張建杰，袁亮，等.基于四元數互補濾波的小型四旋翼姿態解算[J].組合機床與自動化加工技術，2019（3）：37-39，43.

[7] 張波，李杰，劉偉，等.基于卡爾曼濾波的動態角度測量系統設計[J].電子器件，2018，41（2）：375-379.

[8] 王丹.卡爾曼濾波在動態傾角傳感器測量中的應用研究[D].北京：北京信息科技大學，2013.

[9] 張棟，焦嵩鳴，劉延泉.互補濾波和卡爾曼濾波的融合姿態解算方法[J].傳感器與微系統，2017，36（3）：62-65，69.

[10] 李慶黨，張義龍.基于卡爾曼濾波和互補濾波的改進型姿態解算方法[J].青島科技大學學報（自然科學版），2019，40（6）：85-89，98.

[11] 黃鎮，張浩磊，劉梅，等.一種二階互補濾波與卡爾曼濾波的姿態解算方法設計[J].電子工藝技術，2018，39（3）：168-170，178.

【通聯編輯：梁書】