基于三子樣更新的EKF低成本MEMS姿態(tài)估計(jì)算法

杜思遠(yuǎn) 李杰 鄭濤 楊雁宇

摘要：針對(duì)低成本MEMS慣性器件精度較低、噪聲較大、存在誤差隨時(shí)間積累而無法滿足長時(shí)間載體姿態(tài)測(cè)量的問題，提出一種基于三子樣姿態(tài)更新的擴(kuò)展卡爾曼姿態(tài)估計(jì)算法。該算法結(jié)合三子樣構(gòu)造姿態(tài)更新四元數(shù)作為擴(kuò)展卡爾曼濾波的狀態(tài)量，利用當(dāng)?shù)貙?dǎo)航系下重力場(chǎng)、磁場(chǎng)中相關(guān)數(shù)據(jù)通過比例積分控制器完成對(duì)角速率誤差的一次補(bǔ)償，同時(shí)作為擴(kuò)展卡爾曼觀測(cè)信息，完成對(duì)姿態(tài)四元數(shù)的實(shí)時(shí)校正，以此降低誤差隨時(shí)間積累的影響。為驗(yàn)證算法精度，設(shè)計(jì)轉(zhuǎn)臺(tái)靜態(tài)及車載動(dòng)態(tài)試驗(yàn)，試驗(yàn)結(jié)果表明在測(cè)試約為400 s的時(shí)間內(nèi)，航向角誤差約為5°，俯仰角及滾轉(zhuǎn)角誤差低于2°，且無發(fā)散問題，滿足長時(shí)姿態(tài)測(cè)量要求。

關(guān)鍵詞：慣性測(cè)量；低成本MEMS慣性器件；三子樣算法；擴(kuò)展卡爾曼濾波；姿態(tài)四元數(shù)