周 恒, 王民慧

(貴州大學 電氣工程學院, 貴陽550000)

0 引 言

隨著可穿戴嵌入式設備、無人機、VR 等技術的快速發(fā)展,物姿信息的求取成為了運動控制的前提條件和必要手段。 單一的傳感器檢測的姿態(tài)信息,存在計算的累計誤差或存在量測誤差,而無法得到物體姿態(tài)的準確信息［1］。 因此,本文設計了基于MPU6050 多軸姿態(tài)傳感器和STM32F103ZET6 單片機的姿態(tài)檢測系統(tǒng),通過多個傳感器對姿態(tài)進行檢測,借助互補濾波器及卡爾曼濾波器對多傳感器數(shù)據(jù)進行融合。 實驗驗證,兩種濾波器均可得到物體準確的姿態(tài)信息。

1 系統(tǒng)的總體設計

本文所設計的姿態(tài)檢測系統(tǒng)以STM32F103ZET6單片機為核心,通過MPU6050 多軸姿態(tài)傳感器對物體的運動姿態(tài)進行檢測,經(jīng)互補濾波器或卡爾曼濾波器處理后的數(shù)據(jù)即可在OLED 顯示屏上顯示,也可通過相應的通信協(xié)議,通過串口將處理前及處理后的數(shù)據(jù)發(fā)送至上位機進行觀測,得到物體的運動曲線。本系統(tǒng)的總體設計如圖1 所示。

2 軟件設計

2.1 系統(tǒng)軟件的總體設計

本姿態(tài)檢測系統(tǒng)的主程序流程圖如圖2 所示,主要由系統(tǒng)初始化程序、傳感器數(shù)據(jù)采集程序、量程轉換程序、互補濾波器程序、卡爾曼濾波器程序、按鍵掃描程序、OLED 顯示程序、通信協(xié)議程序等幾部分組成。 系統(tǒng)啟動后,首先對MPU6050 進行初始化,并判斷是否初始化成功。 如果初始化失敗,則OLED 顯示MPU6050Init error；如果初始化成功,則讀取MPU6050 傳感器采集的姿態(tài)并對按鍵進行檢測,如果key1 按鍵按下,則狀態(tài)標志位置1,并調(diào)用互補濾波器對進行量測轉換后的數(shù)據(jù)進行數(shù)據(jù)融合處理,得到物體實時姿態(tài)信息。 將得到的融合姿態(tài)信息及原始數(shù)據(jù)發(fā)送至上位機進行監(jiān)測；如果key2按鍵按下,則狀態(tài)標志位置2,調(diào)用卡爾曼濾波器對進行量測轉換后數(shù)據(jù)進行數(shù)據(jù)融合處理,將得到的融合姿態(tài)信息及原始數(shù)據(jù)經(jīng)通信協(xié)議打包發(fā)送至上位機進行檢測。

圖1 系統(tǒng)總體設計框圖Fig. 1 Block diagram of the overall design of the system

圖2 系統(tǒng)的主程序流程圖Fig. 2 Main program flow chart of the system

2.2 互補濾波器設計

互補濾波器就是根據(jù)不同傳感器特性,通過不同的高通濾波器或低通濾波器,相加得到整個頻帶的信號［2］。 針對MPU6050,由于加速度計對傳感器運動過程中的加速度比較敏感,取加速度計的瞬時值計算的姿態(tài)信息誤差較大；而陀螺儀測得的角速度經(jīng)過積分后得到的姿態(tài)信息不受傳感器運動過程中加速度的影響。 但隨著時間的推移,通過積分所得到的姿態(tài)信息便會有一個較大的累計誤差,可采用互補濾波器對兩個傳感器得到姿態(tài)信息進行校正濾波,進而得到一個準確的姿態(tài)信息。

本實驗所設計的互補濾波器以對陀螺儀數(shù)據(jù)積分得到的姿態(tài)信息為最優(yōu)值,定時對加速度計的數(shù)據(jù)采樣得到的姿態(tài)信息來校正陀螺儀得到的姿態(tài),通過加速度計濾除高頻信號,陀螺儀濾除低頻信號［3］,將陀螺儀和加速度計的優(yōu)點融合起來,使之在高頻段及低頻段都可以得到一個準確的姿態(tài)信息,其算法框圖如圖3 所示：

圖3 互補濾波器算法框圖Fig. 3 Block diagram of complementary filter algorithm

其互補濾波器的姿態(tài)信息輸出方程(1)：

