當然，在開(kāi)始之前至少要弄懂什么是

加速度計

，什么是陀螺儀吧，否則那后邊講的都是沒(méi)有意義的。簡(jiǎn)單的說(shuō)，加速度計主要是測量物體運動(dòng)的加速度，陀螺儀主要測量物體轉動(dòng)的角速度。這些理論的知識我就不多說(shuō)了，都可以在網(wǎng)上查到。這里推薦一篇講的比較詳細的文章《AGuide To using IMU (Accelerometer and Gyroscope Devices) inEmbeddedApplications》，在網(wǎng)上可以直接搜索到。

 

2、加速度測量

 

       在開(kāi)始之前，不知大家是否還記得加速度具有合成定理？如果不記得可以先大概了解一下，其實(shí)簡(jiǎn)單的舉個(gè)例子來(lái)說(shuō)就是重力加速度可以理解成是由x,y,z三個(gè)方向的加速度共同作用的結果。反過(guò)來(lái)說(shuō)就是重力加速度可以分解成x,y,z三個(gè)方向的加速度。

 

       加速度計可以測量某一時(shí)刻x,y,z三個(gè)方向的加速度值。而自平衡小車(chē)利用加速度計測出重力加速度在x,y,z軸的分量，然后利用各個(gè)方向的分量與重力加速度的比值來(lái)計算出小車(chē)大致的傾角。其實(shí)在自平衡小車(chē)上非靜止的時(shí)候，加速度計測出的結果并不是非常精確。因為大家在高中物理的時(shí)候都學(xué)過(guò)，物體時(shí)刻都會(huì )受到地球的萬(wàn)有引力作用產(chǎn)生一個(gè)向下的重力加速度，而小車(chē)在動(dòng)態(tài)時(shí)，受電機的作用肯定有一個(gè)前進(jìn)或者后退方向的作用力，而加速度計測出的結果是，重力加速度與小車(chē)運動(dòng)加速度合成得到一個(gè)總的加速度在三個(gè)方向上的分量。

 

       不過(guò)我們暫時(shí)不考慮電機作用產(chǎn)生的運動(dòng)加速度對測量結果的影響。因為我們要先把復雜的事情分解成一個(gè)個(gè)簡(jiǎn)單的事情來(lái)分析，這樣才能看到成果，才會(huì )有信心繼續。

 

       下邊我們就開(kāi)始分析從加速度得到角度的方法。如下圖，把加速度計平放，分別畫(huà)出xyz軸的方向。這三個(gè)軸就是我們后邊分析所要用到的坐標系。如圖一

 

 

圖一

       把mpu6050安裝在自平衡車(chē)上時(shí)也是這樣的水平安裝在小車(chē)底盤(pán)上的，假設兩個(gè)車(chē)輪安裝時(shí)車(chē)軸和y軸在一條直線(xiàn)上。那么小車(chē)擺動(dòng)時(shí)，參考水平面就是桌面，并且車(chē)軸（y軸）與桌面始終是平行的，小車(chē)擺動(dòng)和移動(dòng)過(guò)程中y軸與桌面的夾角是不會(huì )發(fā)生變化的，一直是0度。發(fā)生變化的是x軸與桌面的夾角以及z軸與桌面的夾角，而且桌面與x軸z軸夾角變化度數是一樣的。所以我們只需要計算出x軸和z軸中任意一個(gè)軸的夾角就可以反映出小車(chē)的傾斜的情況了。如圖二

 

 

圖二

       為了方便分析，由于y軸與桌面夾角始終不變，我們從y軸的方向俯看下去，那么這個(gè)問(wèn)題就會(huì )簡(jiǎn)化成只有x軸和z軸的二維關(guān)系。假設某一時(shí)刻小車(chē)上加速度計（mpu6050）處于如下?tīng)顟B(tài)，下圖是我們看到簡(jiǎn)化后的模型。如圖三

 

 

