Arduino教程：MPU6050的數據獲取、分析與處理

時間 2020-06-19

標籤 arduino 教程 mpu6050 mpu 數據獲取分析處理简体版

原文原文鏈接

Arduino教程：MPU6050的數據獲取、分析與處理

轉載

摘要html

MPU6050是一種很是流行的空間運動傳感器芯片，能夠獲取器件當前的三個加速度份量和三個旋轉角速度。因爲其體積小巧，功能強大，精度較高，不只被普遍應用於工業，同時也是航模愛好者的神器，被安裝在各種飛行器上馳騁藍天。算法

隨着Arduino開發板的普及，許多朋友但願可以本身製做基於MPU6050的控制系統，但因爲缺少專業知識而難以上手。此外，MPU6050的數據是有較大噪音的，若不進行濾波會對整個控制系統的精準確帶來嚴重影響。編程

MPU6050芯片內自帶了一個數據處理子模塊DMP，已經內置了濾波算法，在許多應用中使用DMP輸出的數據已經可以很好的知足要求。關於如何獲取DMP的輸出數據，我將在之後的文章中介紹。本文將直接面對原始測量數據，從連線、芯片通訊開始一步一步教你如何利用Arduino獲取MPU6050的數據並進行卡爾曼濾波，最終得到穩定的系統運動狀態。數組

1、Arduino與MPU-6050的通訊緩存

爲避免糾纏於電路細節，咱們直接使用集成的MPU6050模塊。MPU6050的數據接口用的是I2C總線協議，所以咱們須要Wire程序庫的幫助來實現Arduino與MPU6050之間的通訊。請先確認你的Arduino編程環境中已安裝Wire庫。ide

Wire庫的官方文檔（http://www.arduino.cc/en/Reference/Wire）中指出：在UNO板子上，SDA接口對應的是A4引腳，SCL對應的是A5引腳。MPU6050須要5V的電源，可由UNO板直接供電。按照下圖連線。
函數

（紫色線是中斷線，這裏用不到，能夠不接） oop

MPU6050的數據寫入和讀出均經過其芯片內部的寄存器實現，這些寄存器的地址都是1個字節，也就是8位的尋址空間，其寄存器的詳細列表說明書請點擊下載：https://www.olimex.com/Products/Modules/Sensors/MOD-MPU6050/resources/RM-MPU-60xxA_rev_4.pdf測試

1.1 將數據寫入MPU-6050ui

在每次向器件寫入數據前要先打開Wire的傳輸模式，並指定器件的總線地址，MPU6050的總線地址是0x68（AD0引腳爲高電平時地址爲0x69）。而後寫入一個字節的寄存器起始地址，再寫入任意長度的數據。這些數據將被連續地寫入到指定的起始地址中，超過當前寄存器長度的將寫入到後面地址的寄存器中。寫入完成後關閉Wire的傳輸模式。下面的示例代碼是向MPU6050的0x6B寄存器寫入一個字節0。

Wire.beginTransmission(0x68); //開啓MPU6050的傳輸
Wire.write(0x6B); //指定寄存器地址
Wire.write(0); //寫入一個字節的數據
Wire.endTransmission(true); //結束傳輸，true表示釋放總線

1.2 從MPU-6050讀出數據

讀出和寫入同樣，要先打開Wire的傳輸模式，而後寫一個字節的寄存器起始地址。接下來將指定地址的數據讀到Wire庫的緩存中，並關閉傳輸模式。最後從緩存中讀取數據。下面的示例代碼是從MPU6050的0x3B寄存器開始讀取2個字節的數據：

Wire.beginTransmission(0x68); //開啓MPU6050的傳輸
Wire.write(0x3B); //指定寄存器地址
Wire.requestFrom(0x68, 2, true); //將輸據讀出到緩存
Wire.endTransmission(true); //關閉傳輸模式
int val = Wire.read() << 8 | Wire.read(); //兩個字節組成一個16位整數

1.3 具體實現

一般應當在setup函數中對Wire庫進行初始化：

Wire.begin();

在對MPU6050進行各項操做前，必須啓動該器件，向它的0x6B寫入一個字節0便可啓動。一般也是在setup函數完成，代碼見1.1節。

2、 MPU6050的數據格式

咱們感興趣的數據位於0x3B到0x48這14個字節的寄存器中。這些數據會被動態更新，更新頻率最高可達1000HZ。下面列出相關寄存器的地址，數據的名稱。注意，每一個數據都是2個字節。