式中,Angle 為互補濾波器輸出的傳感器的當前角度值,k 為對加速度計取值的權重,Y＿gyro 為陀螺儀的Y 軸輸出值,X＿acc 為加速度計X 軸輸出值,dt 為采樣周期。

本文中互補濾波器的初始參數(shù)設置如表1 所示：

表1 互補濾波器的初始值參數(shù)Tab. 1 Initial value parameters of complementary filter

2.3 卡爾曼濾波器的設計

首先,建立系統(tǒng)的狀態(tài)方程(2)及量測方程(3)如下：

狀態(tài)方程：

式中,Xk為系統(tǒng)k 時刻的狀態(tài)量,A 為系統(tǒng)從k- 1 時刻到k 時刻的狀態(tài)轉移矩陣,Uk為系統(tǒng)k 時刻的輸入量,B 為與之對應的輸入控制加權矩陣,Wk是系統(tǒng)k 時刻的過程演化噪聲,Zk為系統(tǒng)k 時刻量測向量,H 為系統(tǒng)k 時刻的量測矩陣,Vk為系統(tǒng)k 時刻的量測噪聲。

在本系統(tǒng)中,選擇陀螺儀X 軸方向上的角度值作為一個狀態(tài)向量,以加速度計傳感器的值估計得到的陀螺儀的常值偏差b 作為另一個狀態(tài)向量［4］,建立其量測傾角的模型(4)：

式中,θk為MPU6050 傳感器真實旋轉的角度,ωk為陀螺儀k 時刻的角速度,bk為陀螺儀k 時刻的常值偏差,vk為陀螺儀的噪聲, dt 為采樣周期。 根據(jù)式(2)～(4),建立離散化后的系統(tǒng)狀態(tài)方程和量測方程(5)：

式中,Q＿angle 為角度估算值的協(xié)方差,Q＿gyro 為陀螺儀位偏差的協(xié)方差,其數(shù)值表示卡爾曼濾波器對傳感器數(shù)據(jù)的信任程度,數(shù)值越小,表明信任程度越高。 因此,卡爾曼濾波遞推公式可概括為5 個核心公式(7) ～(11)：

系統(tǒng)此刻狀態(tài)的預估值為公式(7)：

本文中卡爾曼濾波器的初始參數(shù)設置如表2 所示。 經(jīng)多次試驗,該初始值設置使卡爾曼濾波器具有很好的魯棒特性和準確性。

表2 卡爾曼濾波器的初始參數(shù)值Tab. 2 Initial parameter values ofKalman filter

2.4 通信協(xié)議設計

為了便于學生對實驗數(shù)據(jù)的觀察,本系統(tǒng)不僅可通過oled 顯示屏查看MPU6050 傳感器的當前姿態(tài)信息,還可通過上位機以波形的形式查看傳感器的姿態(tài)變化情況,其下位機與上位機的通信協(xié)議如表3 所示：

表3 上位機與下位機之間的通信協(xié)議Tab. 3 Communication protocol between upper computer and lower computer

表中DATA 表示要發(fā)送的數(shù)據(jù),最大數(shù)據(jù)長度不超過28 個字節(jié),SUM 表示從幀頭0x88 一直到DATA 最后一個字節(jié)的和,通過該協(xié)議幀格式,可將數(shù)據(jù)通過串口上傳至上位機進行觀測。

3 實驗測試

通過兩種濾波器均有效濾除了陀螺儀傳感器產(chǎn)生的漂移誤差,得到物體的實際姿態(tài)情況,如圖4、圖5 所示。

圖4 互補濾波器實驗測試Fig. 4 Complementary filter experiment test

圖5 卡爾曼濾波器實驗測試Fig. 5 Kalman filter experimental test

4 結束語

本姿態(tài)檢測系統(tǒng)利用MPU6050 傳感器和STM32F103ZET6 單片機搭建,實現(xiàn)了MPU6050 傳感器六軸數(shù)據(jù)的讀取,通過量程轉換之后以本系統(tǒng)所設計的互補濾波器和卡爾曼濾波器對陀螺儀、加速度計傳感器的值進行數(shù)據(jù)融合,提高了物體姿態(tài)檢測的準確性。 該系統(tǒng)具有響應快、量測準確、數(shù)據(jù)顯示直觀、實時性好等特點,可廣泛應用于本科實驗教學,增強學生對互補濾波和卡爾曼濾波算法的理解以及學生對STM32 單片機的應用。