《電子技術(shù)應(yīng)用》
您所在的位置:首頁 > 嵌入式技術(shù) > 設(shè)計應(yīng)用 > 基于ARM與低成本MEMS器件的AHRS設(shè)計
基于ARM與低成本MEMS器件的AHRS設(shè)計
單片機與嵌入式系統(tǒng)應(yīng)用
梁建宏,田偉程,王田苗
摘要: 自平衡機器人、多旋翼無人飛行器的控制需要高精度的姿態(tài)運動信息作為反饋輸入,要求測量模塊具有響應(yīng)快、體積小和功耗低的特點。采用低成本的MEMS器件與STM32單片機構(gòu)建了航向姿態(tài)參考系統(tǒng)硬件平臺。針對傳感器的特點,設(shè)計了基于擴展卡爾曼濾波算法的雙矢量校正方法,并給出了陀螺儀的溫度補償、磁傳感器的校正方法。
關(guān)鍵詞: ARM MEMS 機器人
Abstract:
Key words :

引言

航向姿態(tài)參考系統(tǒng)(Attitude and Heading Reference System,AHRS)能夠提供航向、橫滾和側(cè)翻等姿態(tài)信息,機械陀螺儀及光纖陀螺儀等高精度慣性導(dǎo)航器件價格昂貴,難以得到推廣。目前MEMS傳感器在消費類電子產(chǎn)品中得到廣泛應(yīng)用,但是MEMS角速率陀螺儀存在嚴重的零點漂移和隨機誤差,在捷聯(lián)慣性導(dǎo)航解算中會產(chǎn)生積分誤差,難以達到應(yīng)用的精度。加速度計和磁場計能分別測量出重力加速度和地磁場這兩個不相關(guān)的三維矢量,可以作為平臺姿態(tài)的觀測矢量來校準陀螺儀。擴展卡爾曼濾波可以結(jié)合這幾種傳感器的特點,以陀螺儀測量得到的角速率作預(yù)測更新,以重力加速度和磁場觀測更新,得到更高精度的姿態(tài)角信息。

1 硬件結(jié)構(gòu)

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

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

圖1 航向姿態(tài)參考系統(tǒng)的硬件結(jié)構(gòu)

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

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

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

四元數(shù)微分方程如式(4)所示,四元數(shù)姿態(tài)矩陣微分

方程只要解4個微分方程,比方向余弦姿態(tài)矩陣微分方程減少了大量的運算,便于微處理器的編程實現(xiàn)。

2.1 時間更新

系統(tǒng)的狀態(tài)方程如式(5)所示。

其中狀態(tài)變量為四元數(shù)X=[q0,q1,q2,q3]T,Wk-1為四維過程噪聲。矩陣A可以根據(jù)陀螺儀測得的三軸角速率[ωX,ωY,ωZ]T得到,如式(6)所示。其中Δt為兩次時間預(yù)測更新所流逝的時間。

狀態(tài)變量的時間更新如式(7)所示。

協(xié)方差矩陣P預(yù)測如式(8)所示,式中Q為四維過程激勵噪聲協(xié)方差。

2.2 觀測更新

AHRS的觀測更新是通過本體坐標系上的重力加速度和地磁場的參考矢量旋轉(zhuǎn)至導(dǎo)航坐標系上,再與加速度和磁場傳感器比較,得到觀測變量的殘余。由本體系轉(zhuǎn)換至導(dǎo)航系的轉(zhuǎn)移矩陣由四元數(shù)可以表示為式(9)。

三維參考向量v轉(zhuǎn)移至導(dǎo)航系中可由觀測方程式(10)表示。

