基于卡爾曼濾波的三維空間姿態(tài)仿真系統(tǒng)設(shè)計(jì)

喻順志 諶雅潔 石銀標(biāo) 劉靜斯

【摘要】 本文設(shè)計(jì)的基于卡爾曼濾波的三維空間姿態(tài)仿真系統(tǒng)設(shè)計(jì)，本系統(tǒng)的開發(fā)以ARM內(nèi)核的STM32微控制器為處理核心，采集加速度計(jì)、陀螺儀和電子羅盤信號，通過Kalman濾波算法對傳感器信號進(jìn)行濾波，然后通過四元數(shù)算法求解出真實(shí)的空間角，然后通過nrf24l01射頻模塊將數(shù)據(jù)傳送到上位機(jī)接收模塊。通過自主開發(fā)的一款基于C#.net的三位姿態(tài)仿真平臺讀取并解析數(shù)據(jù)，實(shí)時(shí)仿真顯示，從而在計(jì)算機(jī)或手機(jī)終端上對被測物體的三維空間姿態(tài)進(jìn)行監(jiān)測。

【關(guān)鍵詞】 STM32 Kalman濾波 四元數(shù) nrf24l01 姿態(tài)仿真

一、引 言

三維仿真系統(tǒng)可真實(shí)模擬工況運(yùn)行狀態(tài)；真實(shí)模擬設(shè)備運(yùn)行狀態(tài)；在海洋工程領(lǐng)域可通過三維仿真動畫來真實(shí)模擬海洋工程裝備的制造過程，不僅能夠有效地縮短造船周期，還能降低投資成本。在航空領(lǐng)域可進(jìn)行各種模擬駕駛飛行培訓(xùn)等。在機(jī)械設(shè)計(jì)與運(yùn)動領(lǐng)域可通過建立系統(tǒng)的數(shù)值模型，利用計(jì)算機(jī)仿真使大量的產(chǎn)品設(shè)計(jì)缺陷在產(chǎn)品成形之前就得到處理。本系統(tǒng)的可開發(fā)前景較廣，可廣泛應(yīng)用于軍事、工業(yè)、以及汽車行業(yè)等領(lǐng)域，例如：改正或消除船載儀器由于船體的垂蕩、橫搖和縱搖引起的測量誤差、飛行器姿態(tài)的測量與矯正、仿人機(jī)器人肢體姿態(tài)的檢測、汽車事故現(xiàn)場還原等等。

二、系統(tǒng)方案設(shè)計(jì)

系統(tǒng)由STM32位控制器、MPU6050陀螺儀加速度計(jì)模塊、HMC5883指南針模塊、NRF24L01射頻模塊、三位姿態(tài)仿真平臺組成。

系統(tǒng)工作流程

1、系統(tǒng)初始化：包括對單片機(jī)、MPU6050模塊、HMC5883指南針模塊、NRF24L01射頻模塊的初始化 2、單片機(jī)讀取MPU6050模塊、HMC5883指南針模塊數(shù)據(jù)3、通過Kalman濾波算法對讀取出的信號進(jìn)行濾波4、通過四元數(shù)算法求解空間姿態(tài)角5、將姿態(tài)角通過NRF24L01射頻模塊發(fā)送至平臺

2.1kalman濾波

Kalman濾波是一種實(shí)時(shí)遞推算法，它所處理的是隨機(jī)信號，利用系統(tǒng)噪聲和觀測噪聲的統(tǒng)計(jì)特性，以系統(tǒng)的觀測作為濾波器的輸入，以所要估計(jì)值（狀態(tài)或參數(shù)）作為濾波器的輸出，濾波器輸入與輸出是由時(shí)間更新和觀測更新算法聯(lián)系在一起的，根據(jù)系統(tǒng)方程和觀測方程估計(jì)出所需要處理的信號實(shí)質(zhì)是一種最優(yōu)估計(jì)方法。

2.2四元數(shù)算法

概念：一個(gè)坐標(biāo)系到另一個(gè)坐標(biāo)系的變換可以通過繞一個(gè)定義在參考坐標(biāo)系中的矢量μ的單次轉(zhuǎn)動來實(shí)現(xiàn)。歐拉角的缺點(diǎn)，必須繞著不同的轉(zhuǎn)動軸旋轉(zhuǎn)。方向余弦法計(jì)算比較復(fù)雜。四元數(shù)是將旋轉(zhuǎn)描述為一次旋轉(zhuǎn)，且四元數(shù)微分方程計(jì)算方便。

2.3三維姿態(tài)仿真平臺

本系統(tǒng)是基于C#.net開發(fā)環(huán)境以O(shè)PENGL為插件開發(fā)的。OpenGL是行業(yè)領(lǐng)域中最為廣泛接納的 2D/3D 圖形 API，其自誕生至今已催生了各種計(jì)算機(jī)平臺及設(shè)備上的數(shù)千優(yōu)秀應(yīng)用程序。OpenGL是獨(dú)立于視窗操作系統(tǒng)或其它操作系統(tǒng)的，亦是網(wǎng)絡(luò)透明的。應(yīng)用在CAD、能源、娛樂、游戲開發(fā)、制造業(yè)及虛擬現(xiàn)實(shí)等行業(yè)領(lǐng)域中。

三、結(jié)論

本系統(tǒng)中MPU6050器件件存在許多隨機(jī)噪聲信號，普通的濾波算法無法實(shí)現(xiàn)對這類隨機(jī)噪聲的濾除，維納濾波又不適合在輕量級的ARM單片機(jī)上運(yùn)行。卡爾曼濾波算法可以很好的克服這兩個(gè)問題，從而還原出真實(shí)的信號。本系統(tǒng)采用的互補(bǔ)濾波型四元數(shù)姿態(tài)角解算算法適合在運(yùn)算量小，角度融合效果較好，能更真實(shí)的用加速度計(jì)信號去補(bǔ)償陀螺儀信號。OpenGL是行業(yè)領(lǐng)域中應(yīng)用最為廣泛的 2D/3D 圖形API，使用方便資源較多適合于初學(xué)者入門。

參 考 文 獻(xiàn)

[1]秦永元， 張洪鉞，王叔華.卡爾曼濾波與組合導(dǎo)航原理[M].西安：西北工業(yè)大學(xué)出版社，2012

[2]秦永元.慣性導(dǎo)航[M].北京：科學(xué)出版社，2014