基于MAEKF算法的航姿參考系統(tǒng)設計

王丁偉，祖家奎*，黃 海

(1.南京航空航天大學自動化學院，南京210016;2.中創(chuàng)航空技術有限公司，浙江嘉興314000)

基于MAEKF算法的航姿參考系統(tǒng)設計

王丁偉1，祖家奎1*，黃 海2

(1.南京航空航天大學自動化學院，南京210016;2.中創(chuàng)航空技術有限公司，浙江嘉興314000)

構建了以低成本MEMS陀螺儀、加速度計和磁傳感器組合的航姿參考系統(tǒng)，提出了一個乘性自適應擴展卡爾曼濾波算法。取乘性誤差四元數(shù)和陀螺儀誤差作為狀態(tài)量，基于重力場和磁場構造了量測矢量，用于修正航姿數(shù)據(jù)。并采用準確量測法，給濾波器加入了四元數(shù)的歸一化約束，最后給出了基于新息的估計量測噪聲方差矩陣的公式。通過仿真和試飛驗證，表明本文設計的低成本的航姿參考系統(tǒng)能夠提供比較準確的航姿信息。與常規(guī)的擴展卡爾曼濾波器比較，本文設計的乘性自適應擴展卡爾曼濾波算法有效提高了系統(tǒng)的精度和穩(wěn)定性，并且具有較好的魯棒性。

航姿參考;乘性擴展卡爾曼濾波;自適應;誤差四元數(shù)

航姿參考系統(tǒng)能夠準確測量載體的空間姿態(tài)信息。在三維空間中，通常采用俯仰角、滾轉角、航向角來描述其姿態(tài)信息。在軍事、航空航天、航海等領域多采用高精度的慣性測量元件，但其價格昂貴。隨著價格低廉的MEMS慣性傳感器的發(fā)展，低成本的航姿參考系統(tǒng)廣泛應用于消費類的無人機、室內(nèi)導航、智能穿戴設備、游戲手柄等領域。低成本的航姿參考系統(tǒng)一般由MEMS陀螺儀、加速度計、磁傳感器組成［1］。但是，MEMS陀螺儀存在嚴重的零點漂移、隨機誤差，直接對原始數(shù)據(jù)進行積分得到的角度會產(chǎn)生很大的累積誤差，并且該誤差會隨著時間不斷增大［2］。因此，輔以加速度計和磁傳感器測量的重力場、地磁場來補償MEMS陀螺儀的誤差能夠顯著提高航姿數(shù)據(jù)精度［3］。

目前廣泛采用的航姿融合算法主要分為互補濾波、擴展卡爾曼濾波兩大類。從融合效果看，互補濾波的精度不高，且動態(tài)響應速度不如卡爾曼濾波迅速［4］。文獻［1，5］分別提出各自的互補濾波算法，前者采用PI調(diào)節(jié)器，先使用加速度計數(shù)據(jù)補償陀螺儀數(shù)據(jù)，然后再融合磁場數(shù)據(jù);后者采用向量叉乘解算陀螺儀誤差，并且能夠直接運用于未校準的傳感器，能夠獲取相對較好的姿態(tài)數(shù)據(jù)。但是兩者的融合精度都不是很好。文獻［4，6］設計了基于四元數(shù)的擴展卡爾曼濾波器，前者著重介紹了使用MPU9250作為MEMS九軸姿態(tài)傳感器，實現(xiàn)EKF算法;后者先用重力場補償陀螺儀誤差，再用磁場修正偏航角，接著構造了自適應的量測噪聲方差矩陣，但是其本質(zhì)仍然還是加性擴展卡爾曼濾波器，脫離了四元數(shù)歸一化的約束，濾波器極易發(fā)散。而乘性擴展卡爾曼濾波器能夠較好的保持四元數(shù)的歸一化約束，文獻［2，7］以乘性誤差四元數(shù)作為狀態(tài)量，設計了乘性擴展卡爾曼濾波器。其中，文獻［2］僅僅采用磁場信息修正陀螺儀誤差，這樣的確避免了振動和載體運動導致的干擾，但是，磁力計易受到外部磁場干擾，導致姿態(tài)數(shù)據(jù)錯誤;文獻［7－8］則是加入了加速度計，但是并未給出如何設計量測噪聲方差矩陣。

