轉(zhuǎn)載自： 陀螺儀

陀螺儀，測(cè)量角速度，具有高動(dòng)態(tài)特性，它是一個(gè)間接測(cè)量角度的器件。它測(cè)量的是角度的導(dǎo)數(shù)，即角速度，要將角速度對(duì)時(shí)間積分才能得到角度。

陀螺儀就是內(nèi)部有一個(gè)陀螺，它的軸由于陀螺效應(yīng)始終與初始方向平行，這樣就可以通過(guò)與初始方向的偏差計(jì)算出旋轉(zhuǎn)方向和角度。傳感器MPU6050實(shí)際上是一個(gè)結(jié)構(gòu)非常精密的芯片，內(nèi)部包含超微小的陀螺。

如果這個(gè)世界是理想的，美好的，那我們的問(wèn)題到此就解決了，從理論上講只用陀螺儀是可以完成姿態(tài)導(dǎo)航的任務(wù)的。只需要對(duì)3個(gè)軸的陀螺儀角速度進(jìn)行積分，得到3個(gè)方向上的旋轉(zhuǎn)角度，也就是姿態(tài)數(shù)據(jù)。這也就是說(shuō)的快速融合。

不過(guò)很遺憾，現(xiàn)實(shí)是殘酷的，由于誤差噪聲等的存在，對(duì)陀螺儀積分并不能夠得到完全準(zhǔn)確的姿態(tài)，尤其是運(yùn)轉(zhuǎn)一段時(shí)間以后，積分誤差的累加會(huì)讓得到的姿態(tài)和實(shí)際的相差甚遠(yuǎn)。

那么哪些原因會(huì)使陀螺儀得到的姿態(tài)結(jié)果不準(zhǔn)確呢？下面列舉除常見(jiàn)的幾種：

零點(diǎn)漂移

假設(shè)陀螺儀固定不動(dòng)，理想角速度值是0dps(degree per second)，但是存在零點(diǎn)漂移，例如有一個(gè)偏置0.1dps加在上面，于是測(cè)量出來(lái)是0.1dps，積分一秒之后，得到的角度是0.1度，1分鐘之后是6度，還能忍受，一小時(shí)之后是360度，轉(zhuǎn)了一圈，也就是說(shuō)，陀螺儀在短時(shí)間內(nèi)有很大的參考價(jià)值。

白噪聲

電信號(hào)的測(cè)量中，一定會(huì)帶有白噪聲，陀螺儀數(shù)據(jù)的測(cè)量也不例外。所以獲得的陀螺儀數(shù)據(jù)中也會(huì)帶有白噪聲，而且這種白噪聲會(huì)隨著積分而累加。

溫度/加速度影響

陀螺儀是一個(gè)溫度和加速度敏感的元器件。例如對(duì)于加速度，多軸飛行器中的馬達(dá)一般會(huì)帶來(lái)較強(qiáng)烈的振動(dòng)，一旦減震控制不好，就會(huì)在飛行過(guò)程中產(chǎn)生很大的加速度，必會(huì)帶來(lái)陀螺輸出的變化，引入誤差。這就是在陀螺儀數(shù)據(jù)手冊(cè)上常見(jiàn)的deg/sec/g指標(biāo)。

積分誤差

對(duì)陀螺儀角速度的積分是離散的，長(zhǎng)時(shí)間的積分會(huì)出現(xiàn)漂移的情況。所以要考慮積分誤差的問(wèn)題。

這是由于陀螺儀測(cè)量姿態(tài)存在這么多的誤差，所以我們必須要使用其它傳感器輔助校正，其中最重要的就是下面的加速度傳感器。

加速度計(jì)的低頻特性好，可以測(cè)量低速的靜態(tài)加速度。在我們的飛行器上，就是對(duì)重力加速度g（也就是前面說(shuō)的靜態(tài)加速度）的測(cè)量和分析，其它瞬間加速度可以忽略。記住這一點(diǎn)對(duì)姿態(tài)解算融合理解非常重要。

當(dāng)我們把加速度計(jì)拿在手上隨意轉(zhuǎn)動(dòng)時(shí)，我們看的是重力加速度在三個(gè)軸上的分量值。加速度計(jì)在自由落體時(shí)，其輸出為0。為什么會(huì)這樣呢？這里涉及到加速度計(jì)的設(shè)計(jì)原理：加速度計(jì)測(cè)量加速度是通過(guò)比力來(lái)測(cè)量，而不是通過(guò)加速度。

加速度計(jì)僅僅測(cè)量的是重力加速度，3軸加速度計(jì)輸出重力加速度在加速度計(jì)所在機(jī)體坐標(biāo)系3個(gè)軸上的分量大小。重力加速度的方向和大小是固定的。通過(guò)這種關(guān)系，可以得到加速度計(jì)所在平面與地面的角度關(guān)系.

加速度計(jì)若是繞著重力加速度的軸轉(zhuǎn)動(dòng)，則測(cè)量值不會(huì)改變，也就是說(shuō)加速度計(jì)無(wú)法感知這種水平旋轉(zhuǎn)。