0x3B，加速度計的X軸份量ACC_X
0x3D，加速度計的Y軸份量ACC_Y
0x3F，加速度計的Z軸份量ACC_Z
0x41，當前溫度TEMP
0x43，繞X軸旋轉的角速度GYR_X
0x45，繞Y軸旋轉的角速度GYR_Y
0x47，繞Z軸旋轉的角速度GYR_Z

MPU6050芯片的座標系是這樣定義的：令芯片表面朝向本身，將其表面文字轉至正確角度，此時，以芯片內部中心爲原點，水平向右的爲X軸，豎直向上的爲Y軸，指向本身的爲Z軸。見下圖：

咱們只關心加速度計和角速度計數據的含義，下面分別介紹。

2.1 加速度計

加速度計的三軸份量ACC_X、ACC_Y和ACC_Z均爲16位有符號整數，分別表示器件在三個軸向上的加速度，取負值時加速度沿座標軸負向，取正值時沿正向。

三個加速度份量均以重力加速度g的倍數爲單位，可以表示的加速度範圍，即倍率能夠統一設定，有4個可選倍率：2g、4g、8g、16g。以ACC_X爲例，若倍率設定爲2g（默認），則意味着ACC_X取最小值-32768時，當前加速度爲沿X軸正方向2倍的重力加速度；若設定爲4g，取-32768時表示沿X軸正方向4倍的重力加速度，以此類推。顯然，倍率越低精度越好，倍率越高表示的範圍越大，這要根據具體的應用來設定。

咱們用f表示倍率，f=0爲2g，f=3爲16g，設定加速度倍率的代碼以下：

Wire.beginTransmission(0x68); //開啓MPU-6050的傳輸
Wire.write(0x1C); //加速度倍率寄存器的地址
Wire.requestFrom(0x68, 1, true); //先讀出原配置
unsigned char acc_conf = Wire.read();
acc_conf = ((acc_conf & 0xE7) | (f << 3));
Wire.write(acc_conf);
Wire.endTransmission(true); //結束傳輸，true表示釋放總線

再以ACC_X爲例，若當前設定的加速度倍率爲4g，那麼將ACC_X讀數換算爲加速度的公式爲： $a_x=4g\times \text{ACC\_X}/32768$ ，g可取當地重力加速度。

2.2 角速度計

繞X、Y和Z三個座標軸旋轉的角速度份量GYR_X、GYR_Y和GYR_Z均爲16位有符號整數。從原點向旋轉軸方向看去，取正值時爲順時針旋轉，取負值時爲逆時針旋轉。

三個角速度份量均以「度/秒」爲單位，可以表示的角速度範圍，即倍率可統一設定，有4個可選倍率：250度/秒、500度/秒、1000度/秒、2000度/秒。以GYR_X爲例，若倍率設定爲250度/秒，則意味着GYR取正最大值32768時，當前角速度爲順時針250度/秒；若設定爲500度/秒，取32768時表示當前角速度爲順時針500度/秒。顯然，倍率越低精度越好，倍率越高表示的範圍越大。

咱們用f表示倍率，f=0爲250度/秒，f=3爲2000度/秒，除角速度倍率寄存器的地址爲0x1B以外，設定加速度倍率的代碼與2.1節代碼一致。

以GYR_X爲例，若當前設定的角速度倍率爲1000度/秒，那麼將GRY_X讀數換算爲角速度（順時針）的公式爲： $g_x=1000\times \text{GYR\_X} / 32768$ 。

3、運動數據

在讀取加速度計和角速度計的數據並換算爲物理值後，根據不一樣的應用，數據有不一樣的解譯方式。本章將以飛行器運動模型爲例，根據加速度和角速度來算出當前的飛行姿態。

3.1 加速度計模型

咱們能夠把加速度計想象爲一個正立方體盒子裏放着一個球，這個球被彈簧固定在立方體的中心。當盒子運動時，根據假想球的位置便可算出當前加速度的值。想象若是在太空中，盒子沒有任何受力時，假想球將處於正中心的位置，三個軸的加速度均爲0。見下圖：

若是咱們給盒子施加一個水平向左的力，那麼顯然盒子就會有一個向左的加速度，此時盒內的假想球會由於慣性做用貼向盒內的右側面。以下圖所示：