針對上述眾多問題，本文設計了一種可靠的航姿參考系統(tǒng)，該系統(tǒng)采用MAEKF算法進行航姿融合，適用于量測噪聲多變的復雜環(huán)境，并且通過仿真和試飛測試驗證了該航姿參考系統(tǒng)的可靠性。

1 系統(tǒng)結構

本文將MEMS陀螺儀、加速度計、磁傳感器的數(shù)據(jù)作為MAEKF的輸入。其中，加速度計的原始數(shù)據(jù)需要經(jīng)過低通濾波器濾除高頻噪聲和震動干擾。本文設計的航姿參考系統(tǒng)結構和實物圖，如圖1所示。

圖1 航姿參考系統(tǒng)結構圖和實物圖

三軸陀螺儀數(shù)據(jù)驅(qū)動誤差四元數(shù)的更新，通過加速度計和磁傳感器所測得的重力場和磁場來修正，從而得到誤差四元數(shù)的最優(yōu)估計。用該誤差四元數(shù)修正陀螺儀數(shù)據(jù)積分的四元數(shù)，再通過姿態(tài)解算，得到精度較高的姿態(tài)角數(shù)據(jù)。該系統(tǒng)采用MPU6050和HMC5803作為姿態(tài)傳感器，STM32F429作為核心處理器。

2 航姿融合算法設計

2.1 乘性誤差四元數(shù)

姿態(tài)解算算法中，有多種參數(shù)化方法，比如歐拉角、四元數(shù)、改進的Rodrigues參數(shù)、等效旋轉矢量等［9］。由于四元數(shù)的非奇異性、計算量小等優(yōu)點，使其廣泛應用于姿態(tài)解算算法中。四元數(shù)必須滿足歸一化約束，然而標準EKF方法釋放了四元數(shù)的歸一化約束。為了克服這種缺陷，本文采用乘性誤差四元數(shù)方法。

對式(1)兩邊求微分得到:

由四元數(shù)運動學方程可得:

綜上可得誤差四元數(shù)運動學方程:

2.2 系統(tǒng)狀態(tài)方程設計

2.2.1 MEMS陀螺儀建模

系統(tǒng)狀態(tài)方程主要描述了陀螺儀輸出的角速率值和誤差四元數(shù)之間的動力學關系。因此，設計狀態(tài)方程之前，首先要完成MEMS陀螺儀的建模。

陀螺儀的誤差主要來源于隨機漂移和噪聲，一種廣泛采用的數(shù)學模型是一階馬爾可夫過程，其主要形式如下:

式中:ηv和 ηu分別表示具有譜密度σ2vI3×3和 σ2uI3×3的零均值高斯白噪聲過程;β為偏置向量;珟ω為測量觀測值［9］。

估計的角速度為

2.2.2 建立狀態(tài)方程

式中:

2.3 量測方程設計

2.3.1 姿態(tài)誤差的測量

EKF中的量測方程能夠起到修正狀態(tài)預測值的作用，在許多關于AHRS的研究中，狀態(tài)方程的建立大多不盡相同，主要區(qū)別在于量測方程的量測量的選取中。本文選用符號p和m表示狀態(tài)量的預測值和量測值，e表示預測值和量測值的外積，θe表示兩個向量的內(nèi)積，則

式中:向量p和m均為單位向量。

通過式(12)、式(13)構建姿態(tài)誤差向量:

乘性誤差四元數(shù)與姿態(tài)誤差向量之間的關系為［7］:

本文選用了加速度計和磁傳感器作為量測的傳感器。接下來，分別對兩個傳感器設計量測方程。

2.3.2 加速度計的量測設計

加速度計的輸出主要包含重力加速度、運動加速度以及噪聲，可用如下形式表達:

式中:am表示加速度計輸出，Cbng表示重力向量在機體軸下的投影，表示運動加速度，b表示加速度計的噪聲。

通過重力加速度在機體軸下的分量可以計算出，當前載體的滾轉角和俯仰角。因此，當載體的運動加速度較小時，式(16)可以近似為:

式中:gb表示重力加速度在機體軸下的投影。

顯然，gb=Cbng是成立的。其中:

由式(12)、式(13)得:

式中:am需要歸一化處理。

2.3.3 磁傳感器的量測設計

