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

朱豪坤

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

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

朱豪坤

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

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

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

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

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

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

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

2 角度的獲取

2.1 陀螺儀獲取角度

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

2.2 加速度計(jì)獲取角度

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

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

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

3.1 預(yù)測(cè)當(dāng)前角度值

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

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

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

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

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

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

3.4 計(jì)算當(dāng)前最優(yōu)化估算值

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

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

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

4 實(shí)驗(yàn)測(cè)試與結(jié)果

4.1 加速度干擾測(cè)試

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

4.2 角速度干擾測(cè)試

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

5 結(jié)語(yǔ)

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

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

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

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

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

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