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

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

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

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

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

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

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)成一個長方形，根據(jù)平行四邊形的原理可以得出： 角c=角d 所以計算出角度d就等效于計算出了x軸與桌面的夾角a。前邊已經(jīng)說過gx是g在x軸的分量，那么根據(jù)正弦定理就可以得出： Sind=gx/g 得到這個公式可是還是得不到想要的角度，因為需要計算反正弦，而反正弦在單片機(jī)里不是很好計算。 為了得到角度，于是又查了相關(guān)資料，

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

8、x=k*x*3.14/180 從上邊的函數(shù)對比圖可以看出，當(dāng)系數(shù)取0.92時，角度范圍可以擴(kuò)大到-4545度。 經(jīng)過這一系列的分析，終于得到角度換算方法： 由 Sind=gx/g Sind=k*d*3.14/180 得到： gx/g=k*d*3.14/180 那么角度就可以通過如下公式計算出： d=180*gx/(k*g*3.14) 而gx可以從加速度計里讀出來，所以這下角度就可以輕松得到了。而之前也說過這個角度不是很精確，但是至少可以反映出角度變化的趨勢。不過可以通過卡爾曼濾波等算法把加速度計讀出的角度和陀螺儀讀出的角度結(jié)合起來，使小車的角度更加準(zhǔn)確。 3、 陀螺儀 通過陀螺儀來測量角度就很

9、簡單了，因為陀螺儀讀出的是角速度，大家都知道，角速度乘以時間，就是轉(zhuǎn)過的角度。把每次計算出的角度做累加就會等到當(dāng)前所在位置的角度。先看下圖：Arduino代碼 在開始之前先介紹一下用到的組件和算法。Mpu6050是通過i2c接口和arduino連接，所以需要i2c庫，另外mpu6050也有現(xiàn)成的庫。由于陀螺儀本身就有偏差，多次積累之后偏差會越來越大越來越不準(zhǔn)確，所以需要用一些濾波算法來校正，這里用的是卡爾曼濾波算法。 卡爾曼濾波在網(wǎng)上講的很多，這里就不多說了。網(wǎng)上有一個測量溫度的通俗版，另外維基百科里的介紹比較簡單。有興趣的可以去研究一下。另外也可以用其它算法來濾波，這里也只是做演示所以不多介

10、紹。 /ArduinoWirelibraryisrequiredifI2CdevI2CDEV_ARDUINO_WIREimplementation/isusedinI2Cdev.h#includeWire.h /I2CdevandMPU6050mustbeinstalledaslibraries,orelsethe.cpp/.hfiles/forbothclassesmustbeintheincludepathofyourproject#includeI2Cdev.h#includeMPU6050.h /classdefaultI2Caddressis0x68 /specificI2Caddr

11、essesmaybepassedasaparameterhere/AD0low=0x68(defaultforInvenSenseevaluationboard)/AD0high=0x69MPU6050accelgyro; int16_tax,ay,az;int16_tgx,gy,gz;doubletotal_angle=0;#defineLED_PIN13 /*把mpu6050放在水平桌面上，分別讀取讀取2000次，然后求平均值*/#defineAX_ZERO(-1476)/*加速度計的0偏修正值*/#defineGX_ZERO(-30.5)/*陀螺儀的0偏修正值*/ voidsetup()

12、 /joinI2Cbus(I2Cdevlibrarydoesntdothisautomatically)Wire.begin();/initializeserialcommunication/(38400chosenbecauseitworksaswellat8MHzasitdoesat16MHz,but/itsreallyuptoyoudependingonyourproject)Serial.begin(38400);/initializedevice Serial.println(InitializingI2Cdevices.);accelgyro.initialize();/verif

13、yconnection Serial.println(Testingdeviceconnections.); Serial.println(accelgyro.testConnection()?MPU6050connectionsuccessful:MPU6050connectionfailed); /*通過卡爾曼濾波得到的最終角度*/floatAngle=0.0; /*由角速度計算的傾斜角度*/floatAngle_gy=0.0; floatQ_angle=0.9;floatQ_gyro=0.001;floatR_angle=0.5; floatdt=0.01;/*dt為kalman濾波器采

14、樣時間;*/charC_0=1; floatQ_bias,Angle_err; floatPCt_0=0.0,PCt_1=0.0,E=0.0; floatK_0=0.0,K_1=0.0,t_0=0.0,t_1=0.0;floatPdot4=0,0,0,0; floatPP22=1,0,0,1; /*卡爾曼濾波函數(shù),具體實現(xiàn)可以參考網(wǎng)上資料,也可以使用其它濾波算法*/voidKalman_Filter(floatAccel,floatGyro)Angle+=(Gyro-Q_bias)*dt;Pdot0=Q_angle-PP01-PP10;Pdot1=-PP11;Pdot2=-PP11;Pdot3

15、=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!=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

16、loop() / read raw accel/gyro measurements from device double ax_angle=0.0; double gx_angle=0.0; unsigned long 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);

17、 /* 加速度量程范圍設(shè)置2g 16384 LSB/g * 計算公式： * 前邊已經(jīng)推導(dǎo)過這里再列出來一次 * x是小車傾斜的角度,y是加速度計讀出的值 * 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 * 陀螺儀角度計算公式: /* 小車傾斜角度是gx_angle,陀螺儀讀數(shù)是y,時間是dt * gx_angle +=(y/(131*1000)*dt */ gy -= GX_ZERO; time

18、=mictime-pretime; gyro=gy/131.0; gx_angle=gyro*time; gx_angle=gx_angle/1000.0; total_angle-=gx_angle; dt=time/1000.0; Kalman_Filter(ax_angle,gyro); Serial.print(ax_angle); Serial.print(,); Serial.print(total_angle); Serial.print(,); Serial.println(Angle); pretime=mictime; Mpu6050取值范圍調(diào)整的方法,修改MPU6050庫中的mpu6050.cpp,修改initialize函數(shù)中標(biāo)成紅色的行。 void MPU6050:initialize() setClockSource(MPU6050_CLOCK_PLL_XGYRO); setFullScaleGyroRange(MPU6050_GYRO_FS_250)