MPU6050數(shù)據(jù)輕松分析

1、MPU6050數(shù)據(jù)輕松分析這個(gè)文章是根據(jù)自己學(xué)習(xí)，查資料的匯總，同時(shí)把一些自己的心得加進(jìn)去。如果有什么不對的，歡迎請大家指正、交流。郵箱：zhb_account最近看到加速度計(jì)和陀螺儀比較火，而且也有很多人都在研究。于是也在網(wǎng)上淘了一個(gè)mpu6050模塊，想用來做自平衡小車?？墒鞘褂闷饋砭桶l(fā)愁了。網(wǎng)上關(guān)于mpu6050的資料的確不少，但是大家都是互相抄襲，然后貼出一段程序，看完之后還是不知道所以然。經(jīng)過翻閱各個(gè)方面的資料，以及自己的研究在處理mpu6050數(shù)據(jù)方面有一些心得，在這里和大家分享一下。在處理加速度計(jì)和陀螺儀用到的方法都是比較簡單的，這里的簡單并不是不需要任何基礎(chǔ)知識，只是這些基本知

2、識都是最基本的，比如簡單的三角函數(shù)，數(shù)學(xué)計(jì)算，物理知識，c語言以及基本的arduino知識（如果不會(huì)arduino會(huì)其它單片機(jī)也是一樣的，本文實(shí)踐是使用arduino），如果還不具備這些知識那就快去補(bǔ)課吧。1、 加速度和陀螺儀原理當(dāng)然，在開始之前至少要弄懂什么是加速度計(jì)，什么是陀螺儀吧，否則那后邊講的都是沒有意義的。簡單的說，加速度計(jì)主要是測量物體運(yùn)動(dòng)的加速度，陀螺儀主要測量物體轉(zhuǎn)動(dòng)的角速度。這些理論的知識我就不多說了，都可以在網(wǎng)上查到。這里推薦一篇講的比較詳細(xì)的文章A Guide To using IMU (Accelerometer and Gyroscope Devices) in Em

3、bedded Applications，在網(wǎng)上可以直接搜索到。2、 加速度測量在開始之前，不知大家是否還記得加速度具有合成定理？如果不記得可以先大概了解一下，其實(shí)簡單的舉個(gè)例子來說就是重力加速度可以理解成是由x,y,z三個(gè)方向的加速度共同作用的結(jié)果。反過來說就是重力加速度可以分解成x,y,z三個(gè)方向的加速度。加速度計(jì)可以測量某一時(shí)刻x,y,z三個(gè)方向的加速度值。而自平衡小車?yán)眉铀俣扔?jì)測出重力加速度在x,y,z軸的分量，然后利用各個(gè)方向的分量與重力加速度的比值來計(jì)算出小車大致的傾角。其實(shí)在自平衡小車上非靜止的時(shí)候，加速度計(jì)測出的結(jié)果并不是非常精確。因?yàn)榇蠹以诟咧形锢淼臅r(shí)候都學(xué)過，物體時(shí)刻都會(huì)受

4、到地球的萬有引力作用產(chǎn)生一個(gè)向下的重力加速度，而小車在動(dòng)態(tài)時(shí)，受電機(jī)的作用肯定有一個(gè)前進(jìn)或者后退方向的作用力，而加速度計(jì)測出的結(jié)果是，重力加速度與小車運(yùn)動(dòng)加速度合成得到一個(gè)總的加速度在三個(gè)方向上的分量。不過我們暫時(shí)不考慮電機(jī)作用產(chǎn)生的運(yùn)動(dòng)加速度對測量結(jié)果的影響。因?yàn)槲覀円劝褟?fù)雜的事情分解成一個(gè)個(gè)簡單的事情來分析，這樣才能看到成果，才會(huì)有信心繼續(xù)。下邊我們就開始分析從加速度得到角度的方法。如下圖，把加速度計(jì)平放，分別畫出xyz軸的方向。這三個(gè)軸就是我們后邊分析所要用到的坐標(biāo)系。把mpu6050安裝在自平衡車上時(shí)也是這樣的水平安裝在小車底盤上的，假設(shè)兩個(gè)車輪安裝時(shí)車軸和y軸在一條直線上。那么小車

5、擺動(dòng)時(shí)，參考水平面就是桌面，并且車軸（y軸）與桌面始終是平行的，小車擺動(dòng)和移動(dòng)過程中y軸與桌面的夾角是不會(huì)發(fā)生變化的，一直是0度。發(fā)生變化的是x軸與桌面的夾角以及z軸與桌面的夾角，而且桌面與x軸z軸夾角變化度數(shù)是一樣的。所以我們只需要計(jì)算出x軸和z軸中任意一個(gè)軸的夾角就可以反映出小車的傾斜的情況了。為了方便分析，由于y軸與桌面夾角始終不變，我們從y軸的方向俯看下去，那么這個(gè)問題就會(huì)簡化成只有x軸和z軸的二維關(guān)系。假設(shè)某一時(shí)刻小車上加速度計(jì)（mpu6050）處于如下狀態(tài)，下圖是我們看到簡化后的模型。在這個(gè)圖中，y軸已經(jīng)簡化和坐標(biāo)系的原點(diǎn)o重合在了一起。我們來看看如何計(jì)算出小車的傾斜角，也就是與桌