磁傳感器測量的是三軸的磁場強度，其輸出形式如下:

式中:mm表示磁傳感器的輸出，m0表示當?shù)氐牡卮艔姸?，b表示磁傳感器的噪聲。

同理，由式(12)、式(13)得:

2.3.4 準確量測法的狀態(tài)約束

由于乘性誤差四元數(shù)是嚴格歸一化的。因此，在濾波過程中，必須滿足歸一化約束才能有效的防止濾波器的發(fā)散［10］。其歸一化方程為:

將式(23)作為狀態(tài)量的偽量測方程，引入到系統(tǒng)的量測中，構成系統(tǒng)的量測方程。

2.3.5 建立量測方程

由式(15)、式(23)可知，量測方程中包含有量測量和狀態(tài)約束兩個部分。系統(tǒng)的量測方程為:

式中:

2.4MEKF算法

基于MEKF的姿態(tài)融合算法區(qū)別于AEKF(加性擴展卡爾曼濾波)的地方在于:AEKF直接選用姿態(tài)四元數(shù)作為濾波器的狀態(tài)量，姿態(tài)誤差采用加性修正，這種加性修正很容易破壞姿態(tài)四元數(shù)的歸一化約束，降低濾波精度，更嚴重可能導致濾波器發(fā)散［9，12－13］。而MEKF雖然在形式上比AEKF復雜一點，但是，卻能夠很好的保持姿態(tài)四元數(shù)的歸一化約束。

狀態(tài)四元數(shù)傳播方程:

狀態(tài)預測方程:

協(xié)方差預測方程:

卡爾曼增益方程:

信息方程:

狀態(tài)更新:

協(xié)方差更新方程:

狀態(tài)量歸一化:

姿態(tài)四元數(shù)修正:

姿態(tài)四元數(shù)歸一化

注意:MEKF迭代過程中，每次姿態(tài)四元數(shù)修正結束后，誤差四元數(shù)需要重置為初始值。

3 航姿融合算法實現(xiàn)

本文構建了兩組量測向量，Murrell對這種濾波器進行了改進［9］。該算法每次迭代過程僅需要處理一組3×1的量測向量。對于本文中設計的算法，一次完整的修正過程需要連續(xù)處理兩組量測向量。

從圖2可以看出，當系統(tǒng)中設計了n個量測向量，該算法采用序貫方式一個一個處理量測向量。這樣處理的優(yōu)點在于:降低了量測方程的維度，在進行增益解算時，矩陣逆的計算復雜度大大降低，能夠有效提高算法實時性。另外，由于不同傳感器的采樣頻率不同，因此這種序貫處理方式使得只有傳感器采樣到新值時才進行量測更新，避免了舊值的重復計算。

4 自適應量測噪聲R陣設計

卡爾曼濾波器的公式中主要有兩個噪聲矩陣，分別是系統(tǒng)噪聲矩陣Q和量測噪聲矩陣R。就系統(tǒng)噪聲而言，其決定于系統(tǒng)內(nèi)部機理，噪聲參數(shù)相對穩(wěn)定，可以通過大量實驗來實現(xiàn)精確測試或建模;但是量測噪聲不僅僅來源于MEMS傳感器的誤差和噪聲，還受到外部環(huán)境以及載體運動狀態(tài)等情況影響［14］。因此量測噪聲是比較容易變化的，有太多的不確定性，難以建模。因此，需要算法對量測噪聲進行實時的自適應估計，這樣才能保證濾波器的精度，防止濾波器發(fā)散。

Sage-Husa自適應擴展卡爾曼濾波算法(SHAEKF)在理論上實現(xiàn)了在進行狀態(tài)估計的同時還可在線自適應估計系統(tǒng)噪聲和量測噪聲統(tǒng)計特性的功能。其Q、R陣的自適應算法如下:

在實際運用中，SHAEKF算法所估計出來的系統(tǒng)噪聲和量測噪聲方差序列分別與真實噪聲方差存在差值誤差，特別是系統(tǒng)噪聲的估計偏離更大，極易導致對量測值的權重過高，等效于狀態(tài)方程失效，大大降低了濾波精度，甚至會導致濾波器發(fā)散［14］。

本文采用改進型的SHAEKF算法估計R陣，其公式如下:

式中:b為漸消因子，一般取b=0.9～0.999。

