基于姿態(tài)傳感器MPU6050的卡爾曼濾波應(yīng)用

朱豪坤

（中北大學(xué)機電工程學(xué)院，山西太原 030051）

基于姿態(tài)傳感器MPU6050的卡爾曼濾波應(yīng)用

朱豪坤

（中北大學(xué)機電工程學(xué)院，山西太原 030051）

現(xiàn)在風(fēng)靡一時的六軸姿態(tài)傳感器MPU6050，單獨使用三軸加速度計或者三軸陀螺儀都很難得到十分準確的姿態(tài)數(shù)據(jù)，且傳感器容易受到電場、磁場等因素的干擾，使數(shù)據(jù)不穩(wěn)定。本文研究利用MPU6050獲取相應(yīng)位置的角加速度和加速度，通過幾何運算分別將陀螺儀和加速度計所獲得的數(shù)據(jù)轉(zhuǎn)換成相應(yīng)的兩個角度，介紹了卡爾曼濾波（Kalman filtering）算法對多慣性數(shù)據(jù)進行融合的實際運用［1］，在單片機平臺上利用卡爾曼濾波算法整合由陀螺儀和加速度計獲得的數(shù)據(jù)，過濾掉相關(guān)外在噪聲，獲得姿態(tài)數(shù)據(jù)的最優(yōu)估計。

MPU6050模塊 卡爾曼濾波 三軸加速度計 三軸陀螺儀 STM32F103ZET6單片機

隨著微機電系統(tǒng)（MEMS）的發(fā)展，低成本的慣性傳感器得到了廣泛的使用，MPU6050就是典型的代表。MPU6050集成了MEMS三軸加速度計和MEMS三軸陀螺儀，是全球首例整合性6軸運動處理組件，被廣泛應(yīng)用于智能型手機等電子產(chǎn)品。在使用MPU6050時，單獨通過陀螺儀和加速度計的數(shù)據(jù)都難以獲得準確的角度數(shù)據(jù)，除了傳感器自身的數(shù)據(jù)誤差、計算誤差之外，外界電場、磁場等對傳感器的干擾也很大?？柭鼮V波很適用于在計算機上運行，我們應(yīng)用STM32單片機運行卡爾曼濾波算法，將陀螺儀和加速度計的相關(guān)數(shù)據(jù)輸入卡爾曼濾波系統(tǒng)，通過系統(tǒng)中的線性系統(tǒng)狀態(tài)方程，輸出觀測數(shù)據(jù)，對系統(tǒng)狀態(tài)進行最優(yōu)估計，從輸入的存在測量噪聲的數(shù)據(jù)中，估計動態(tài)系統(tǒng)的狀態(tài)，獲得最優(yōu)估計的姿態(tài)數(shù)據(jù)［2］，并對現(xiàn)場采集的數(shù)據(jù)進行實時的更新和處理。

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

圖1是本文研究的系統(tǒng)方框圖，STM32是一款32位的單片機，處理速度快、功能強大，具有豐富的外圍接口，STM32單片機通過一個IIC接口與MPU6050相連，通過IIC讀取MPU6050陀螺儀和加速度計的數(shù)據(jù)，同時運行卡爾曼濾波系統(tǒng)，單片機每隔時間t中斷一次，連續(xù)從MPU6050中讀出的數(shù)據(jù)輸入卡爾曼濾波系統(tǒng)，并且將卡爾曼濾波輸出后的數(shù)據(jù)通過串口傳給電腦上位機，在PC端整理好上位機獲得的數(shù)據(jù)，并將其導(dǎo)入MATLAB中，在MATLAB中繪制出坐標圖，便于我們更直觀的分析數(shù)據(jù)，這是我們的硬件部分的大體結(jié)構(gòu)框架。

軟件部分主要包括以下結(jié)構(gòu)：首先，由宏定義，用于驅(qū)動MPU6050的底層驅(qū)動程序，實現(xiàn)卡爾曼濾波的算法程序等構(gòu)成；其次，有用于STM32讀取MPU6050數(shù)據(jù)的IIC通信程序，定時將MPU6050數(shù)據(jù)換算成角度的幾何換算，并將角度導(dǎo)入卡爾曼濾波系統(tǒng)進行卡爾曼濾波的定時器中斷程序；另外，還有將濾波后的數(shù)據(jù)傳輸給電腦上位機端的串口通信程序，和將整理好的數(shù)據(jù)轉(zhuǎn)換成坐標圖的MATLAB處理程序。

2 角度的獲取

2.1 陀螺儀獲取角度

