19.1 MPU6050簡介

19.1.1 芯片概述

MPU6050是InvenSense公司推出的一款6軸運動處理芯片，內(nèi)置3軸陀螺儀及3軸速度傳感器，內(nèi)置兩組I2C接口，其中一組用于通信，另一組則用于連接外部磁力傳感器，采用自帶的數(shù)字運動處理器DMP（Digital Motion Processor），通過主I2C接口，直接讀取完整的9軸融合演算數(shù)據(jù)。MPU6050檢測軸及其檢測方向如下圖所示。

19.1.2 引腳介紹

MPU6050采用QFN-24封裝，端口描述如下表所示。

19.1.3 硬件電路

由于MPU6050內(nèi)部是可以自動計算X，Y和Z軸的方向及加速度的，使用者可以不考慮實際的數(shù)據(jù)轉(zhuǎn)換問題，但是為了詳細的了解MPU6050的計算過程，使用者最好還是應(yīng)該具備了解原始數(shù)據(jù)如何轉(zhuǎn)換為我們需要的角度與加速度值。

19.2 姿態(tài)解算與融合算法基礎(chǔ)概念

19.2.1 方向矩陣

設(shè)有一個三位直角坐標系Oxyz，如下圖所示。

19.2.2 方向余弦矩陣

19.2.3 歐拉角

歐拉角是用于確定定點轉(zhuǎn)動缸體位置的3個1組的獨立角參量，由章動角θ，旋轉(zhuǎn)角（進動角）ψ和自轉(zhuǎn)角φ組成，歐拉角有多種取法，下面是比較常見的一種。

如上圖所示，由定點O做出固定坐標系Oxyz以及固定連在剛體的坐標系Ox’y’z’，以軸Oz和Oz’為基本軸。其垂直面Oxy和Ox’y’為基本平面，由軸Oz量到Oz’的角度θ稱為章動角，平面zOz’的垂線ON稱為節(jié)線，同時ON又是基本平面Ox’y’和Oxy的交線，在右手坐標系中，由ON的正端看，角θ應(yīng)按照逆時針方向計算，由固定軸Ox到節(jié)線ON的角度ψ稱為進動角，也叫作旋轉(zhuǎn)角，由節(jié)線ON到動軸Ox’的角度φ稱為自轉(zhuǎn)角，有Oz和Oz’正端看，進動角ψ與自轉(zhuǎn)角φ也應(yīng)該按照逆時針方向計算。

從上面的描述過程可以發(fā)現(xiàn)，歐拉角實際是可以分解成三步來計算的：

第1步：繞z軸旋轉(zhuǎn)α，使得x軸與N軸重合

第2步：繞x軸旋轉(zhuǎn)β，使z軸與旋轉(zhuǎn)后的z軸重合

第3步：繞z軸旋轉(zhuǎn)y，是坐標系與旋轉(zhuǎn)后的完全重合

根據(jù)上面的三個步驟，我們來通過以下實例來說明歐拉角與方向余弦矩陣的轉(zhuǎn)換過程。

19.2.4 四元數(shù)與歐拉角的轉(zhuǎn)換

四元數(shù)是一個簡單的超復(fù)數(shù)，是由實數(shù)加上三個虛數(shù)單位i，j，k組成，每個四元數(shù)都是1，i，j，k的線性組合，四元數(shù)是愛爾蘭數(shù)學(xué)家哈密頓在1843年發(fā)明的數(shù)學(xué)概念，四元數(shù)的乘法不符合交換律。

四元數(shù)姿態(tài)表達式是一個四參數(shù)的表達式，它的基本思路是一個坐標系轉(zhuǎn)換到另一個坐標系可以通過繞一個定義在參考系中的矢量μ的單次轉(zhuǎn)動來實現(xiàn)，四元數(shù)用符號q表示，是一個具有4個元素的矢量，這些元素是該矢量方向和轉(zhuǎn)動大小的函數(shù)。定義四元數(shù)如下所示。

這里直接給出結(jié)論，不作證明。會用即可。四元數(shù)與歐拉角的轉(zhuǎn)換公式為：

用方向余弦表示歐拉角，這里歐拉角不允許等于90度。

用四元數(shù)表示歐拉角

在姿態(tài)解算中常用的算法由歐拉角法，方向余弦法和四元數(shù)法，歐拉角在求解姿態(tài)時存在奇點，無法用于全姿態(tài)結(jié)算，方向余弦沒有奇點，但是計算量大，無法滿足實時性要求，四元數(shù)法，計算量小，無奇點可以滿足飛行器運動過程中姿態(tài)的實時解算，姿態(tài)解算的原理是對于一個確定的向量，用不同的坐標系表示時，他們所表示的大小和方向一定是相同的。但是由于這兩個坐標系的旋轉(zhuǎn)矩陣存在誤差，那么當一個向量經(jīng)過一個有誤差存在的旋轉(zhuǎn)矩陣后，在另一個坐標系中肯定和理論值是有偏差的，我們通過這個偏差來修正這個旋轉(zhuǎn)矩陣。這個旋轉(zhuǎn)矩陣的元素是四元數(shù)，我們修正的就是四元數(shù)，以此來修正姿態(tài)。

19.3 實驗例程

實驗內(nèi)容：利用MPU6050采集到數(shù)據(jù)獲取歐拉角顯示在TFTLCD上。

19.3.1 MPU6050內(nèi)部相關(guān)寄存器

（1） 電源管理寄存器1 （地址0x6B）

Bit 7：軟件復(fù)位

0：不復(fù)位MPU6050

1：復(fù)位MPU6050

Bit 6：休眠模式

0：正常工作模式

1：睡眠模式

Bit 5：循環(huán)模式

0：默認狀態(tài)

1：睡眠模式與喚醒模式交替運行

Bit 3：溫度傳感器使能

0：使能溫度傳感器

1：禁用溫度傳感器

Bit 2~Bit 0：選擇系統(tǒng)時鐘源

000：內(nèi)部8M RC時鐘源

001：PLL，使用X軸陀螺作為參考

010：PLL，使用Y軸陀螺作為參考

011：PLL，使用Z軸陀螺作為參考

100：PLL，使用外部32.768kHz作為參考

101：PLL，使用外部19.2MHz作為參考

110：保留

111：關(guān)閉時鐘，保持時序產(chǎn)生電路復(fù)位狀態(tài)