爲了保證數據的物理意義，MPU6050的加速度計是以假想球在三軸上座標值的相反數做爲三個軸的加速度值。當假想球的位置偏向一個軸的正向時，該軸的加速度讀數爲負值，當假想球的位置偏向一個軸的負向時，該軸的加速度讀數爲正值。

根據以上分析，當咱們把MPU6050芯片水平放於地方，芯片表面朝向天空，此時因爲受到地球重力的做用，假想球的位置偏向Z軸的負向，所以Z軸的加速度讀數應爲正，且在理想狀況下應爲g。注意，此加速度的物理意義並非重力加速度，而是自身運動的加速度，能夠這樣理解：正由於其自身運動的加速度與重力加速度大小相等方向相反，芯片才能保持靜止。

3.2 Roll-pitch-yaw模型與姿態計算

表示飛行器當前飛行姿態的一個通用模型就是創建下圖所示座標系，並用Roll表示繞X軸的旋轉，Pitch表示繞Y軸的旋轉，Yaw表示繞Z軸的旋轉。

因爲MPU6050能夠獲取三個軸向上的加速度，而地球重力則是長期存在且永遠豎直向下，所以咱們能夠根據重力加速度相對於芯片的指向爲參考算得當前姿態。

爲方便起見，咱們讓芯片正面朝下固定在上圖飛機上，且座標系與飛機的座標系徹底重合，以三個軸向上的加速度爲份量，可構成加速度向量。假設當前芯片處於勻速直線運動狀態，那麼應垂直於地面上向，即指向Z軸負方向，模長爲 $|a|=g=\sqrt{x^2+y^2+z^2}$ （與重力加速度大小相等，方向相反，見3.1節）。若芯片（座標系）發生旋轉，因爲加速度向量仍然豎直向上，因此Z軸負方向將再也不與重合。見下圖。

爲了方便表示，上圖座標系的Z軸正方向（機腹以及芯片正面）向下，X軸正方向（飛機前進方向）向右。此時芯片的Roll角 $\phi$ （黃色）爲加速度向量與其在XZ平面上投影的夾角，Pitch角 $\omega$ （綠色）與其在YZ平面上投影的夾角。求兩個向量的夾角可用點乘公式： $a \cdot b=|a|\cdot|b|\cdot\cos\theta$ ，簡單推導可得：

$\phi=\cos^{-1}(\sqrt{x^2+z^2}/g)$ ，以及 $\omega=\cos^{-1}(\sqrt{y^2+z^2}/g)$

注意，由於arccos函數只能返回正值角度，所以還須要根據不一樣狀況來取角度的正負值。當y值爲正時，Roll角要取負值，當x軸爲負時，Pitch角要取負值。

3.4 Yaw角的問題

由於沒有參考量，因此沒法求出當前的Yaw角的絕對角度，只能獲得Yaw的變化量，也就是角速度GYR_Z。固然，咱們能夠經過對GYR_Z積分的方法來推算當前Yaw角（以初始值爲準），但因爲測量精度的問題，推算值會發生漂移，一段時間後就徹底失去意義了。然而在大多數應用中，好比無人機，只須要得到GRY_Z就能夠了。

若是必需要得到絕對的Yaw角，那麼應當選用MPU9250這款九軸運動跟蹤芯片，它能夠提供額外的三軸羅盤數據，這樣咱們就能夠根據地球磁場方向來計算Yaw角了，具體方法此處再也不贅述。

4、數據處理與實現

MPU6050芯片提供的數據夾雜有較嚴重的噪音，在芯片處理靜止狀態時數據擺動均可能超過2%。除了噪音，各項數據還會有偏移的現象，也就是說數據並非圍繞靜止工做點擺動，所以要先對數據偏移進行校準，再經過濾波算法消除噪音。

4.1 校準

校準是比較簡單的工做，咱們只須要找出擺動的數據圍繞的中心點便可。咱們以GRY_X爲例，在芯片處理靜止狀態時，這個讀數理論上講應當爲0，但它每每會存在偏移量，好比咱們以10ms的間隔讀取了10個值以下：

-158.4, -172.9, -134.2, -155.1, -131.2, -146.8, -173.1, -188.6, -142.7, -179.5

這10個值的均值，也就是這個讀數的偏移量爲-158.25。在獲取偏移量後，每次的讀數都減去偏移量就能夠獲得校準後的讀數了。固然這個偏移量只是估計值，比較準確的偏移量要對大量的數據進行統計才能獲知，數據量越大越準，但統計的時間也就越慢。通常校準能夠在每次啓動系統時進行，那麼你應當在準確度和啓動時間之間作一個權衡。