如圖2，通過陀螺儀獲取的角度αa，通過陀螺儀可以讀出三個軸的角速度Gx、Gy、Gz，角速度乘以時間就是這段時間內(nèi)轉(zhuǎn)過的角度，把每次計算的角度進行累加，從而得到當前所在位置的角度。如下圖：以x軸為例，用x軸的角速度Gx乘以單片機讀取陀螺儀數(shù)據(jù)的時間間隔t就得到了這段時間內(nèi)轉(zhuǎn)過的角度α1，即α1=Gx*t，把每次x軸轉(zhuǎn)過的角度α1、α2…αn進行加減運算，就可以得到當前時刻的角度，即αa=α1+α2+…αn。這種計算方法存在一定的誤差，而且總的角度是通過累加的方法得到，相當于一個積分過程，所以累加次數(shù)越多，誤差就會越大，最終可能導(dǎo)致測出來的角度與實際角度相差很大。

2.2 加速度計獲取角度

如圖3，通過加速度計獲取的的角度αb，重力加速度可以分解成x，y，z三個方向的加速度gx，gy，gz，加速度計可以測量某一時刻x，y，z三個方向的加速度值A(chǔ)x，Ay，Az。這里我們暫時不考慮傳感器運動的影響，假設(shè)重力加速度在三個軸方向的加速度分量等于三個軸的加速度，即Ax=gx，Ay=gy，Az=gz，利用重力在各個方向的分量的幾何關(guān)系來計算出傳感器大致的傾角。如圖3：以x軸為例，αb為x軸與水平面的夾角，∠d為z軸與豎直方向的夾角，g為重力角速度，αb=∠d，∠d的正弦tand=Ax/Az，所以αb=∠d=arctan（Ax/ Az）。只要通過加速度計獲取相應(yīng)軸實時的加速度，就可獲得相應(yīng)軸的角度。因為物體時刻都會受到一個向下的重力加速度，而傳感器在動態(tài)時，會受到其他方向的作用力，此時加速度計測出的結(jié)果是重力加速度與傳感器運動加速度合成得到一個總的加速度在三個方向上的分量。所以在傳感器運動的時候，通過加速度計這樣測出的結(jié)果并不是非常精確。

3 卡爾曼濾波算法的應(yīng)用

通過陀螺儀和加速度計獲得的角度都存在一定的誤差，陀螺儀獲得角度是一個積分的過程，累加次數(shù)越多，誤差就越大。通過加速度計獲得角度，在姿態(tài)傳感器非靜止的時候，加速度計獲取的數(shù)據(jù)是重力加速度與運動加速度的和值，獲取的數(shù)據(jù)本身具有一定誤差。在此我們使用卡爾曼濾波算法將陀螺儀和加速度計獲取的數(shù)據(jù)融合，不斷根據(jù)實時數(shù)據(jù)把協(xié)方差矩陣遞歸，從而估算出最優(yōu)的角度值，它只保留了上一時刻的協(xié)方差矩陣，可以隨不同的時刻的數(shù)據(jù)而改變卡爾曼增益系數(shù)的值，從而可以根據(jù)不同時刻的數(shù)據(jù)進行最優(yōu)估計［3］。通過調(diào)節(jié)卡爾曼濾波中的系數(shù)Q，R可以調(diào)節(jié)系統(tǒng)對陀螺儀數(shù)據(jù)和對加速度計數(shù)據(jù)的信任度，調(diào)參數(shù)也是卡爾曼濾波中很重要的一個環(huán)節(jié)。以下是詳細的實施步驟［4］：

3.1 預(yù)測當前角度值

首先預(yù)測當前角度值，我們認為此時的角度可以近似認為是上一時刻的角度值加上上一時刻陀螺儀測得的角加速度值乘以時間，因為Angle=Gyro*t，但是陀螺儀有個靜態(tài)漂移Q_gyro，這個值沒有意義的，計算時要把它減去。由此我們得到了當前角度的預(yù)測值A(chǔ)ngle。

其中等號左邊Angle為此時預(yù)測的角度，等號右邊Angle為上一時刻預(yù)測的角度，Gyro 為陀螺儀測的角速度的值，t是兩次濾波之間的時間間隔。Q_gyro也是一個變化的量，但是就預(yù)測來說認為現(xiàn)在的漂移跟上一時刻是相同的即Q_gyro=Q_gyro。

3.2 預(yù)測協(xié)方差矩陣

第二步是預(yù)測方差陣的預(yù)測值，Q_Gyro是漂移的噪聲，Q_Angle是角度值的噪聲為系統(tǒng)噪聲協(xié)方差矩陣，方差值D（Q_Angle）、D（Q_Gyro）為指定常數(shù)，所以系統(tǒng)噪聲協(xié)方差矩陣是已知的，通過設(shè)置系統(tǒng)噪聲協(xié)方差矩陣Q可以設(shè)置系統(tǒng)對陀螺儀的信任度。

