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