基于卡爾曼濾波的三維空間姿態仿真系統設計

喻順志 諶雅潔 石銀標 劉靜斯

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

【關鍵詞】 STM32 Kalman濾波 四元數 nrf24l01 姿態仿真

一、引 言

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

二、系統方案設計

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

系統工作流程

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

2.1kalman濾波

Kalman濾波是一種實時遞推算法，它所處理的是隨機信號，利用系統噪聲和觀測噪聲的統計特性，以系統的觀測作為濾波器的輸入，以所要估計值（狀態或參數）作為濾波器的輸出，濾波器輸入與輸出是由時間更新和觀測更新算法聯系在一起的，根據系統方程和觀測方程估計出所需要處理的信號實質是一種最優估計方法。

2.2四元數算法

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

2.3三維姿態仿真平臺

本系統是基于C#.net開發環境以OPENGL為插件開發的。OpenGL是行業領域中最為廣泛接納的 2D/3D 圖形 API，其自誕生至今已催生了各種計算機平臺及設備上的數千優秀應用程序。OpenGL是獨立于視窗操作系統或其它操作系統的，亦是網絡透明的。應用在CAD、能源、娛樂、游戲開發、制造業及虛擬現實等行業領域中。

三、結論

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

參 考 文 獻

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

[2]秦永元.慣性導航[M].北京：科學出版社，2014