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

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

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

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

一、引 言

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

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

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

系統(tǒng)工作流程

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

2.1kalman濾波

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

2.2四元數(shù)算法

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

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

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

三、結(jié)論

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

參 考 文 獻

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

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