三個角速度讀數GYR_X、GYR_Y和GYR_Z都可經過統計求平均的方法來得到，但三個加速度份量就不能這樣簡單的完成了，由於芯片靜止時的加速度並不爲0。

加速度值的偏移來自兩個方面，一是因爲芯片的測量精度，導至它測得的加速度向量並不垂直於大地；二是芯片在整個系統（如無人機）上安裝的精度是有限的，系統與芯片的座標系很難達到完美重合。前者咱們稱爲讀數偏移，後者咱們稱爲角度偏移。由於讀數和角度之間是非線性關係，因此要想以高精度進行校準必須先單獨校準讀數偏移，再把芯片固定在系統中後校準角度偏移。然而，因爲校準角度偏移須要專業設備，且對於通常應用來講，兩步校準帶來的精度提高並不大，所以一般只進行讀數校準便可。下面介紹讀數校準的方法。咱們還3.2節的飛機爲例，分如下幾個步驟：

首先要肯定飛機的座標系，對於多軸飛行器來講這很是重要。若是座標系原點的位置或座標軸的方向存在較大誤差，將會給後面的飛控形成不良影響。
在肯定了飛機的座標系後，爲了儘可能避免讀數偏移帶來的影響，首先將MPU6050緊緊地固定在飛機上，並使兩者座標系儘量的重合。固然把Z軸反過來裝也是能夠的，就是須要從新推算一套角度換算公式。
將飛機置於水平、堅固的平面上，並充分預熱。對於多軸無人機而言，空中懸停時的XY平面應當平行於校準時的XY平面。此時，咱們認爲芯片的加速度方向應當與Z軸負方向重合，且加速度向量的模長爲g，所以ACC_X和ACC_Y的理論值應爲0，ACC_Z的理論值應爲-16384（假設咱們設定2g的倍率，1g的加速度的讀數應爲最大值-32768的一半）。
因爲ACC_X和ACC_Y的理論值應爲0，與角速度量的校準相似，這兩個讀數偏移量可用統計均值的方式校準。ACC_Z則須要多一步處理，即在統計偏移量的過程當中，每次讀數都要加上16384，再進行統計均值校準。

4.2 卡爾曼濾波

對於夾雜了大量噪音的數據，卡爾曼濾波器的效果無疑是最好的。若是不想考慮算法細節，能夠直接使用Arduino的Klaman Filter庫完成。在咱們的模型中，一個卡爾曼濾波器接受一個軸上的角度值、角速度值以及時間增量，估計出一個消除噪音的角度值。跟據當前的角度值和上一輪估計的角度值，以及這兩輪估計的間隔時間，咱們還能夠反推出消除噪音的角速度。

實現代碼見4.3節。下面介紹卡爾曼濾波算法細節，不感興趣的可跳過。

（想看的人多了再寫）

4.3 實現代碼

如下代碼在Arduino軟件1.65版本中編譯、燒寫以及測試經過。

