1、介紹：MPU6050 是 InvenSense 公司推出的全球首款整合性 6 軸運動處理元件，相較於多元件方案，免除了組合陀螺儀與加速器時之軸間差的問題，減少了安裝空間。

（1）繞X軸旋轉角度為roll，繞Y軸旋轉角度為pitch，繞Z軸旋轉角度為yaw。

（2）加速度感測器，本質是力感測器。用來檢查上下左右前後哪幾個面都受了多少力（包括重力），然後計算角度。

（3）陀螺儀，本質是角速度檢測儀。比如，一塊板，以X軸為軸心，在一秒鐘的時間轉到了90度，那麼它在X軸上的角速度就是 90度/秒  （DPS, 角速度單位，Degree Per Second的縮寫°/S ，體現了轉動的快慢）

（4）MPU解析度：3軸加速度 和3軸陀螺儀分別用了3個16位的ADC, 也就是說，加速度有3個16位ADC，其中每個軸使用了一個。也是說，每個軸輸出的資料，是2^16 也就是 -32768 ---- +32768。陀螺儀也是一樣。

2、擴充套件性：MPU6050 內部整合了 3 軸陀螺儀和 3 軸加速度感測器，並且含有一個第二 IIC 介面，可用於連線外部磁力感測器即AUX_CL 和 AUX_DA，並利用自帶的數字運動處理器（DMP: DigitalMotion Processor）硬體加速引擎，通過主 IIC 介面，嚮應用端輸出完整的 9 軸融合演算資料。

3、姿態解算：我們可以使用 InvenSense 公司提供的DMP非常方便的實現姿態解算，降低了運動處理運算對作業系統的負荷。

4、MPU6050原理圖

其中，SCL 和 SDA 是連線 MCU 的 IIC 介面，MCU 通過這個 IIC 介面來控制 MPU6050，另外還有一個 IIC 介面：AUX_CL 和 AUX_DA，這個介面可用來連線外部從裝置，比如磁感測器，這樣就可以組成一個九軸感測器。

5、初始化步驟

（1）初始化 IIC  介面

（2）復位 MPU6050

MPU6050 內部所有暫存器恢復預設值

（3）設定角速度感測器（陀螺儀）和加速度感測器的滿量程範圍

上面說的-32768 --- +32768 ，那麼這個數字到底代表了什麼呢？比如陀螺儀 32768 到底是指角速度達到多少度/秒 ？這個其實是根據MPU6050設定的量程來決定的，量程不一樣，32768代表的值就不一樣。按陀螺儀來說，MPU6050 有四個量程可選： ±250，±500，±1000，±2000 度/s 比方說，設定了是 ±250 , 那麼-32768  ---- +32768 就代表了 -250 ---- +250 。此時它的LSB(最低有效位) 是 131 LSB/(度/s)

（4）設定其它引數

     關閉中斷、關閉 AUX IIC 介面、禁止 FIFO、設定陀螺儀取樣率和設定數字低通濾波器（DLPF）等。

（5）配置系統時鐘源並使能角速度感測器和加速度感測器

6、DMP（Digital Motion Processing ）

我們可以讀出 MPU6050 的加速度感測器和角速度感測器的原始資料。DMP可以將原始角速度資料轉換為4元組資料，這正是DMP的意義所在。進而完成尤拉角的計算。

MPU6050 的 DMP 輸出的四元數是 q30 格式的，也就是浮點數放大了 2 的 30 次方倍。在換算成尤拉角之前，必須先將其轉換為浮點數，也就是除以 2 的 30 次方，然後再進行計算，計算公式為：

q0=quat[0] / q30; //q30 格式轉換為浮點數

q1=quat[1] / q30;

q2=quat[2] / q30;

q3=quat[3] / q30;

//計算得到尤拉角：俯仰角/橫滾角/航向角

pitch=asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; //俯仰角

roll=atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; //橫滾角

yaw=atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //航向角

其中 quat[0]~ quat[3]是四元組，57.3 是弧度轉換為角度，即 180/π，這樣得到的結果就是以度（°）為單位的。

7、MPU6050的四元數解算姿態方法

我們先來看看如何用尤拉角描述一次平面旋轉(座標變換)：

設座標系繞旋轉α角後得到座標系，在空間中有一個向量在座標系中的投影為，在內的投影為由於旋轉繞進行，所以Z座標未變，即有。

轉換成矩陣形式表示為：

也就是

所以從旋轉到可以寫成上面僅僅是繞一根軸的旋轉，如果三維空間中的尤拉角旋轉要轉三次:

