基于A(yíng)RM與MEMS的AHRS設計
加入技術(shù)交流群
卡爾曼增益矩陣Kk如式(12)所示，式中R陣為三維觀(guān)測噪聲協(xié)方差矩陣。
觀(guān)測更新:
當重力加速度觀(guān)測更新時(shí)zk為加速度，傳感器測量得到的三維矢量zk=[aXaYaZ]T，當磁場(chǎng)觀(guān)測更新時(shí)zk為磁阻傳感器，測量得到的三維矢量zk=[mXmYmZ]T。協(xié)方差更新：
3 程序結構
AHRS的軟件設計主要分為：
?、?傳感器初始化，包括設置傳感器的更新速率、量程。
?、?初始化卡爾曼濾波的相關(guān)矩陣，根據傳感器的特點(diǎn)設置過(guò)程激勵噪聲協(xié)方差矩陣Q，設為對角元素為0.1的四維對角方陣。
?、?若成功讀取陀螺儀數據，進(jìn)行卡爾曼濾波的時(shí)間更新。
?、?采集加速度傳感器和磁阻傳感器的數據，若讀取成功則進(jìn)行觀(guān)測更新。加速度觀(guān)測更新與磁場(chǎng)觀(guān)測更新算法差別在于觀(guān)測方差的R，可根據兩種傳感器的置信度設置相應的值，航向姿態(tài)參考系的程序流程如圖2所示。
圖2 航向姿態(tài)參考系的程序流程
4 傳感器校準
4.1 陀螺儀溫度補償
低成本MEMS陀螺儀存在著(zhù)較大的零點(diǎn)偏移，一般可以達到1~3 °/s?？梢酝ㄟ^(guò)增加擴展卡爾曼濾波的狀態(tài)變量的維數，即增加三維陀螺零偏做數據融合得以解決。優(yōu)點(diǎn)是可以動(dòng)態(tài)地估計陀螺的零偏，有較強的適應能力，缺點(diǎn)是卡爾曼濾波算法的計算量以維數的三次方增加，因此實(shí)時(shí)性會(huì )有所降低，在要求姿態(tài)更新速率高而單片機計算性能有限的情況下，可以采用溫度補償的方法解決。陀螺儀的零點(diǎn)偏移與傳感器溫度和溫度梯度密切相關(guān)。MPU6000陀螺儀溫度變化緩慢的情況下，可以認為其溫度零點(diǎn)漂移對應此時(shí)傳感器的溫度。
評論