基于A(yíng)RM與MEMS的AHRS設計

目前MEMS傳感器在消費類(lèi)電子產(chǎn)品中得到廣泛應用，但是MEMS角速率陀螺儀存在嚴重的零點(diǎn)漂移和隨機誤差，在捷聯(lián)慣性導航解算中會(huì )產(chǎn)生積分誤差，難以達到應用的精度。加速度計和磁場(chǎng)計能分別測量出重力加速度和地磁場(chǎng)這兩個(gè)不相關(guān)的三維矢量，可以作為平臺姿態(tài)的觀(guān)測矢量來(lái)校準陀螺儀。擴展卡爾曼濾波可以結合這幾種傳感器的特點(diǎn)，以陀螺儀測量得到的角速率作預測更新，以重力加速度和磁場(chǎng)觀(guān)測更新，得到更高精度的姿態(tài)角信息。

　　1　硬件結構

　　MEMS器件的AHRS硬件基本組成為三軸角速率陀螺儀、三軸加速度計、三軸磁阻傳感器和STM32系列微處理器STM32F103U8T6。航向姿態(tài)參考系統的硬件結構如圖1所示。

　　IMU采用整合了16位的三軸陀螺儀和三軸加速度計的MPU6000，與多組件方案相比，有效避免了組合陀螺儀與加速器時(shí)之軸間安裝誤差的問(wèn)題，節省了安裝空間。同時(shí)，內部自帶了16位A/D轉換器，簡(jiǎn)化了電路設計。MPU6000的角速率量程為±250 °/s、±500 °/s、±1000 °/s與±2000 °/s。加速度測量范圍為±2g、±4g、±8g與±16g。內部自帶16位的數字溫度傳感器，方便對傳感器進(jìn)行溫度補償。數據可通過(guò)最高可達400 kHz的I2C總線(xiàn)或最高可達20 MHz的SPI接口傳輸，采樣更新速率達到8 kHz，可保證系統測量的實(shí)時(shí)性。

　　圖1　航向姿態(tài)參考系統的硬件結構

　　霍尼韋爾HMC5883為三軸12位I2C總線(xiàn)數字量輸出磁阻傳感器，測量范圍為±1~±8 Gs，數據更新速率為80 Hz。內置OFFSET/SET/RESET電路，不會(huì )出現磁飽和與累加誤差現象，支持自動(dòng)校準程序，簡(jiǎn)化使用步驟，可以滿(mǎn)足地磁場(chǎng)的測量要求。選用基于CortexM3內核的STM32系列ARM處理器STM32F103U8T6，主頻達72 MHz，1.25 DMIPS/MHz;具有硬件單周期乘法器，保證姿態(tài)更新的實(shí)時(shí)性;具有豐富外設接口，可采用I2C總線(xiàn)接口從傳感器中讀取數據，通過(guò)串口與上位機進(jìn)行通信。

　　2　四維擴展卡爾曼濾波算法

　　擴展卡爾曼濾波算法(Extended Kalman Filter, EKF)是一套由計算機實(shí)現的實(shí)時(shí)遞推算法，所處理的對象是隨機信號，利用系統噪聲和觀(guān)測噪聲的統計特性，以系統的觀(guān)測量作為濾波器的輸入，以所要求的估計值(系統的狀態(tài)變量)作為濾波器的輸出，濾波器的輸入和輸出由時(shí)間更新和觀(guān)測更新算法聯(lián)系在一起，根據系統的狀態(tài)方程和觀(guān)測方程估算出所需要處理的信號。AHRS擴展卡爾曼濾波算法的狀態(tài)變量采用四維四元數，與采用歐拉角相比，避免了采用歐拉角計算時(shí)涉及的大量三角函數運算，保證了更新速率和實(shí)時(shí)性，同時(shí)不存在采用歐拉角運算出現的奇異性。歐拉角與四元數的轉換關(guān)系如式(1)~(3)所示。

　　四元數微分方程如式(4)所示，