5 仿真與試飛驗證

5.1 仿真條件

本文使用MATLAB對MIMU的源數(shù)據(jù)進行處理。MIMU傳感器由MPU6050和HMC5803組合而成，處理器選用了意法半導體(ST)公司Cortex－M4架構的STM32F429(圖1)。設置MPU6050采樣頻率500 Hz，HMC5803為75 Hz，數(shù)據(jù)上傳頻率為100 Hz。因此，在MATLAB下仿真時，算法解算周期為0.01 s。

5.2 仿真結果

根據(jù)上述條件，對MIMU靜置1 h，在MATLAB下對采集到的數(shù)據(jù)進行了MAEKF算法和傳統(tǒng)EKF算法的姿態(tài)解算，取中間50 s數(shù)據(jù)，如圖3和圖4所示。

圖3 俯仰角靜態(tài)圖

圖中，橫軸為時間軸，單位是s，縱軸表示航姿數(shù)據(jù)，單位是(°)。從圖中可以看出，本文設計的MAEKF算法的靜態(tài)精度比傳統(tǒng)的EKF算法精度要高。MAEKF算法的俯仰角和滾轉角靜態(tài)精度分別達到了0.016°和0.013°。

仿真中，對量測噪聲方差矩陣R取了二范數(shù)，根據(jù)本文的自適應算法，其曲線如圖5所示。圖5中R陣初始值給的是單位矩陣，最終，其二范數(shù)收斂至0.166 7。

圖4 滾轉角靜態(tài)圖

圖5 R陣估計結果圖

5.3 試飛驗證與分析

試飛驗證的飛行器為一個三軸六槳飛行器，如圖6所示。該飛行器搭載有本文設計的AHRS傳感器，該傳感器中同時運行了標準擴展卡爾曼濾波算法(EKF)和本文設計的乘性自適應擴展卡爾曼濾波算法(MAEKF)，通過uCOS－Ⅱ來調(diào)度，算法解算頻率均為100 Hz。另外，飛行器上還搭載了一款參考IMU，型號為IMU520，能夠提供精確的滾轉角和俯仰角。

圖6 飛行器實物圖

如圖7、圖8給出參考IMU的輸出、EKF算法的輸出、文中設計的MAEKF算法的輸出曲線。

圖7 試飛俯仰角曲線圖

圖8 試飛滾轉角曲線圖

圖9給出了兩種算法的誤差對比曲線圖。本文設計的航姿融合算法一般的擴展卡爾曼濾波算法精度要高很多，能夠較好的適應不確定的量測噪聲。

圖9 MAEKF與EKF誤差比較曲線

表1中統(tǒng)計了兩種算法角度誤差的最大值、平均值和標準差。與EKF算法相比，MAEKF算法產(chǎn)生的最大誤差和標準差更小，較小的最大誤差說明了該算法的具有更好的精度，而較小的標準差則說明了在具有量測擾動的情況下，該算法更加穩(wěn)定，具有更好的魯棒性。

圖10給出了飛行過程中，飛行器機體軸3個軸向的加速度信息。可以看出，飛行過程中有無刷電機導致的高頻振動。常規(guī)的EKF算法直接采用姿態(tài)四元數(shù)作為狀態(tài)量，且采用常值R陣，不能適應噪聲多變的飛行環(huán)境;而本文設計的MAEKF算法，采用自適應R陣，根據(jù)新息估計量測噪聲的方差，同時，采用了直接觀測法引入了四元數(shù)的歸一化約束，提高了航姿融合算法的濾波精度和魯棒性。

表1 EKF和MAEKF比較

圖10 試飛加速度曲線圖

6 結論

本文從實際應用角度出發(fā)，設計了一個低成本航姿參考系統(tǒng)。其中包括MAEKF算法，該算法通過采用約束濾波和自適應的量測噪聲矩陣，與常規(guī)的EKF算法比較，仿真與試飛驗證的結果表明，MAEKF算法不僅有更高的精度，而且對不確定的量測噪聲具有更好的魯棒性。在航姿參考應用領域具有很好的應用前景。

［1］ 杜彬彬，吳昊，張繼文，等.一種面向AHRS的改進互補濾波融合算法［J］.國外電子測量技術，2015，34(3):13－18.