圖三

       在這個(gè)圖中，y軸已經(jīng)簡(jiǎn)化和坐標系的原點(diǎn)o重合在了一起。我們來(lái)看看如何計算出小車(chē)的傾斜角，也就是與桌面的夾角a。（上圖g是重力加速度，gx、gz分別是g在x軸和z軸的分量。）

 

       由于重力加速度是垂直于水平面的，得到：∠a+∠b=90°，∠b+∠d=90°于是輕松的就可以得出：∠a=∠d。

 

       所以計算出角度d就等效于計算出了x軸與桌面的夾角a。根據正弦定理得出：sin（d）=gx/g。得到這個(gè)公式可是還是得不到想要的角度，因為需要計算反正弦，而反正弦在單片機里不是很好計算。

 

       為了得到角度，于是又查了相關(guān)資料，原來(lái)在角度較小的情況下，角度的正弦與角度對應的弧度成線(xiàn)性關(guān)系。如圖四：

 

 

圖四

這個(gè)圖x軸是角度，取值范圍是0~90度，有三個(gè)函數曲線(xiàn)，分別是：

 

y = sin(x)                           正弦曲線(xiàn)

 

y = x*π/180                       弧度

 

y = 0.92*x*π/180               乘以一個(gè)0.92系數的弧度

 

從圖上可以看出，當角度范圍是0~29度時(shí)，對于自平衡車(chē)來(lái)說(shuō)擺動(dòng)范圍在-29~29度之內sinx = x*3.14/180可用來(lái)代替y = sinx；如果超過(guò)這個(gè)范圍，小車(chē)姿態(tài)也無(wú)法調整。當然有時(shí)候也會(huì )擔心-29~29度的搖擺范圍還是無(wú)法滿(mǎn)足需求，那可以給上邊的公式乘一個(gè)系數，得到如下公式：sinx = k*x*3.14/180。從上邊的函數對比圖可以看出，當系數取0.92時(shí)，角度范圍可以擴大到-45~45度。

 

   經(jīng)過(guò)這一系列的分析，終于得到角度換算方法：

 

由

 

   sin(d) = gx/g

 

   sin(d) = k*d*3.14/180

 

得到：

 

   gx/g = k*d*3.14/180

 

   那么角度就可以通過(guò)如下公式計算出：

 

   d = 180*gx/(k*g*3.14)

 

而gx可以從加速度計里讀出來(lái)，所以這下角度就可以輕松得到了。而之前也說(shuō)過(guò)這個(gè)角度不是很精確，但是至少可以反映出角度變化的趨勢。不過(guò)可以通過(guò)卡爾曼濾波等算法把加速度計讀出的角度和陀螺儀讀出的角度結合起來(lái)，使小車(chē)的角度更加準確。

 

3、陀螺儀

 

       通過(guò)陀螺儀來(lái)測量角度就很簡(jiǎn)單了，因為陀螺儀讀出的是角速度，大家都知道，角速度乘以時(shí)間，就是轉過(guò)的角度。把每次計算出的角度做累加就會(huì )等到當前所在位置的角度。如圖五

 

 

圖五

假設最初陀螺儀是與桌面平行，單片機每t ms讀一次陀螺儀的角速度，當讀了三次角速度以后z軸轉到上圖的位置，則在這段時(shí)間中轉過(guò)的角度為a：

 

∠a = ∠1+∠2+∠3

 

假設從陀螺儀讀出的角速度為w，那總角度為：

 

∠a = (w1*t1+w2*t2+w3*t3)/1000

 

假設經(jīng)過(guò)n次，那么總的角度如下：

 

∠a = (w1*t1+w2*t2+w3*t3+…+w(n)*t(n))/1000

 

實(shí)際上這就是一個(gè)積分過(guò)程。其實(shí)這種計算出來(lái)的角度也存在一定的誤差，而且總的角度是經(jīng)過(guò)多次相加得到的，這樣誤差就會(huì )越積累越大，最終導致計算出的角度與實(shí)際角度相差很大。于是也可以使用卡爾曼濾波把加速度計讀出的角度結合在一起，使計算出的角度更準確。