當重力加速度觀測更新時參考向量v等于重力加速度參考矢量(可設(shè)置為當平臺靜止水平放置時,加速度計測量得到的三維矢量為:

當磁場觀測更新時v等于磁場參考矢量(可設(shè)置為當平臺靜止水平放置且航向指向正北時,磁阻計測量得到的三維矢量為:

H是h對X求偏導(dǎo)的雅可比矩陣,如式(11)所示。

卡爾曼增益矩陣Kk如式(12)所示,式中R陣為三維觀測噪聲協(xié)方差矩陣。

觀測更新:

當重力加速度觀測更新時zk為加速度,傳感器測量得到的三維矢量zk=[aXaYaZ]T,當磁場觀測更新時zk為磁阻傳感器,測量得到的三維矢量zk=[mXmYmZ]T。協(xié)方差更新:

3 程序結(jié)構(gòu)

AHRS的軟件設(shè)計主要分為:

① 傳感器初始化,包括設(shè)置傳感器的更新速率、量程。

② 初始化卡爾曼濾波的相關(guān)矩陣,根據(jù)傳感器的特點設(shè)置過程激勵噪聲協(xié)方差矩陣Q,設(shè)為對角元素為0.1的四維對角方陣。

③ 若成功讀取陀螺儀數(shù)據(jù),進行卡爾曼濾波的時間更新。

④ 采集加速度傳感器和磁阻傳感器的數(shù)據(jù),若讀取成功則進行觀測更新。加速度觀測更新與磁場觀測更新算法差別在于觀測方差的R,可根據(jù)兩種傳感器的置信度設(shè)置相應(yīng)的值,航向姿態(tài)參考系的程序流程如圖2所示。

圖2 航向姿態(tài)參考系的程序流程

4 傳感器校準

4.1 陀螺儀溫度補償

低成本MEMS陀螺儀存在著較大的零點偏移,一般可以達到1~3 °/s??梢酝ㄟ^增加擴展卡爾曼濾波的狀態(tài)變量的維數(shù),即增加三維陀螺零偏做數(shù)據(jù)融合得以解決。優(yōu)點是可以動態(tài)地估計陀螺的零偏,有較強的適應(yīng)能力,缺點是卡爾曼濾波算法的計算量以維數(shù)的三次方增加,因此實時性會有所降低,在要求姿態(tài)更新速率高而單片機計算性能有限的情況下,可以采用溫度補償?shù)姆椒ń鉀Q。陀螺儀的零點偏移與傳感器溫度和溫度梯度密切相關(guān)。MPU6000陀螺儀溫度變化緩慢的情況下,可以認為其溫度零點漂移對應(yīng)此時傳感器的溫度。

可設(shè)溫漂曲線為三次多項式BX=At3+Bt2+Ct1+D,通過最小二乘法擬合之后可得到各項系數(shù)。經(jīng)過溫度補償后的陀螺儀曲線如圖3所示。

圖3 靜止時減去溫漂后的角速率曲線

4.2 硬磁及非正交度校正

地磁場正常情況下測量到的三維數(shù)據(jù)在空間上的包絡(luò)應(yīng)該是一個標準的圓球。但是磁場計測量出來的數(shù)據(jù)由于受到外界磁場的影響,加上磁阻傳感器各軸的標度因子和非正交度,導(dǎo)致傳感器采集到的數(shù)據(jù)在三維空間內(nèi)分布的包絡(luò)面為球心偏移原點的橢球面,磁場裸數(shù)據(jù)三維分布如圖4所示。

圖4 磁場裸數(shù)據(jù)三維分布

橢球面約束方程如式(15)所示。

其中m為傳感器測得的三維磁場強度,c為球心偏移的三維向量,U為標度因子及非正交度校矩陣。磁場強度沒有實際意義,關(guān)心的是傳感器測量的地磁三維矢量方向,所以設(shè)磁場向量模為1。通過最小二乘法可以計算出U和c。磁場數(shù)據(jù)校正前后對比如圖5所示,左右兩圖為校正前后數(shù)據(jù)在XY平面上的投影。

圖5 磁場數(shù)據(jù)校正前后對比

5 實驗結(jié)果

AHRS放置在與1024線光柵編碼器固連的轉(zhuǎn)動平臺上,測試俯仰姿態(tài)角的測量精度及跟蹤性能,AHRS與編碼器測量曲線對比如圖6所示。

圖6 AHRS與編碼器測量曲線對比

圖中實線為AHRS的測量值,點劃線為編碼器的測量值。當測試平臺以幅度約±10°的幅度擺動時。AHRS與編碼器測量數(shù)據(jù)相比在時間上滯后最大不超過5 ms,峰峰值相差不超過03°。

結(jié)語

基于四元數(shù)擴展卡爾曼濾波算法的AHRS具有更新速率高、實時性好、價格低廉的特點,能夠廣泛應(yīng)用于手機、平板電腦等消費類電子產(chǎn)品, 也能滿足一些機器人對姿態(tài)控制的測量需求。

參考文獻

[1] 付夢印,鄧志紅,閆莉萍.Kalman濾波理論及其在導(dǎo)航系統(tǒng)中的應(yīng)用[M].2版.北京:科學(xué)出版社,2010:1718.

[2] 毛奔,林玉榮.慣性器件測試與建模[M].哈爾濱:哈爾濱工程大學(xué)出版社,2007:9394.

[3] 鄧正隆.慣性技術(shù)[M].哈爾濱:哈爾濱工業(yè)大學(xué)出版社, 2006:620.

[4] 李建利.新型硅MEMS陀螺儀和角加速度計結(jié)構(gòu)設(shè)計及MIMU誤差標定補償[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] 張樹俠.捷聯(lián)式慣性導(dǎo)航系統(tǒng)[M].北京:國防工業(yè)出版社,1992:1517.

[7] 吳永亮,王田苗,梁建宏.微小型無人機三軸磁強計現(xiàn)場誤差校正方法[J].航空學(xué)報, 2011, 32(2):330336.

此內(nèi)容為AET網(wǎng)站原創(chuàng),未經(jīng)授權(quán)禁止轉(zhuǎn)載。
亚洲一区二区欧美_亚洲丝袜一区_99re亚洲国产精品_日韩亚洲一区二区
中日韩午夜理伦电影免费| 欧美在线看片| 国产三区二区一区久久| 欧美三级电影一区| 欧美激情亚洲| 欧美77777| 男女激情久久| 久久中文字幕导航| 久久综合电影| 女同性一区二区三区人了人一| 久久午夜国产精品| 久久另类ts人妖一区二区| 久久免费的精品国产v∧| 久久久91精品国产一区二区三区| 欧美一区二区视频网站| 欧美亚洲日本国产| 欧美在线观看网站| 久久久久久久久久久一区 | 亚洲欧美日韩精品久久久| 亚洲黄一区二区三区| 亚洲激情图片小说视频| 亚洲欧洲日本专区| 亚洲作爱视频| 午夜精品久久久久久久白皮肤| 香蕉久久夜色精品| 欧美一区中文字幕| 久久精品一区四区| 玖玖玖免费嫩草在线影院一区| 久久综合久久综合这里只有精品| 猛男gaygay欧美视频| 欧美精品日韩| 国产精品乱码一区二区三区| 国产欧美精品一区二区三区介绍| 国产在线观看一区| 亚洲高清自拍| 一本色道久久综合亚洲精品高清 | 欧美日韩中文字幕日韩欧美| 欧美午夜在线视频| 国产精品尤物| 在线成人黄色| 亚洲精品国产精品国产自| 亚洲一级黄色片| 欧美中日韩免费视频| 亚洲美女中出| 亚欧成人在线| 免费在线成人| 国产精品日韩一区二区| 国内精品免费在线观看| 亚洲日本成人女熟在线观看| 亚洲在线视频一区| 最新成人在线| 午夜精品久久久| 蜜臀av在线播放一区二区三区| 欧美日韩免费区域视频在线观看| 国产精品一区二区三区乱码 | 欧美日韩久久精品| 国产亚洲精品aa午夜观看| 亚洲国产欧美一区二区三区同亚洲| 夜夜爽99久久国产综合精品女不卡| 欧美中文日韩| 亚洲网址在线| 久久综合久久久| 国产精品视频第一区| 在线日韩av永久免费观看| 一区二区国产精品| 亚洲国产精品久久久久| 亚洲主播在线播放| 欧美大胆人体视频| 国产一区二区日韩精品欧美精品| 日韩午夜精品视频| 亚洲国产99| 性做久久久久久| 欧美日韩亚洲三区| 在线成人国产| 欧美一级黄色网| 亚洲一区二区在线播放| 麻豆精品一区二区av白丝在线| 国产精品久久久91| 最近看过的日韩成人| 欧美一进一出视频| 亚洲免费在线视频一区 二区| 欧美不卡视频| 国产一区在线视频| 这里只有精品在线播放| 亚洲精品日产精品乱码不卡| 久久久久久久999| 国产精品日韩在线一区| 亚洲精品一区在线观看| 亚洲电影专区| 久久天天狠狠| 国产在线播精品第三| 亚洲欧美日本国产有色| 亚洲视频碰碰| 欧美日韩国产丝袜另类| 亚洲第一网站| 亚洲国产老妈| 久久久精品一品道一区| 国产欧美一区二区三区视频| 一区二区三区国产精华| 一本久久精品一区二区| 欧美激情视频一区二区三区不卡| 黄色成人av| 久久激情网站| 久久久久高清| 国产原创一区二区| 欧美影院午夜播放| 久久国产黑丝| 国产一区二区三区免费观看| 午夜视频在线观看一区| 性欧美暴力猛交另类hd| 国产精品久久久久久超碰| 一区二区精品在线| 亚洲天堂免费观看| 欧美日韩在线精品| 夜夜嗨网站十八久久| 亚洲视频电影在线| 欧美日韩一区二区三区在线视频 | 亚洲一区二区在线视频| 欧美日韩色一区| 99re这里只有精品6| 亚洲视频日本| 国产精品美女久久久久av超清| 在线亚洲免费| 亚洲视频在线一区| 国产精品v片在线观看不卡| 一区二区三区视频免费在线观看| 亚洲网址在线| 国产精品一级| 欧美一级成年大片在线观看| 久久免费黄色| 在线欧美影院| 一区二区日韩| 欧美性猛片xxxx免费看久爱| 国产精品99久久久久久www| 午夜精品久久久久久99热| 国产精品亚洲第一区在线暖暖韩国| 亚洲一区999| 久久精品天堂| 亚洲电影下载| 亚洲一区日本| 国产午夜精品在线| 亚洲国产欧美日韩另类综合| 欧美精品国产| 亚洲午夜性刺激影院| 久久国产视频网站| 亚洲高清av| 亚洲欧美日韩综合| 激情五月婷婷综合| 日韩网站在线观看| 国产精品久久久一区二区| 先锋影音一区二区三区| 蜜月aⅴ免费一区二区三区| 亚洲精品一区中文| 欧美一区=区| 在线播放中文一区| 亚洲天堂av综合网| 国产一区二区三区不卡在线观看| 亚洲青色在线| 国产精品视频免费观看www| 欧美怡红院视频| 欧美日本免费| 欧美一区不卡| 欧美精品一区在线| 欧美伊人久久久久久午夜久久久久 | 欧美一区二区播放| 欧美高清视频免费观看| 亚洲午夜激情网站| 蜜臀a∨国产成人精品| 亚洲午夜在线视频| 麻豆免费精品视频| 亚洲视频一区二区| 毛片av中文字幕一区二区| 99视频精品全国免费| 久久久美女艺术照精彩视频福利播放| 91久久精品国产91性色| 欧美亚洲系列| 亚洲欧洲精品成人久久奇米网 | 亚洲国产日韩欧美综合久久| 欧美日韩不卡一区| 欧美淫片网站| 国产精品高清网站| 亚洲国产精品电影在线观看| 欧美三级视频在线播放| 亚洲高清电影| 国产精品入口尤物| 亚洲精品一区二区三区四区高清| 国产欧美丝祙| 亚洲视频一区| 亚洲国产高清视频| 久久精品国产精品亚洲精品| 99国产成+人+综合+亚洲欧美| 久久久中精品2020中文| 一本一本久久a久久精品综合麻豆| 玖玖精品视频| 亚洲欧美综合一区| 欧美午夜免费| 一本久久综合亚洲鲁鲁| 激情综合色丁香一区二区| 亚洲欧美中文日韩v在线观看| 亚洲人体一区|