上面得到了一個表示旋轉的方向餘弦矩陣。

不過要想用尤拉角解算姿態，其實我們套用尤拉角微分方程就行了：

上式中左側，是本次更新後的尤拉角，對應row、pit、yaw。右側，是上個週期測算出來的角度，三個角速度等於三軸陀螺儀在這個週期轉動的角度，單位為弧度，計算間隔時T陀螺角速度，比如0.02秒0.01弧度/秒=0.0002弧度。間因此求解這個微分方程就能解算出當前的尤拉角。

我們為什麼不用尤拉角來表示旋轉而要引入四元數呢？

一方面是因為尤拉角微分方程中包含了大量的三角運算，這給實時解算帶來了一定的困難。而且當俯仰角為90度時方程式會出現神奇的“GimbalLock”。所以尤拉角方法只適用於水平姿態變化不大的情況，而不適用於全姿態飛行器的姿態確定。

四元數法只求解四個未知量的線性微分方程組，計算量小，易於操作，是比較實用的工程方法。

我們知道在平面(x,y)中的旋轉可以用複數來表示，同樣的三維中的旋轉可以用單位四元數來描述。我們來定義一個四元數：

一個四元數可以表示一個完整的旋轉。只有單位四元數才可以表示旋轉，至於為什麼，因為這就是四元數表示旋轉的約束條件。

而剛才用尤拉角描述的方向餘弦矩陣用四元數描述則為：

要首先把加速度計採集到的值(三維向量)轉化為單位向量，即向量除以模，傳入引數是陀螺儀x、y、z值和加速度計x、y、z值：

void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) 

{ float norm; 

float vx, vy, vz; float ex, ey, ez;           

norm = sqrt(ax*ax + ay*ay + az*az);       

ax = ax / norm; 

ay = ay / norm; 

az = az / norm;  

下面把四元數換算成方向餘弦中的第三行的三個元素。剛好vx、vy、vz 。其實就是上一次的尤拉角（四元數）的機體座標參考系換算出來的重力的單位向量。

estimated direction of gravity vx = 2*(q1*q3 - q0*q2);

vy = 2*(q0*q1 + q2*q3); 

vz = q0*q0 - q1*q1 - q2*q2 + q3*q3; 

axyz是機體座標參照系上，加速度計測出來的重力向量，也就是實際測出來的重力向量。

axyz是測量得到的重力向量，vxyz是陀螺積分後的姿態來推算出的重力向量，它們都是機體座標參照系上的重力向量。

那它們之間的誤差向量，就是陀螺積分後的姿態和加計測出來的姿態之間的誤差。

向量間的誤差，可以用向量叉積（也叫向量外積、叉乘）來表示，exyz就是兩個重力向量的叉積。

這個叉積向量仍舊是位於機體座標系上的，而陀螺積分誤差也是在機體座標系，而且叉積的大小與陀螺積分誤差成正比，正好拿來糾正陀螺。（你可以自己拿東西想象一下）由於陀螺是對機體直接積分，所以對陀螺的糾正量會直接體現在對機體座標系的糾正。

integral error scaled integral gain exInt = exInt + ex*Ki;

eyInt = eyInt + ey*Ki; 

ezInt = ezInt + ez*Ki; 

用叉積誤差來做PI修正陀螺零偏

integral error scaled integral gain exInt = exInt + ex*Ki; 

eyInt = eyInt + ey*Ki; 

ezInt = ezInt + ez*Ki;  // adjusted gyroscope measurements 

gx = gx + Kp*ex + exInt; 

gy = gy + Kp*ey + eyInt; 

gz = gz + Kp*ez + ezInt; 

四元數微分方程，其中T為測量週期，為陀螺儀角速度，以下都是已知量，這裡使用了一階龍哥庫塔求解四元數微分方程：

integrate quaternion rate and normalise 

q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT; 

q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT; 

q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT; 

q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;   

最後根據四元數方向餘弦陣和尤拉角的轉換關係，把四元數轉換成尤拉角：

所以有：

ANGLE.Yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* 57.3; // yaw 

ANGLE.Y= asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch 

ANGLE.X= atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll

8、MEMS感測器

（1）加速度感測器

通過微機械加工技術在矽片上加工形成了一個機械懸臂。它與相鄰的電極形成了兩個電容。由於加速度使得機械懸臂與兩個電極之間的距離發生變化，從而改變了兩個電容的引數。通過整合的開關電容放大電路量測電容引數的變化，形成了與加速度成正比的電壓輸出。

參考部落格：http://blog.sina.com.cn/s/blog_c5a00db10102wd7d.html