6、面的夾角a。上圖g是重力加速度，gx、gz分別是g在x軸和z軸的分量。由于重力加速度是垂直于水平面的，得到：角a+角b=90度X軸與y軸是垂直關(guān)系，得到：角c+角b=90度于是輕松的就可以得出：角a=角c根據(jù)力的分解，g、gx、gz三者構(gòu)成一個(gè)長方形，根據(jù)平行四邊形的原理可以得出：角c=角d所以計(jì)算出角度d就等效于計(jì)算出了x軸與桌面的夾角a。前邊已經(jīng)說過gx是g在x軸的分量，那么根據(jù)正弦定理就可以得出：Sind=gx/g得到這個(gè)公式可是還是得不到想要的角度，因?yàn)樾枰?jì)算反正弦，而反正弦在單片機(jī)里不是很好計(jì)算。為了得到角度，于是又查了相關(guān)資料，原來在角度較小的情況下，角度的正弦與角度對應(yīng)的弧度成

7、線性關(guān)系。先看看下邊的圖：這個(gè)圖x軸是角度，取值范圍是090度，有三個(gè)函數(shù)曲線，分別是：Y=sinx 正弦曲線Y=x*3.14/180 弧度Y=0.92*x*3.14/180 乘以一個(gè)0.92系數(shù)的弧度從圖上可以看出，當(dāng)角度范圍是029度時(shí)：sinx=x*3.14/180對于自平衡車來說，小車的擺動(dòng)范圍在-2929度之內(nèi)，如果超過這個(gè)范圍，小車姿態(tài)也無法調(diào)整，所以對于自平衡小車sinx=x*3.14/180基本上是成立的。當(dāng)然有時(shí)候也會(huì)擔(dān)心-2929度的搖擺范圍還是無法滿足需求。那可以給上邊的公式乘一個(gè)系數(shù)。得到如下公式：Sinx=k*x*3.14/180從上邊的函數(shù)對比圖可以看出，當(dāng)系數(shù)取0

8、.92時(shí)，角度范圍可以擴(kuò)大到-4545度。經(jīng)過這一系列的分析，終于得到角度換算方法：由Sind=gx/gSind=k*d*3.14/180得到：gx/g=k*d*3.14/180那么角度就可以通過如下公式計(jì)算出：d=180*gx/(k*g*3.14)而gx可以從加速度計(jì)里讀出來，所以這下角度就可以輕松得到了。而之前也說過這個(gè)角度不是很精確，但是至少可以反映出角度變化的趨勢。不過可以通過卡爾曼濾波等算法把加速度計(jì)讀出的角度和陀螺儀讀出的角度結(jié)合起來，使小車的角度更加準(zhǔn)確。3、 陀螺儀通過陀螺儀來測量角度就很簡單了，因?yàn)橥勇輧x讀出的是角速度，大家都知道，角速度乘以時(shí)間，就是轉(zhuǎn)過的角度。把每次計(jì)算出

9、的角度做累加就會(huì)等到當(dāng)前所在位置的角度。先看下圖：假設(shè)最初陀螺儀是與桌面平行，單片機(jī)每tms讀一次陀螺儀的角速度，當(dāng)讀了三次角速度以后 z軸轉(zhuǎn)到上圖的位置，則在這段時(shí)間中轉(zhuǎn)過的角度為x：角x=角1+角2+角3假設(shè)從陀螺儀讀出的角速度為w，那總角度為：X=(w1*t1+w2*t2+w3*t3)/1000假設(shè)經(jīng)過n次，那么總的角度如下：X=(w1*t1+w2*t2+w3*t3+wn*tn)/1000實(shí)際上這就是一個(gè)積分過程。其實(shí)這種計(jì)算出來的角度也存在一定的誤差，而且總的角度是經(jīng)過多次相加得到的，這樣誤差就會(huì)越積累越大，最終導(dǎo)致計(jì)算出的角度與實(shí)際角度相差很大。于是也可以使用卡爾曼濾波把加速度計(jì)讀出

10、的角度結(jié)合在一起，使計(jì)算出的角度更準(zhǔn)確。4、 mpu6050與arduino連接為了方便大家學(xué)習(xí)和復(fù)制，同時(shí)也應(yīng)大家的要求，詳細(xì)列出從哪里可以找到，對比了很多覺得這個(gè)是最劃算和實(shí)用的，供大家參考名稱來源Mpu6050模塊Arduino模塊其它Mpu6050可以直接買現(xiàn)成的模塊，模塊使用i2c接口和arduino通信，連接方式如下：Mpu6050ArduinoVCCVCCGNDGNDSCLAD5SDAAD45、 Arduino代碼在開始之前先介紹一下用到的組件和算法。Mpu6050是通過i2c接口和arduino連接，所以需要i2c庫，另外mpu6050也有現(xiàn)成的庫。由于陀螺儀本身就有偏差，多次

