基于ARM與MEMS的AHRS設(shè)計(jì)

　　卡爾曼增益矩陣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í)傳感器的溫度。