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

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

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

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

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

5、軸在一條直線上。那么小車擺動(dòng)時(shí)，參考水平面就是桌面，并且車軸（y軸）與桌面始終是平行的，小車擺動(dòng)和移動(dòng)過(guò)程中y軸與桌面的夾角是不會(huì)發(fā)生變化的，一直是0度。發(fā)生變化的是x軸與桌面的夾角以及z軸與桌面的夾角，而且桌面與x軸z軸夾角變化度數(shù)是一樣的。所以我們只需要計(jì)算出x軸和z軸中任意一個(gè)軸的夾角就可以反映出小車的傾斜的情況了。 為了方便分析，由于y軸與桌面夾角始終不變，我們從y軸的方向俯看下去，那么這個(gè)問(wèn)題就會(huì)簡(jiǎn)化成只有x軸和z軸的二維關(guān)系。假設(shè)某一時(shí)刻小車上加速度計(jì)（mpu6050）處于如下?tīng)顟B(tài)，下圖是我們看到簡(jiǎn)化后的模型。 在這個(gè)圖中，y軸已經(jīng)簡(jiǎn)化和坐標(biāo)系的原點(diǎn)o重合在了一起。我們來(lái)看看如何計(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è)長(zhǎng)方形，根據(jù)平行四邊形的原理可以得出： 角c=角d 所以計(jì)算出角度d就等效于計(jì)算出了x軸與桌面的夾角a。前邊已經(jīng)說(shuō)過(guò)gx是g在x軸的分量，那么根據(jù)正弦定理就可以得出： Sind=gx/g 得到這個(gè)公式可是還是得不到想要的角度，因?yàn)樾枰?jì)算反正弦，而反正弦在單片機(jī)里不是很好計(jì)算。 為了得到角度，于是又查了相關(guān)資料，

7、原來(lái)在角度較小的情況下，角度的正弦與角度對(duì)應(yīng)的弧度成線性關(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 對(duì)于自平衡車來(lái)說(shuō)，小車的擺動(dòng)范圍在-2929度之內(nèi)，如果超過(guò)這個(gè)范圍，小車姿態(tài)也無(wú)法調(diào)整，所以對(duì)于自平衡小車sinx=x*3.14/180基本上是成立的。當(dāng)然有時(shí)候也會(huì)擔(dān)心-2929度的搖擺范圍還是無(wú)法滿足需求。那可以給上邊的公式乘一個(gè)系數(shù)。得到如下公式： Sin

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

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

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)/*加速度計(jì)的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); /*通過(guò)卡爾曼濾波得到的最終角度*/floatAngle=0.0; /*由角速度計(jì)算的傾斜角度*/floatAngle_gy=0.0; floatQ_angle=0.9;floatQ_gyro=0.001;floatR_angle=0.5; floatdt=0.01;/*dt為kalman濾波器采

14、樣時(shí)間;*/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ù),具體實(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ì)算公式： * 前邊已經(jīng)推導(dǎo)過(guò)這里再列出來(lái)一次 * 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

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庫(kù)中的mpu6050.cpp,修改initialize函數(shù)中標(biāo)成紅色的行。 void MPU6050:initialize() setClockSource(MPU6050_CLOCK_PLL_XGYRO); setFullScaleGyroRange(MPU6050_GYRO_FS_250)