其中等號左邊的abcd為此次預(yù)測的協(xié)方差矩陣元素，等號右邊abcd為上一次的預(yù)測的協(xié)方差矩陣元素。

3.3 計算卡爾曼增益系數(shù)

3.4 計算當前最優(yōu)化估算值

通過卡爾曼增益對數(shù)據(jù)進行修正，其中Acc_Angle是加速度計計算得來的角度，為測量值，右邊為當前的預(yù)測值，通過結(jié)合預(yù)測值和測量值，我們可以得到顯著狀態(tài)的最優(yōu)化估計值，即左邊的

3.5 更新協(xié)方差矩陣

為了要另卡爾曼濾波器不斷的運行下去直到系統(tǒng)過程結(jié)束，我們要更新協(xié)方差矩陣。

4 實驗測試與結(jié)果

4.1 加速度干擾測試

在加速度干擾測試中，將MPU6050傳感器水平放置，對傳感器進行Y軸向的震蕩作用，即對Y軸方向施加前后變化的加速度，單片機定時器采樣周期為t=1ms，采樣1000次，采集此時MPU6050數(shù)據(jù)［5］，將數(shù)據(jù)導(dǎo)入MATLAB得到圖4。其中藍色曲線為只用加速度計的情況下測得的傳感器的角度變化，紅色曲線為卡爾曼濾波融合加速度計數(shù)據(jù)和陀螺儀數(shù)據(jù)所得角度變化?？梢钥闯觯{色曲線比較粗糙，紅色曲線相對光滑，且藍色曲線振幅高紅色曲線很多，說明使用卡爾曼濾波后，平移運動時動態(tài)噪聲對角度的干擾減少了很多，數(shù)據(jù)更加準確。

4.2 角速度干擾測試

在角度的干擾測試中，以MPU6050的X軸為旋轉(zhuǎn)中心，使傳感器繞X軸進行左右往復(fù)翻轉(zhuǎn)運動，單片機定時器采樣周期為t=1ms，采樣1000次，采集此時MPU6050的數(shù)據(jù)，導(dǎo)入MATLAB中，得出圖5。其中綠色曲線為用陀螺儀測得的傳感器的角度變化，紅色曲線為卡爾曼濾波融合加速度計數(shù)據(jù)和陀螺儀數(shù)據(jù)所得角度變化?？梢钥闯?，綠色曲線的震蕩幅度比紅色曲線大很多，數(shù)據(jù)較為不穩(wěn)定，說明使用卡爾曼濾波后，很好的減少了翻轉(zhuǎn)運動時動態(tài)噪聲對角度的干擾，使姿態(tài)數(shù)據(jù)更接近實際數(shù)據(jù)。

5 結(jié)語

通過上述分析和實驗的測試，我們可以發(fā)現(xiàn)通過使用卡爾曼濾波算法對加速度傳感器數(shù)據(jù)和陀螺儀數(shù)據(jù)進行融合，實時地改變測量噪聲協(xié)方差的值，很好的避免了動態(tài)噪聲對加速度傳感器的影響，提高了MPU6050傳感器姿態(tài)數(shù)據(jù)測量的精度，為MPU6050姿態(tài)數(shù)據(jù)的修正提供了很好的方法，也為MPU6050在實際應(yīng)用方面解決了很多問題，提高了MPU6050在使用中的性能，提供更精確的數(shù)據(jù)，具有一定的應(yīng)用價值。

［1］邱云平，伍寶玉.MPU-6050模塊角度算法處理及在嵌入式中的應(yīng)用［J］.江西科技學(xué)院學(xué)報，2014（2）∶26-29.

［2］王帥，魏國.卡爾曼濾波在四旋翼飛行器姿態(tài)測量中的應(yīng)用［J］.兵工自動化，2011（1）∶73-74+80.

［3］張道德，王強.Kalman濾波算法在自平衡機器人中的應(yīng)用.湖北工業(yè)大學(xué)學(xué)報，2012（5）∶1-5.

［4］傅忠云，朱海霞，孫金秋，劉文波.基于慣性傳感器MPU6050的濾波算法研究［J］.壓電與聲光，2015（5）∶821-825+829.

［5］賴義漢，王凱.基于MPU6050的雙輪平衡車控制系統(tǒng)設(shè)計［J］.河南工程學(xué)院學(xué)報，2014（1）∶53-57.