摘要

MPU6050是一種非常流行的空間運動感測器晶片，可以獲取器件當前的三個加速度分量和三個旋轉角速度。由於其體積小巧，功能強大，精度較高，不僅被廣泛應用於工業，同時也是航模愛好者的神器，被安裝在各類飛行器上馳騁藍天。

隨著Arduino開發板的普及，許多朋友希望能夠自己製作基於MPU6050的控制系統，但由於缺乏專業知識而難以上手。此外，MPU6050的資料是有較大噪音的，若不進行濾波會對整個控制系統的精準確帶來嚴重影響。

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

一、Arduino與MPU-6050的通訊

為避免糾纏於電路細節，我們直接使用整合的MPU6050模組。MPU6050的資料介面用的是I2C匯流排協議，因此我們需要Wire程式庫的幫助來實現Arduino與MPU6050之間的通訊。請先確認你的Arduino程式設計環境中已安裝Wire庫。
Wire庫的官方文件（http://www.arduino.cc/en/Reference/Wire）中指出：在UNO板子上，SDA介面對應的是A4引腳，SCL對應的是A5引腳。MPU6050需要5V的電源，可由UNO板直接供電。按照下圖連線。

（紫色線是中斷線，這裡用不到，可以不接）

MPU6050的資料寫入和讀出均通過其晶片內部的暫存器實現，這些暫存器的地址都是1個位元組，也就是8位的定址空間，其暫存器的詳細列表說明書請點選下載：

1.1 將資料寫入MPU-6050

在每次向器件寫入資料前要先開啟Wire的傳輸模式，並指定器件的匯流排地址，MPU6050的匯流排地址是0x68（AD0引腳為高電平時地址為0x69）。然後寫入一個位元組的暫存器起始地址，再寫入任意長度的資料。這些資料將被連續地寫入到指定的起始地址中，超過當前暫存器長度的將寫入到後面地址的暫存器中。寫入完成後關閉Wire的傳輸模式。下面的示例程式碼是向MPU6050的0x6B暫存器寫入一個位元組0。
1. Wire.beginTransmission(0x68); //開啟MPU6050的傳輸
2. Wire.write(0x6B); //指定暫存器地址
3. Wire.write(0); //寫入一個位元組的資料
4. Wire.endTransmission(true); //結束傳輸，true表示釋放匯流排
複製程式碼

1.2 從MPU-6050讀出資料

讀出和寫入一樣，要先開啟Wire的傳輸模式，然後寫一個位元組的暫存器起始地址。接下來將指定地址的資料讀到Wire庫的快取中，並關閉傳輸模式。最後從快取中讀取資料。下面的示例程式碼是從MPU6050的0x3B暫存器開始讀取2個位元組的資料：
1. Wire.beginTransmission(0x68); //開啟MPU6050的傳輸
2. Wire.write(0x3B); //指定暫存器地址
3. Wire.requestFrom(0x68, 2, true); //將輸據讀出到快取
4. Wire.endTransmission(true); //關閉傳輸模式
5. int val = Wire.read() << 8 | Wire.read(); //兩個位元組組成一個16位整數
複製程式碼

1.3 具體實現

通常應當在setup函式中對Wire庫進行初始化：
1. Wire.begin();
複製程式碼

在對MPU6050進行各項操作前，必須啟動該器件，向它的0x6B寫入一個位元組0即可啟動。通常也是在setup函式完成，程式碼見1.1節。

二、 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，設定加速度倍率的程式碼如下：

1. Wire.beginTransmission(0x68); //開啟MPU-6050的傳輸
2. Wire.write(0x1C); //加速度倍率暫存器的地址
3. Wire.requestFrom(0x68, 1, true); //先讀出原配置
4. unsigned char acc_conf = Wire.read();
5. acc_conf = ((acc_conf & 0xE7) | (f << 3));
6. Wire.write(acc_conf);
7. Wire.endTransmission(true); //結束傳輸，true表示釋放匯流排
   複製程式碼

再以ACC_X為例，若當前設定的加速度倍率為4g，那麼將ACC_X讀數換算為加速度的公式為： ，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讀數換算為角速度（順時針）的公式為：

三、運動資料分析

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

3.1 加速度計模型

我們可以把加速度計想象為一個正立方體盒子裡放著一個球，這個球被彈簧固定在立方體的中心。當盒子運動時，根據假想球的位置即可算出當前加速度的值。想象如果在太空中，盒子沒有任何受力時，假想球將處於正中心的位置，三個軸的加速度均為0。見下圖：

如果我們給盒子施加一個水平向左的力，那麼顯然盒子就會有一個向左的加速度，此時盒內的假想球會因為慣性作用貼向盒內的右側面。如下圖所示：

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

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

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

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

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

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

為了方便表示，上圖座標系的Z軸正方向（機腹以及晶片正面）向下，X軸正方向（飛機前進方向）向右。此時晶片的Roll角φ（黃色）為加速度向量與其在XZ平面上投影(x,0,z)的夾角，Pitch角ω（綠色）與其在YZ平面上投影(0,y,z)的夾角。求兩個向量的夾角可用點乘公式： ，簡單推導可得：
以及
注意，因為arccos函式只能返回正值角度，因此還需要根據不同情況來取角度的正負值。當y值為正時，Roll角要取負值，當x軸為負時，Pitch角要取負值。

3.4 Yaw角的問題

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

如果必須要獲得絕對的Yaw角，那麼應當選用MPU9250這款九軸運動跟蹤晶片，它可以提供額外的三軸羅盤資料，這樣我們就可以根據地球磁場方向來計算Yaw角了，具體方法此處不再贅述。

四、資料處理與實現

MPU6050晶片提供的資料夾雜有較嚴重的噪音，在晶片處理靜止狀態時資料擺動都可能超過2%。除了噪音，各項資料還會有偏移的現象，也就是說資料並不是圍繞靜止工作點擺動，因此要先對資料偏移進行校準 ，再通過濾波演算法消除噪音。

4.1 校準

校準是比較簡單的工作，我們只需要找出擺動的資料圍繞的中心點即可。我們以GRY_X為例，在晶片處理靜止狀態時，這個讀數理論上講應當為0。但它往往會存在偏移量，比如我們以10ms的間隔讀取了10個值如下：
1. -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節的飛機為例，分以下幾個步驟：

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

4.2 卡爾曼濾波

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

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

（想看的人多了再寫）

4.3 實現程式碼

以下程式碼在Arduino軟體1.65版本中編譯、燒寫以及測試通過。
1. // 本程式碼版權歸Devymex所有，以GNU GENERAL PUBLIC LICENSE V3.0釋出
2. // http://www.gnu.org/licenses/gpl-3.0.en.html
3. // 相關文件參見作者於知乎專欄發表的原創文章：
4. // http://zhuanlan.zhihu.com/devymex/20082486
5.
6. //連線方法
7. //MPU-UNO
8. //VCC-VCC
9. //GND-GND
10. //SCL-A5
11. //SDA-A4
12. //INT-2 (Optional)
13.
14. #include