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