11、積累之后偏差會(huì)越來越大越來越不準(zhǔn)確，所以需要用一些濾波算法來校正，這里用的是卡爾曼濾波算法?？柭鼮V波在網(wǎng)上講的很多，這里就不多說了。網(wǎng)上有一個(gè)測量溫度的通俗版，另外維基百科里的介紹比較簡單。有興趣的可以去研究一下。另外也可以用其它算法來濾波，這里也只是做演示所以不多介紹。/ Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation/ is used in I2Cdev.h#include "Wire.h"/ I2Cdev and MPU6050 must be install

12、ed as libraries, or else the .cpp/.h files/ for both classes must be in the include path of your project#include "I2Cdev.h"#include "MPU6050.h"/ class default I2C address is 0x68/ specific I2C addresses may be passed as a parameter here/ AD0 low = 0x68 (default for InvenSense eva

13、luation board)/ AD0 high = 0x69MPU6050 accelgyro;int16_t ax, ay, az;int16_t gx, gy, gz;double total_angle=0;#define LED_PIN 13/* 把mpu6050放在水平桌面上，分別讀取讀取2000次，然后求平均值 */#define AX_ZERO (-1476) /* 加速度計(jì)的0偏修正值 */#define GX_ZERO (-30.5) /* 陀螺儀的0偏修正值 */void setup() / join I2C bus (I2Cdev library doesn't

14、 do this automatically) Wire.begin(); / initialize serial communication / (38400 chosen because it works as well at 8MHz as it does at 16MHz, but / it's really up to you depending on your project) Serial.begin(38400); / initialize device Serial.println("Initializing I2C devices."); acc

15、elgyro.initialize(); / verify connection Serial.println("Testing device connections."); Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");/* 通過卡爾曼濾波得到的最終角度 */float Angle=0.0; /*由角速度計(jì)算的傾斜角度 */float Angle_gy=0.0; fl

16、oat Q_angle=0.9; float Q_gyro=0.001;float R_angle=0.5;float dt=0.01;/* dt為kalman濾波器采樣時(shí)間; */char C_0 = 1;float Q_bias, Angle_err;float PCt_0=0.0, PCt_1=0.0, E=0.0;float K_0=0.0, K_1=0.0, t_0=0.0, t_1=0.0;float Pdot4 =0,0,0,0;float PP22 = 1, 0 , 0, 1 ;/* 卡爾曼濾波函數(shù),具體實(shí)現(xiàn)可以參考網(wǎng)上資料,也可以使用其它濾波算法 */void Kalman_

17、Filter(float Accel,float Gyro)Angle+=(Gyro - Q_bias) * dt; Pdot0=Q_angle - PP01 - PP10; Pdot1=- PP11;Pdot2=- PP11;Pdot3=Q_gyro;PP00 += Pdot0 * dt; PP01 += Pdot1 * dt; PP10 += Pdot2 * dt;PP11 += Pdot3 * dt;Angle_err = Accel - Angle;PCt_0 = C_0 * PP00;PCt_1 = C_0 * PP10;E = R_angle + C_0 * PCt_0; if(E

18、!=0) K_0 = PCt_0 / E; K_1 = PCt_1 / E; t_0 = PCt_0;t_1 = C_0 * PP01;PP00 -= K_0 * t_0;PP01 -= K_0 * t_1;PP10 -= K_1 * t_0;PP11 -= K_1 * t_1;Angle+= K_0 * Angle_err;Q_bias+= K_1 * Angle_err;void loop() / read raw accel/gyro measurements from device double ax_angle=0.0; double gx_angle=0.0; unsigned l

19、ong time=0; unsigned long mictime=0; static unsigned long pretime=0; float gyro=0.0; if(pretime=0) pretime=millis(); return; mictime=millis(); accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);/* 加速度量程范圍設(shè)置2g 16384 LSB/g* 計(jì)算公式：* 前邊已經(jīng)推導(dǎo)過這里再列出來一次* x是小車傾斜的角度,y是加速度計(jì)讀出的值* sinx = 0.92*3.14*x/180 = y/16384* x=180*y/(0.92*3.14*16384)=*/ ax -= AX_ZERO;ax_angle=ax/262; /* 陀螺儀量程范圍設(shè)置250 131 LSB/s * 陀螺儀角度計(jì)算公式:* 小車傾斜角度是gx_angle,陀螺儀讀數(shù)是y,時(shí)間是dt* gx_angle +=(y/(131*1000)*dt */ gy -= GX_ZERO; time=mictime-pretime; gyro=gy/131.0; gx_angle=gyro*time;gx_angle=gx_angle/1000.0;total_angle