有關(guān)陀螺儀和加速度計(jì)和關(guān)系，姿態(tài)解算融合的原理，再把下面這個(gè)比喻放到這里一遍。

機(jī)體好似一條船，姿態(tài)就是航向（船頭的方位），重力是燈塔，陀螺（角速度積分）是舵手，加速度計(jì)是瞭望手。舵手負(fù)責(zé)估計(jì)和把穩(wěn)航向，他相信自己，本來(lái)船向北開(kāi)的，就一定會(huì)一直往北開(kāi)，覺(jué)得轉(zhuǎn)了90度彎，那就會(huì)往東開(kāi)。當(dāng)然如果舵手很牛逼，也許能估計(jì)很準(zhǔn)確，維持很長(zhǎng)時(shí)間。不過(guò)只信任舵手，肯定會(huì)迷路，所以一般都有瞭望手來(lái)觀察誤差。

瞭望手根據(jù)地圖燈塔方位和船的當(dāng)前航向，算出燈塔理論上應(yīng)該在船的X方位。然而看到實(shí)際燈塔在船的Y方位，那肯定船的當(dāng)前航向有偏差了，偏差就是ERR=X-Y。舵手收到瞭望手給的ERR報(bào)告，覺(jué)得可靠，那就聽(tīng)個(gè)90%ERR，覺(jué)得天氣不好、地圖誤差大，那就聽(tīng)個(gè)10%ERR，根據(jù)這個(gè)來(lái)糾正估算航向。

MPU-60x0是全球首例9軸運(yùn)動(dòng)處理傳感器。它集成了3軸MEMS陀螺儀，3軸MEMS加速度計(jì)，以及一個(gè)可擴(kuò)展的數(shù)字運(yùn)動(dòng)處理器DMP（Digital Motion Processor），可用I2C接口連接一個(gè)第三方的數(shù)字傳感器，比如磁力計(jì)。擴(kuò)展之后就可以通過(guò)其I2C或SPI接口輸出一個(gè)9軸的信號(hào)（SPI接口僅在MPU-6000可用）。MPU-60x0也可以通過(guò)其I2C接口連接非慣性的數(shù)字傳感器，比如壓力傳感器。

MPU-60x0對(duì)陀螺儀和加速度計(jì)分別用了三個(gè)16位的ADC，將其測(cè)量的模擬量轉(zhuǎn)化為可輸出的數(shù)字量。為了精確跟蹤快速和慢速的運(yùn)動(dòng)，傳感器的測(cè)量范圍都是用戶可控的，陀螺儀可測(cè)范圍為±250，±500，±1000，±2000°/秒（dps），加速度計(jì)可測(cè)范圍為±2，±4，±8，±16g。一個(gè)片上1024字節(jié)的FIFO，有助于降低系統(tǒng)功耗。和所有設(shè)備寄存器之間的通信采用400kHz的I2C接口或1MHz 的SPI接口（SPI僅MPU-6000可用）。對(duì)于需要高速傳輸?shù)膽?yīng)用，對(duì)寄存器的讀取和中斷可用20MHz的SPI。另外，片上還內(nèi)嵌了一個(gè)溫度傳感器和在工作環(huán)境下僅有±1%變動(dòng)的振蕩器。

在crazepony上，MPU6050/HMC5883/MS5611傳感器之間的連接如下圖所示。

在Crazepony上，測(cè)試了軟件解算四元素，然后通過(guò)四元素解算姿態(tài)角這種實(shí)現(xiàn)方式，其實(shí)總的來(lái)說(shuō)，并沒(méi)感覺(jué)36MHz的主控壓力有多大，沒(méi)有出現(xiàn)機(jī)身不穩(wěn)，卡死的情況。

同時(shí)，本著務(wù)實(shí)他的態(tài)度，我們也測(cè)試了MPU6050的硬解四元素，即從IIC總線上讀到的數(shù)據(jù)不再是MPU60x0的AD值，而是通過(guò)初始化對(duì)DMP引擎的配置，從IIC總線上讀到的直接就是四元素的值，從而跳過(guò)了程序通過(guò)AD值計(jì)算四元素這個(gè)看起來(lái)繁瑣的步驟。測(cè)試結(jié)果是，機(jī)身反應(yīng)的確要比之前反應(yīng)靈活，最關(guān)鍵的一點(diǎn)是，這樣得出的偏航角（Yaw）很穩(wěn)很穩(wěn)，基本不會(huì)漂移或者說(shuō)漂移小到了可以容忍的地步。

最后，MPU60x0的強(qiáng)大之處不僅于此，它支持一個(gè)從IIC接口，可以外部接上一個(gè)磁力計(jì)，如HMC5883，這樣一來(lái)，DMP引擎可以直接輸出一個(gè)絕對(duì)的方向姿態(tài)，即能夠輸出一個(gè)帶東西南北的姿態(tài)數(shù)據(jù)包，很厲害的樣子。

在Crazepony的5.2版本及以后，我們都會(huì)使用軟件解算的辦法，自己做姿態(tài)融合，不會(huì)再使用MPU6050自帶的DMP進(jìn)行硬件解算。