19.1 MPU6050簡(jiǎn)介

19.1.1 芯片概述

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

19.1.2 引腳介紹

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

19.1.3 硬件電路

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

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

19.2.1 方向矩陣

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

19.2.2 方向余弦矩陣

19.2.3 歐拉角

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

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

從上面的描述過(guò)程可以發(fā)現(xiàn)，歐拉角實(shí)際是可以分解成三步來(lái)計(jì)算的：

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

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

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

根據(jù)上面的三個(gè)步驟，我們來(lái)通過(guò)以下實(shí)例來(lái)說(shuō)明歐拉角與方向余弦矩陣的轉(zhuǎn)換過(guò)程。

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

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

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

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

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

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

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

19.3 實(shí)驗(yàn)例程

實(shí)驗(yàn)內(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：默認(rèn)狀態(tài)

1：睡眠模式與喚醒模式交替運(yùn)行

Bit 3：溫度傳感器使能

0：使能溫度傳感器

1：禁用溫度傳感器

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

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

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

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

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

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

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

110：保留

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