［2］ 薛亮，李天志，李曉瑩，等.基于MEMS傳感器的微型姿態(tài)確定系統(tǒng)研究［J］.傳感技術學報，2008，21(3):457－460.

［3］ 趙代弟.MARG姿態(tài)測量系統(tǒng)抗干擾算法研究［D］.2015.

［4］ 劉春陽，徐軍領，程洪濤，等.MPU9250傳感器的姿態(tài)檢測與數(shù)據(jù)融合［J］.河南科技大學學報，2015，36(4):14－17.

［5］ Hamel T，Mahony R.Attitude Estimation on SO(3)Based onDirect Inertial Measurements［J］.IEEE International Conference on Robotics and Automation，2006:2170－2175.

［6］ 劉興川，張盛，李麗哲，等.基于四元數(shù)的MARG傳感器姿態(tài)測量算法［J］.清華大學學報，2012，52(5):627－631.

［7］ Mohammad A，Alireza K，Paknush K.Orientation Estimation by Error-State Extended Kalman Filter in Quaternion vector space［J］.SICE，2007 Annual Conference，2007:60－67.

［8］ 葉锃鋒，馮恩信.基于四元數(shù)和卡爾曼濾波的兩輪姿態(tài)穩(wěn)定方法［J］.傳感技術學報，2012，25(4):524－528.

［9］ John L Crassidis，John L Junkins.動態(tài)系統(tǒng)最優(yōu)估計［M］.2版.北京:國防工業(yè)出版社，2016，1:357－365.

［10］薛亮，姜澄宇，常洪龍，等.基于狀態(tài)約束的MIMU/磁強計組合姿態(tài)估計濾波算法［J］.中國慣性技術學報，2009，17(3): 338－343.

［11］ James K Hall，Nathan B Knoebel，Timothy W McLain.Quaternion Attitude Estimation for Miniature Air Vehicles Using a Multiplicative Extended Kalman Filter［J］.IEEE Position，Location and Navigation Symposium，2008:1230－1237.

［12］李翔.基于MARG傳感器的AHRS關鍵技術研究［D］.2013.

［13］李建國，催祜濤，田陽，等.基于乘性四元數(shù)和約束濾波的飛行器姿態(tài)估計算法［J］.系統(tǒng)工程與電子技術，2013，35(5): 1031－1036.

［14］張欣.多旋翼無人機的姿態(tài)與導航信息融合算法研究［D］.2015.

王丁偉(1991－)，男，江蘇揚州人，碩士研究生，主要研究方向為無人機飛行控制，812724408@qq.com;

祖家奎(1974)，男，江蘇徐州人，副研究員，主要從事無人直升機飛行控制技術、多變量控制技術及其應用、系統(tǒng)仿真與建模技術、嵌入式系統(tǒng)開發(fā)等方向，zujiakui@nuaa.edu.cn。

The Design of AHRS Based on MAEKF

WANG Dingwei1，ZU Jiakui1*，HUANG Hai2
(1.College of Automation Engineering，Nanjing University of Aeronautics and Astronautics，Nanjing 210016，China; 2.ZhongChuang Aviation Technology Co.Ltd，Jiaxing Zhejiang 314000，China)

An attitude and heading reference system(AHRS)is designed which is composed of low cost MEMS gyroscopes，accelerometers and magnetometer and a multiplicative adaptive extended kalman filter algorithm(MAEKF)is proposed based on the gravity field and magnetic field constructed measurement vector.In order to correct attitude and heading data，the MAEKF algorithm selects quaternion error and bias of gyroscopes as state vector.Then，the accurate measurement method is adopted to provide the filterwith normalized constraint.Furthermore，this paper presents an innovation-based formula which is used to estimate measurement noise covariance matrix.Finally，simulation and flight-test show that the designed AHRS can provide accurate attitude and heading data.Compared with traditional extended kalman filter(EKF)，the MAEKF algorithm effectively improves the accuracy，stability and robustness of the system.

attitude and heading reference system;multiplicative extended kalman filter;adaptive;quaternion error EEACC:2575;7230;7630 doi:10.3969/j.issn.1004－1699.2017.02.026

10.3969/j.issn.1004－1699.2017.02.026

TJ765

A

1004－1699(2017)02－0319－07

2016－08－03 修改日期:2016－09－22