摘要
MPU6050是一種非常流行的空間運動傳感器晶片，可以擷取器件目前的三個加速度分量和三個旋轉角速度。由于其體積小巧，功能強大，精度較高，不僅被廣泛應用于工業，同時也是航模愛好者的神器，被安裝在各類飛行器上馳騁藍天。
随着Arduino開發闆的普及，許多朋友希望能夠自己制作基于MPU6050的控制系統，但由于缺乏專業知識而難以上手。此外，MPU6050的資料是有較大噪音的，若不進行濾波會對整個控制系統的精準确帶來嚴重影響。
MPU6050晶片内自帶了一個資料處理子子產品DMP，已經内置了濾波算法，在許多應用中使用DMP輸出的資料已經能夠很好的滿足要求。關于如何擷取DMP的輸出資料，我将在以後的文章中介紹。本文将直接面對原始測量資料，從連線、晶片通信開始一步一步教你如何利用Arduino擷取MPU6050的資料并進行卡爾曼濾波，最終獲得穩定的系統運動狀态。
一、Arduino與MPU-6050的通信
為避免糾纏于電路細節，我們直接使用內建的MPU6050子產品。MPU6050的資料接口用的是I2C總線協定，是以我們需要Wire程式庫的幫助來實作Arduino與MPU6050之間的通信。請先确認你的Arduino程式設計環境中已安裝Wire庫。
（紫色線是中斷線，這裡用不到，可以不接）
1.1 将資料寫入MPU-6050
在每次向器件寫入資料前要先打開Wire的傳輸模式，并指定器件的總線位址，MPU6050的總線位址是0x68（AD0引腳為高電平時位址為0x69）。然後寫入一個位元組的寄存器起始位址，再寫入任意長度的資料。這些資料将被連續地寫入到指定的起始位址中，超過目前寄存器長度的将寫入到後面位址的寄存器中。寫入完成後關閉Wire的傳輸模式。下面的示例代碼是向MPU6050的0x6B寄存器寫入一個位元組0。
1.2 從MPU-6050讀出資料
讀出和寫入一樣，要先打開Wire的傳輸模式，然後寫一個位元組的寄存器起始位址。接下來将指定位址的資料讀到Wire庫的緩存中，并關閉傳輸模式。最後從緩存中讀取資料。下面的示例代碼是從MPU6050的0x3B寄存器開始讀取2個位元組的資料：
1.3 具體實作
通常應當在setup函數中對Wire庫進行初始化：
在對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，設定加速度倍率的代碼如下：
再以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可以擷取三個軸向上的加速度，而地球重力則是長期存在且永遠豎直向下，是以我們可以根據重力加速度相對于晶片的指向為參考算得目前姿态。
為友善起見，我們讓晶片正面朝下固定在上圖飛機上，且座标系與飛機的坐标系完全重合，以三個軸向上的加速度為分量，可構成加速度向量
。假設目前晶片處于勻速直線運動狀态，那麼
應垂直于地面上向，即指向Z軸負方向，模長為
（與重力加速度大小相等，方向相反，見3.1節）。若晶片（座标系）發生旋轉，由于加速度向量
仍然豎直向上，是以Z軸負方向将不再與
重合。見下圖。
為了友善表示，上圖坐标系的Z軸正方向（機腹以及晶片正面）向下，X軸正方向（飛機前進方向）向右。此時晶片的Roll角
（黃色）為加速度向量與其在XZ平面上投影
的夾角，Pitch角
（綠色）與其在YZ平面上投影
的夾角。求兩個向量的夾角可用點乘公式：
，簡單推導可得：
，以及
注意，因為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個值如下：
這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版本中編譯、燒寫以及測試通過。
<b>本文轉自莫水千流部落格園部落格，原文連結：</b><b>http://www.cnblogs.com/zhoug2020/p/7599851.html</b><b>，如需轉載請自行聯系原作者</b>