基于嵌入式的姿態(tài)檢測系統(tǒng)設(shè)計

周 恒, 王民慧

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

0 引 言

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

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

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

2 軟件設(shè)計

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

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

圖1 系統(tǒng)總體設(shè)計框圖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 互補(bǔ)濾波器設(shè)計

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

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

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

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

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

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

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

2.3 卡爾曼濾波器的設(shè)計

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

狀態(tài)方程：

式中,Xk為系統(tǒng)k 時刻的狀態(tài)量,A 為系統(tǒng)從k- 1 時刻到k 時刻的狀態(tài)轉(zhuǎn)移矩陣,Uk為系統(tǒng)k 時刻的輸入量,B 為與之對應(yīng)的輸入控制加權(quán)矩陣,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 傳感器真實(shí)旋轉(zhuǎn)的角度,ω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)的預(yù)估值為公式(7)：

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

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

2.4 通信協(xié)議設(shè)計

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

表3 上位機(jī)與下位機(jī)之間的通信協(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ù)通過串口上傳至上位機(jī)進(jìn)行觀測。

3 實(shí)驗(yàn)測試

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

圖4 互補(bǔ)濾波器實(shí)驗(yàn)測試Fig. 4 Complementary filter experiment test

圖5 卡爾曼濾波器實(shí)驗(yàn)測試Fig. 5 Kalman filter experimental test

4 結(jié)束語

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