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

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

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

一、引 言

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

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

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

系統(tǒng)工作流程

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

2.1kalman濾波

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

2.2四元數(shù)算法

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

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

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

三、結(jié)論

本系統(tǒng)中MPU6050器件件存在許多隨機(jī)噪聲信號(hào)，普通的濾波算法無(wú)法實(shí)現(xiàn)對(duì)這類隨機(jī)噪聲的濾除，維納濾波又不適合在輕量級(jí)的ARM單片機(jī)上運(yùn)行?？柭鼮V波算法可以很好的克服這兩個(gè)問(wèn)題，從而還原出真實(shí)的信號(hào)。本系統(tǒng)采用的互補(bǔ)濾波型四元數(shù)姿態(tài)角解算算法適合在運(yùn)算量小，角度融合效果較好，能更真實(shí)的用加速度計(jì)信號(hào)去補(bǔ)償陀螺儀信號(hào)。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