基于卡爾曼濾波和互補(bǔ)濾波的AHRS系統(tǒng)研究

蔡陽 胡杰

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

關(guān)鍵詞：AHRS;MEMS;姿態(tài)解算;卡爾曼濾波;互補(bǔ)濾波

中國分類號：TP301? 文獻(xiàn)標(biāo)識碼：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

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

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

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

AHRS結(jié)合MEMS傳感器各自優(yōu)點(diǎn)，通過融合濾波算法，從而能夠得出精度較高的航姿姿態(tài)角。

1 互補(bǔ)濾波

1.1 互補(bǔ)濾波原理

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

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

1.2 四元數(shù)

四元數(shù)基本表示形式為：

當(dāng)姿態(tài)角發(fā)生變化時，相當(dāng)于載體坐標(biāo)系分別繞地理坐標(biāo)系X，Y，Z進(jìn)行旋轉(zhuǎn)，得到

四元數(shù)的載體坐標(biāo)系b旋轉(zhuǎn)到地理坐標(biāo)系n的矩陣為[Cnb]為：

由公式（1）（2）（3）可以得出，歐拉角與四元數(shù)之間的關(guān)系為：

2 卡爾曼濾波

在AHRS系統(tǒng)中通常采用卡爾曼濾波，對原始傳感器數(shù)據(jù)進(jìn)行建模，通過過程的預(yù)測更新，進(jìn)而對下一個時刻的姿態(tài)角進(jìn)行預(yù)測[9]。

狀態(tài)方程（5）和測量方程（6）的建立，其中：

狀態(tài)方程：

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

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

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

3 卡爾曼濾波融合互補(bǔ)濾波

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

4 仿真實(shí)驗(yàn)與分析

本實(shí)驗(yàn)采用MPU9255MEMS九軸傳感器原始數(shù)據(jù)，對融合濾波進(jìn)行驗(yàn)證，將傳感器采集的數(shù)據(jù)加速計和陀螺數(shù)據(jù)經(jīng)過卡爾曼濾波后，傳遞給互補(bǔ)濾波器結(jié)合磁力計輸出姿態(tài)角。

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

實(shí)驗(yàn)結(jié)果圖3，圖4中的輸出表明，單一的卡爾曼濾波或者單一的互補(bǔ)濾波在輸出姿態(tài)角時仍存在不穩(wěn)定的因素，得到的角度存在波動。相比較而言融合濾波輸出姿態(tài)角精度更高，在圖形中波形更為平穩(wěn)。

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

5 結(jié)語

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

參考文獻(xiàn)：

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

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

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

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

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

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

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

[8] 王丹.卡爾曼濾波在動態(tài)傾角傳感器測量中的應(yīng)用研究[D].北京：北京信息科技大學(xué)，2013.

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

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

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

【通聯(lián)編輯：梁書】