// 本代碼版權歸Devymex全部，以GNU GENERAL PUBLIC LICENSE V3.0發佈 // http://www.gnu.org/licenses/gpl-3.0.en.html // 相關文檔參見做者於知乎專欄發表的原創文章： // http://zhuanlan.zhihu.com/devymex/20082486 //連線方法 //MPU-UNO //VCC-VCC //GND-GND //SCL-A5 //SDA-A4 //INT-2 (Optional) #include <Kalman.h> #include <Wire.h> #include <Math.h> float fRad2Deg = 57.295779513f; //將弧度轉爲角度的乘數 const int MPU = 0x68; //MPU-6050的I2C地址 const int nValCnt = 7; //一次讀取寄存器的數量 const int nCalibTimes = 1000; //校準時讀數的次數 int calibData[nValCnt]; //校準數據 unsigned long nLastTime = 0; //上一次讀數的時間 float fLastRoll = 0.0f; //上一次濾波獲得的Roll角 float fLastPitch = 0.0f; //上一次濾波獲得的Pitch角 Kalman kalmanRoll; //Roll角濾波器 Kalman kalmanPitch; //Pitch角濾波器 void setup() { Serial.begin(9600); //初始化串口，指定波特率 Wire.begin(); //初始化Wire庫 WriteMPUReg(0x6B, 0); //啓動MPU6050設備 Calibration(); //執行校準 nLastTime = micros(); //記錄當前時間 } void loop() { int readouts[nValCnt]; ReadAccGyr(readouts); //讀出測量值 float realVals[7]; Rectify(readouts, realVals); //根據校準的偏移量進行糾正 //計算加速度向量的模長，均以g爲單位 float fNorm = sqrt(realVals[0] * realVals[0] + realVals[1] * realVals[1] + realVals[2] * realVals[2]); float fRoll = GetRoll(realVals, fNorm); //計算Roll角 if (realVals[1] > 0) { fRoll = -fRoll; } float fPitch = GetPitch(realVals, fNorm); //計算Pitch角 if (realVals[0] < 0) { fPitch = -fPitch; } //計算兩次測量的時間間隔dt，以秒爲單位 unsigned long nCurTime = micros(); float dt = (double)(nCurTime - nLastTime) / 1000000.0; //對Roll角和Pitch角進行卡爾曼濾波 float fNewRoll = kalmanRoll.getAngle(fRoll, realVals[4], dt); float fNewPitch = kalmanPitch.getAngle(fPitch, realVals[5], dt); //跟據濾波值計算角度速 float fRollRate = (fNewRoll - fLastRoll) / dt; float fPitchRate = (fNewPitch - fLastPitch) / dt; //更新Roll角和Pitch角 fLastRoll = fNewRoll; fLastPitch = fNewPitch; //更新本次測的時間 nLastTime = nCurTime; //向串口打印輸出Roll角和Pitch角，運行時在Arduino的串口監視器中查看 Serial.print("Roll:"); Serial.print(fNewRoll); Serial.print('('); Serial.print(fRollRate); Serial.print("),\tPitch:"); Serial.print(fNewPitch); Serial.print('('); Serial.print(fPitchRate); Serial.print(")\n"); delay(10); } //向MPU6050寫入一個字節的數據 //指定寄存器地址與一個字節的值 void WriteMPUReg(int nReg, unsigned char nVal) { Wire.beginTransmission(MPU); Wire.write(nReg); Wire.write(nVal); Wire.endTransmission(true); } //從MPU6050讀出一個字節的數據 //指定寄存器地址，返回讀出的值 unsigned char ReadMPUReg(int nReg) { Wire.beginTransmission(MPU); Wire.write(nReg); Wire.requestFrom(MPU, 1, true); Wire.endTransmission(true); return Wire.read(); } //從MPU6050讀出加速度計三個份量、溫度和三個角速度計 //保存在指定的數組中 void ReadAccGyr(int *pVals) { Wire.beginTransmission(MPU); Wire.write(0x3B); Wire.requestFrom(MPU, nValCnt * 2, true); Wire.endTransmission(true); for (long i = 0; i < nValCnt; ++i) { pVals[i] = Wire.read() << 8 | Wire.read(); } } //對大量讀數進行統計，校準平均偏移量 void Calibration() { float valSums[7] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0}; //先求和 for (int i = 0; i < nCalibTimes; ++i) { int mpuVals[nValCnt]; ReadAccGyr(mpuVals); for (int j = 0; j < nValCnt; ++j) { valSums[j] += mpuVals[j]; } } //再求平均 for (int i = 0; i < nValCnt; ++i) { calibData[i] = int(valSums[i] / nCalibTimes); } calibData[2] += 16384; //設芯片Z軸豎直向下，設定靜態工做點。 } //算得Roll角。算法見文檔。 float GetRoll(float *pRealVals, float fNorm) { float fNormXZ = sqrt(pRealVals[0] * pRealVals[0] + pRealVals[2] * pRealVals[2]); float fCos = fNormXZ / fNorm; return acos(fCos) * fRad2Deg; } //算得Pitch角。算法見文檔。 float GetPitch(float *pRealVals, float fNorm) { float fNormYZ = sqrt(pRealVals[1] * pRealVals[1] + pRealVals[2] * pRealVals[2]); float fCos = fNormYZ / fNorm; return acos(fCos) * fRad2Deg; } //對讀數進行糾正，消除偏移，並轉換爲物理量。公式見文檔。 void Rectify(int *pReadout, float *pRealVals) { for (int i = 0; i < 3; ++i) { pRealVals[i] = (float)(pReadout[i] - calibData[i]) / 16384.0f; } pRealVals[3] = pReadout[3] / 340.0f + 36.53; for (int i = 4; i < 7; ++i) { pRealVals[i] = (float)(pReadout[i] - calibData[i]) / 131.0f; } }