引言
航向姿態參考系統（Attitude and Heading Reference System，AHRS）能夠提供航向、橫滾和側翻等姿態信息，機械陀螺儀及光纖陀螺儀等高精度慣性導航器件價格昂貴，難以得到推廣。目前MEMS傳感器在消費類電子產品中得到廣泛應用，但是MEMS角速率陀螺儀存在嚴重的零點漂移和隨機誤差，在捷聯慣性導航解算中會產生積分誤差，難以達到應用的精度。加速度計和磁場計能分別測量出重力加速度和地磁場這兩個不相關的三維矢量，可以作為平臺姿態的觀測矢量來校準陀螺儀。擴展卡爾曼濾波可以結合這幾種傳感器的特點，以陀螺儀測量得到的角速率作預測更新，以重力加速度和磁場觀測更新，得到更高精度的姿態角信息。
1 硬件結構
MEMS器件的AHRS硬件基本組成為三軸角速率陀螺儀、三軸加速度計、三軸磁阻傳感器和STM32系列微處理器STM32F103U8T6。航向姿態參考系統的硬件結構如圖1所示。
IMU采用整合了16位的三軸陀螺儀和三軸加速度計的MPU6000，與多組件方案相比，有效避免了組合陀螺儀與加速器時之軸間安裝誤差的問題，節省了安裝空間。同時，內部自帶了16位A/D轉換器，簡化了電路設計。MPU6000的角速率量程為±250 °/s、±500 °/s、±1000 °/s與±2000 °/s。加速度測量范圍為±2g、±4g、±8g與±16g。內部自帶16位的數字溫度傳感器，方便對傳感器進行溫度補償。數據可通過最高可達400 kHz的I2C總線或最高可達20 MHz的SPI接口傳輸，采樣更新速率達到8 kHz，可保證系統測量的實時性。
圖1 航向姿態參考系統的硬件結構
霍尼韋爾HMC5883為三軸12位I2C總線數字量輸出磁阻傳感器，測量范圍為±1~±8 Gs，數據更新速率為80 Hz。內置OFFSET/SET/RESET電路，不會出現磁飽和與累加誤差現象，支持自動校準程序，簡化使用步驟，可以滿足地磁場的測量要求。選用基于CortexM3內核的STM32系列ARM處理器STM32F103U8T6，主頻達72 MHz，1.25 DMIPS/MHz；具有硬件單周期乘法器，保證姿態更新的實時性；具有豐富外設接口，可采用I2C總線接口從傳感器中讀取數據，通過串口與上位機進行通信。
2 四維擴展卡爾曼濾波算法
擴展卡爾曼濾波算法(Extended Kalman Filter, EKF)是一套由計算機實現的實時遞推算法，所處理的對象是隨機信號，利用系統噪聲和觀測噪聲的統計特性，以系統的觀測量作為濾波器的輸入，以所要求的估計值（系統的狀態變量）作為濾波器的輸出，濾波器的輸入和輸出由時間更新和觀測更新算法聯系在一起，根據系統的狀態方程和觀測方程估算出所需要處理的信號。AHRS擴展卡爾曼濾波算法的狀態變量采用四維四元數，與采用歐拉角相比，避免了采用歐拉角計算時涉及的大量三角函數運算，保證了更新速率和實時性，同時不存在采用歐拉角運算出現的奇異性。歐拉角與四元數的轉換關系如式(1)~(3)所示。
四元數微分方程如式(4)所示，四元數姿態矩陣微分
方程只要解4個微分方程，比方向余弦姿態矩陣微分方程減少了大量的運算，便于微處理器的編程實現。
2.1 時間更新
系統的狀態方程如式（5）所示。
其中狀態變量為四元數X=[q0,q1,q2,q3]T，Wk-1為四維過程噪聲。矩陣A可以根據陀螺儀測得的三軸角速率[ωX，ωY，ωZ]T得到，如式(6)所示。其中Δt為兩次時間預測更新所流逝的時間。
狀態變量的時間更新如式(7)所示。
協方差矩陣P預測如式(8)所示，式中Q為四維過程激勵噪聲協方差。
2.2 觀測更新
AHRS的觀測更新是通過本體坐標系上的重力加速度和地磁場的參考矢量旋轉至導航坐標系上，再與加速度和磁場傳感器比較，得到觀測變量的殘余。由本體系轉換至導航系的轉移矩陣由四元數可以表示為式（9）。
三維參考向量v轉移至導航系中可由觀測方程式(10)表示。
當重力加速度觀測更新時參考向量v等于重力加速度參考矢量（可設置為當平臺靜止水平放置時，加速度計測量得到的三維矢量為：
當磁場觀測更新時v等于磁場參考矢量（可設置為當平臺靜止水平放置且航向指向正北時，磁阻計測量得到的三維矢量為：
H是h對X求偏導的雅可比矩陣，如式（11）所示。
卡爾曼增益矩陣Kk如式（12）所示，式中R陣為三維觀測噪聲協方差矩陣。
觀測更新:
當重力加速度觀測更新時zk為加速度，傳感器測量得到的三維矢量zk=[aXaYaZ]T，當磁場觀測更新時zk為磁阻傳感器，測量得到的三維矢量zk=[mXmYmZ]T。協方差更新：
3 程序結構
AHRS的軟件設計主要分為：
① 傳感器初始化，包括設置傳感器的更新速率、量程。
② 初始化卡爾曼濾波的相關矩陣，根據傳感器的特點設置過程激勵噪聲協方差矩陣Q，設為對角元素為0.1的四維對角方陣。
③ 若成功讀取陀螺儀數據，進行卡爾曼濾波的時間更新。
④ 采集加速度傳感器和磁阻傳感器的數據，若讀取成功則進行觀測更新。加速度觀測更新與磁場觀測更新算法差別在于觀測方差的R，可根據兩種傳感器的置信度設置相應的值，航向姿態參考系的程序流程如圖2所示。
圖2 航向姿態參考系的程序流程
4 傳感器校準
4.1 陀螺儀溫度補償
低成本MEMS陀螺儀存在著較大的零點偏移，一般可以達到1~3 °/s。可以通過增加擴展卡爾曼濾波的狀態變量的維數，即增加三維陀螺零偏做數據融合得以解決。優點是可以動態地估計陀螺的零偏，有較強的適應能力，缺點是卡爾曼濾波算法的計算量以維數的三次方增加，因此實時性會有所降低，在要求姿態更新速率高而單片機計算性能有限的情況下，可以采用溫度補償的方法解決。陀螺儀的零點偏移與傳感器溫度和溫度梯度密切相關。MPU6000陀螺儀溫度變化緩慢的情況下，可以認為其溫度零點漂移對應此時傳感器的溫度。
可設溫漂曲線為三次多項式BX=At3+Bt2+Ct1+D，通過最小二乘法擬合之后可得到各項系數。經過溫度補償后的陀螺儀曲線如圖3所示。
圖3 靜止時減去溫漂后的角速率曲線
4.2 硬磁及非正交度校正
地磁場正常情況下測量到的三維數據在空間上的包絡應該是一個標準的圓球。但是磁場計測量出來的數據由于受到外界磁場的影響，加上磁阻傳感器各軸的標度因子和非正交度，導致傳感器采集到的數據在三維空間內分布的包絡面為球心偏移原點的橢球面，磁場裸數據三維分布如圖4所示。
圖4 磁場裸數據三維分布
橢球面約束方程如式(15)所示。
其中m為傳感器測得的三維磁場強度，c為球心偏移的三維向量，U為標度因子及非正交度校矩陣。磁場強度沒有實際意義，關心的是傳感器測量的地磁三維矢量方向，所以設磁場向量模為1。通過最小二乘法可以計算出U和c。磁場數據校正前后對比如圖5所示，左右兩圖為校正前后數據在XY平面上的投影。
圖5 磁場數據校正前后對比
5 實驗結果
AHRS放置在與1024線光柵編碼器固連的轉動平臺上，測試俯仰姿態角的測量精度及跟蹤性能，AHRS與編碼器測量曲線對比如圖6所示。
圖6 AHRS與編碼器測量曲線對比
圖中實線為AHRS的測量值，點劃線為編碼器的測量值。當測試平臺以幅度約±10°的幅度擺動時。AHRS與編碼器測量數據相比在時間上滯后最大不超過5 ms，峰峰值相差不超過03°。
結語
基于四元數擴展卡爾曼濾波算法的AHRS具有更新速率高、實時性好、價格低廉的特點，能夠廣泛應用于手機、平板電腦等消費類電子產品， 也能滿足一些機器人對姿態控制的測量需求。
參考文獻
[1] 付夢印,鄧志紅,閆莉萍.Kalman濾波理論及其在導航系統中的應用[M].2版.北京：科學出版社,2010:1718.
[2] 毛奔,林玉榮.慣性器件測試與建模[M].哈爾濱:哈爾濱工程大學出版社,2007:9394.
[3] 鄧正隆.慣性技術[M].哈爾濱:哈爾濱工業大學出版社, 2006:620.
[4] 李建利.新型硅MEMS陀螺儀和角加速度計結構設計及MIMU誤差標定補償[D].北京:北京航空航天大學,2008.
[5] C C Foster, G H Elkaim, UC Santa Cruz. Extension of a twoStep calibration methodology to include nonorthogonal sensor axes[J]. IEEE Transactionson Aerospace and ElectronicSystems, 2008,44(3):10701078.
[6] 張樹俠．捷聯式慣性導航系統[M]．北京：國防工業出版社，1992：1517.
[7] 吳永亮，王田苗，梁建宏.微小型無人機三軸磁強計現場誤差校正方法[J].航空學報, 2011, 32(2):330336.