基于ARM與低成本MEMS器件的AHRS設(shè)計
加入技術(shù)交流群
當(dāng)重力加速度觀測更新時zk為加速度，傳感器測量得到的三維矢量zk=[ax ay az]T，當(dāng)磁場觀測更新時zK為磁阻傳感器，測量得到的三維矢量zK=[mx mymz]T。協(xié)方差更新：
3 程序結(jié)構(gòu)
AHRS的軟件設(shè)計主要分為：
①傳感器初始化，包括設(shè)置傳感器的更新速率、量程。
②初始化卡爾曼濾波的相關(guān)矩陣，根據(jù)傳感器的特點設(shè)置過程激勵噪聲協(xié)方差矩陣Q，設(shè)為對角元素為0．1的四維對角方陣。
③若成功讀取陀螺儀數(shù)據(jù)，進(jìn)行卡爾曼濾波的時間更新。
④采集加速度傳感器和磁阻傳感器的數(shù)據(jù)，若讀取成功則進(jìn)行觀測更新。加速度觀測更新與磁場觀測更新算法差別在于觀測方差的R，可根據(jù)兩種傳感器的置信度沒置相應(yīng)的值，航向姿態(tài)參考系的程序流程如圖2所示。本文引用地址：http://www.ex-cimer.com/article/148911.htm
4 傳感器校準(zhǔn)
4．1 陀螺儀溫度補(bǔ)償
低成本MEMS陀螺儀存在著較大的零點偏移，一般可以達(dá)到1～3°／s。可以通過增加擴(kuò)展卡爾曼濾波的狀態(tài)變量的維數(shù)，即增加三維陀螺零偏做數(shù)據(jù)融合得以解決。優(yōu)點是可以動態(tài)地估計陀螺的零偏，有較強(qiáng)的適應(yīng)能力，缺點是卡爾曼濾波算法的計算量以維數(shù)的三次方增加，因此實時性會有所降低，在要求姿態(tài)更新速率高而單片機(jī)計算性能有限的情況下，可以采用溫度補(bǔ)償?shù)姆椒ń鉀Q。陀螺儀的零點偏移與傳感器溫度和溫度梯度密切相關(guān)。MPU-6000陀螺儀溫度變化緩慢的情況下，可以認(rèn)為其溫度零點漂移對應(yīng)此時傳感器的溫度。
可設(shè)溫漂曲線為三次多項式BX=At3+Bt2+Ct1+D，通過最小二乘法擬合之后可得到各項系數(shù)。經(jīng)過溫度補(bǔ)償后的陀螺儀曲線如圖3所示。
評論