加速度計和陀螺儀指南

本文轉自中文版--加速度計和陀螺儀指南，我隻是重新用

重新編輯了一下公式，便于閱讀

翻譯自

這篇文章主要介紹加速度計個陀螺儀的數學模型和基本算法，以及如何融合這兩者，側重算法、思想的讨論。

——————————————————————————————————————————————————————————————————————————————————————————————————————————

本指南旨在向興趣者介紹慣性MEMS(Micro-Electro-Mechanical System，微機電系統)傳感器，特别是加速度計和陀螺儀以及其他整合IMU(Inertial Measurement Unit，慣性測量單元）裝置。

在這篇文章中我将概括這麼幾個基本并且重要的話題：

• 加速度計（accelerometer）檢測什麼
• 陀螺儀(gyroscope，也稱作grro)檢測什麼
• 如何将傳感器ADC讀取的資料轉換為實體單元（加速度傳感器的機關是g,陀螺儀的機關是 度/秒）
• 如何結合加速度傳感器和陀螺儀的資料以得到裝置和平面之間的傾角的準确資訊

在整篇文章中我盡量将數學運算降低到最少。如果你知道什麼是正弦、餘弦、正切函數，那無論你的項目使用哪種平台你應該都會明白和運用這篇文章中的思想，這些平台如Arduino、Propeller、Basic Stamp、Ateml晶片、PIC晶片等等。總有些人認為使用IMU單元需要複雜的數學運算（複雜的FIR或IIR濾波，如卡爾曼濾波，Parks-McClellan濾波等）。你如果研究這些會得到很棒且很複雜的結果。我解釋事情的方式，隻需要基本的數學。我非常堅信簡單的原則。我認為一個簡單的系統更容易操作和監控，另外許多嵌入式裝置并不具備能力和資源去實作需要進行矩陣的複雜算法。

第一章 加速度計

要了解這個子產品我們先從加速度計開始，當我們在想象一個加速度計的時候，我們可以把它想作一個圓球在一個方盒子中，你可能會把它想作一個餅幹或者甜圈，但我就把它當做一個球好了：

我們假定這個盒子不在重力場中或者其他任何會影響球的位置的場中，球處于盒子的正中央。你可以想象盒子在外太空中，如果很難想象，那就當做盒子在航天飛機中，一切東西都處于無重力狀态。在上面的圖中你可以看到我們給每個軸配置設定一對牆（我們移除了

以此來觀察裡面的情況）。設想 每面都能感測壓力。如果，我們突然把盒子向左移動（加速度為

） ，那麼球會撞上

牆，然後我們檢測球撞擊牆面産生的壓力，

軸輸出值為

請注意加速度計檢測到的力的方向與它本身加速度的方向是相反的。這種力量被稱為慣性力或假想力。在這個模型中你應該學到加速度計是通過間接測量力對一個牆面的作用來才測量加速度的，在實際應用中，可能通過彈簧等裝置來測量力。這個力可以是加速度引起的，但在下面的例子中，我們會發現它不一定是加速度引起的。

如果我們把模型放在地球上，球會落在

牆面上并對其施加一個

的力，見下圖：

在這種情況下盒子沒有移動但我們任然讀取到

軸有

的值。球在牆壁上施加的壓力是有引力造成的。在理論上，它可以是不同類型的力量，例如，你可以想象我們的球是鐵質的，将一個磁鐵放在盒子旁邊那球就會撞上另一面牆。引用這個例子隻是為了說明加速度計的本質是檢測力而非加速度。隻是加速度所引起的慣性力正好能被加速度計的檢測裝置所捕獲。

雖然這個模型并非一個MEMS傳感器的真實構造，但它用來解決與加速度計相關的問題相當有效。實際上有些類似傳感器有金屬小球，他們稱作傾角開關，但是它們的功能更弱，隻能檢測裝置是否在一定程度内傾斜，卻不能得到傾斜的程度。

到目前為止，我們已經分析了單軸的加速度計輸出，這是使用；單軸加速度計所能得到的。三軸加速度計的真正價值在于它們能夠檢測全部三個軸的慣性力。讓我們回到盒子模型，并将盒子向右旋轉45度。現在球會與兩個面接觸：

和

，見下圖：

0.71這個值不是任意的，它們實際上是0.5的平方根的近似值。我們介紹介紹加速度計的下一個模型時這一點會更清楚。

在上一個模型中，我們引入了重力并旋轉了盒子。在最後的兩個例子中我們分析了盒子在兩種情況下的輸出值，力矢量保持不變。雖然這有助于了解加速度計是怎麼和外部力互相作用的，但如果我們将坐标系換為加速度的三個軸并想象矢量力在周圍旋轉，這會更友善計算。

請看看上面的模型，我保留了軸的顔色，以便你的思維能更好的從上一個模型轉到新的模型中，想象新模型中對的每個軸都分别于垂直于原模型中的各自的牆面。矢量

是加速度計所檢測的矢量（它可能是重力或上面例子中慣性力的合成），

、

、

是矢量

在

、

、

上的投影。請注意下列關系：

（公式1）

此公式等價于三維空間勾股定理。

還記得我剛才說的1/2的平方根0.71不是個随機值吧。如果你把它們帶回去，回顧一下重力加速度是1g，那我們就能驗證：

，

在公式1中簡單的取代：

。

經過一大段理論序言後，我們和實際的加速度計很靠近了。

、

、

值是實際中加速度計輸出的線性相關值，你可以用它們進行各種計算。

大多數加速度計可歸為兩類：數字和模拟。數字加速計可通過12C，SPI或USART方式擷取資訊，而模拟加速度計的輸出是一個預定範圍内的電壓值，你需要用ADC（模拟量轉數字量）子產品将其轉換為數字值。我将不會詳細介紹ADC是怎麼工作的，部分原因是這是個很廣的話題，另一個原因是不同平台的ADC都會有差别。有些MCU具有内置的ADC子產品，而有些則需要外部電路進行ADC轉換。不管使用什麼類型的ADC子產品，你都會得到一個在一定範圍内的數值。例如一個10位ADC子產品的輸出範圍在0—1023之間，請注意，1023=2^10-1.一個12位的ADC子產品的輸出範圍在0—4095内，注意，4095=2^12-1。我們繼續，先考慮下一個簡單的例子，假設我們從10位ADC子產品得到了以下三個軸的資料：

每個子產品都有一個參考電壓，假設在我們的例子中，它是3.3V。要将一個10位的ADC值轉成電壓值，我們使用下列公式：

每個加速度計都有一個零加速度的電壓值，你可以在它的說明書中找到，這個電壓值對應于加速度為0g。通過計算相對0g電壓的偏移量我們可以得到一個有符号的電壓值。比如說，0g電壓值

，通過下面的方式可以得到相對0g電壓的偏移量：

現在我們得到了加速度計的電壓值，但它的機關還不是g(9.8m/s)，最後的轉換，我們還需要引入加速度計的靈敏度(Sensitivity)，機關通常是mv/g。比如說。加速度計的靈敏度

。注意靈敏度值可以在加速度計說明書中找到。要獲得最後的機關為g的加速度，我們使用下列公式計算：

當然，我們可以把所有的步驟全部放在一個式子裡，但我想通過介紹每一個步驟以便讓你了解怎麼讀取一個ADC值并将其轉換為機關為g的矢量力的分量。

 （公式2）

現在我們得到了慣性力矢量的三個分量，如果裝置除了重力外不受任何外力影響，那我們就可以認為這個方向就是重力矢量的方向。如果你想計算裝置相對于地面的傾斜角，可以計算這個矢量和Z軸之間的夾角。如果，你對每個軸的傾角都感興趣，你可以把這個結果分為兩個分量：X軸、Y軸傾角，這可以通過計算重力矢量和X、Y軸的夾角得到。計算這些角度比你想象的簡單，現在回到我們的上一個加速度模型，再加上一些标注上去：

我們感興趣的角度是向量R和X，Y，Z軸之間的夾角，那就令這些角度為Axr，Ayr，Azr。觀察由R和Rx組成的直角三角形：

我們花了大段的篇幅來解釋加速度計模型，最後所要的隻是以上這幾個公式。根據你的應用場合，你可能會用到我們推導出來的幾個過渡公式。我們接下來要介紹陀螺儀子產品，并向大家介紹怎麼融合加速度計和陀螺儀的資料以得到更精确的傾角值。

但在此之前，我們再介紹幾個很常用的公式：

這三個公式通常稱作方向餘弦，它主要表達了機關向量（長度為1的向量）和R向量具有相同的方向。你可以很容易地驗證：

這是個很好的性質，因為它避免了我們一直檢測R向量的模。通常如果，我們隻是對慣性力的方向感興趣，那标準化以簡化其他計算是個明智的選擇。

第二部分 陀螺儀

對于陀螺儀我們将不會像加速度計一樣介紹它的等價盒子模型，而是直接跳過加速度計的第二個模型，通過這個模型我們會向大家介紹陀螺儀是怎麼工作的。

陀螺儀的每個通道檢測一個軸的旋轉。例如，一個二軸陀螺儀檢測繞X和Y軸的旋轉。為了用數字來表達這些旋轉，我們引進一些符号），首先我們定義：

-慣性力矢量R在XZ平面上的投影

-慣性力矢量R在YZ平面上的投影

在由Rxz和Rz組成的直角三角形中，運用勾股定理可得：

同時注意：

，

這個公式可以和公式1和上面的公式推導出來，也可由R和Ryz所組成的直角三角形推導出來。在這篇文章中我們不會用到這些公式，但有助于了解模型中的那些數值間的關系。

相反，我們按如下方法定義Z軸和

、

向量所成的夾角：

：Z軸和

之間的夾角

：Z軸和

之間的夾角。

現在，我們離陀螺儀要測量的東西又近了一步。陀螺儀測量上面定義的角度的變化率。換句話說，它會輸出一個與上面這些角度變化率線性相關的值。為了解釋這一點。我們先假設在

時刻，我們已測得繞Y軸旋轉的角度，定義為

，之後在

時刻時刻，我們再次測量這個角度為

。角度的變化率按下面的方法計算：

，

如果用度表示角度，用秒來表示時間，那這個值的機關就是 度/秒。這就是陀螺儀檢測的東西。

但是，在實際運用中，陀螺儀一般都不會給你一個機關為度/秒（除非它是一個特殊的數字陀螺儀）。這就像加速度計一樣，你回得到一個ADC值，并且要用類似公式2的式子将其轉換為機關為 度/秒的值。讓我們來介紹陀螺儀輸出值轉換中的ADC部分（假設使用10位ADC子產品，如果是8位ADC,用255代替1023，如果是12位，用4095代替1023）。

  （公式3）

AdcGyroXZ，AdcGyroYZ-這兩個值由ADC讀取，它們分别代表矢量R的投影在XZ和YZ平面内裡轉角，也可等價的說，旋轉可分解為單獨繞Y和X軸的運動。Vref-ADC的參考電壓，上例中我們使用3.3V。VzeroRate-是零變化率電壓，換句話說它是陀螺儀不受任何轉動影響時的輸出值，對Acc Gyro闆來說，可以認為是1.23V(此值通常可以在說明書中找到——但千萬别相信這個值，因為大多數的陀螺儀在焊接後會有一定的偏差，是以可是使用電壓計測量每個通道的輸出值，通常這個值在焊接後就不會改變，如果有跳動，在裝置使用前寫一個校準程式對其進行測量，使用者應當在裝置啟動的時候保持裝置靜止以進行校準)。

Sensitivity-陀螺儀的靈敏度，機關mV/(deg/s)，通常寫為mV/deg/s，它的意思就是如果旋轉速度增加1deg/s，陀螺儀的輸出值就會增加多少mV。Acc_Gyro闆的靈敏度值是2mv/deg/s或0.002V/deg/s。

讓我們舉個例子，假設我們的ADC子產品傳回以下值：

按照公式3，再帶入Acc Gyro闆的參數，可得：

換句話說裝置繞Y軸（也可以說在XZ平面内）以306deg/s的速度和繞X軸（或者說YZ平面内）以-94deg/s的速度旋轉。請注意：負号表示該裝置朝着反方向旋轉。按照慣例，一個方向的旋轉是正值。一份好的陀螺儀說明書會告訴你哪個方向是正的，否則你就要自己測試出哪個方向會使得輸出角電壓增加。最好使用示波器進行測試，因為一旦你停止了旋轉，電壓就會調回零速率水準。如果，你使用的萬用表，你得保持一定的旋轉速度幾秒鐘并同時比較電壓值和零速率電壓值。如果值大于零速率電壓值那說明這個旋轉方向是正向。

第三章  融合加速度計和陀螺儀的資料，将它們綜合起來

在使用整合了加速計和陀螺儀的IMU裝置時，首先要做的就是統一它們的坐标系。最簡單的辦法就是将加速度計作為參考坐标系。大多數的加速計技術說明書都會指出對應于實體晶片或裝置的XYZ軸方向。例如，下面就是Acc Gyro闆的說明書中給出的XYZ軸方向：

接下來的步驟是：

——确定陀螺儀的輸出對應到上述的RateAxz，RateAyz值。

—---根據陀螺儀和加速計的位置決定是否要反轉輸出值

不要設想陀螺儀的輸出有XY，它會适應加速度計坐标系裡的任何軸，盡管這個輸出是IMU子產品的一部分。最好的辦法就是測試。

接下來得執行個體用來确定哪個陀螺儀的輸出對用RateAxz。

——首先将裝置保持水準。加速度計的XY軸輸出會是零加速度電壓（Acc Gyro闆的值是1.65V）

—--接下來将裝置繞Y軸旋轉，換句話說就是裝置在XZ平面内旋轉，是以X,Z的加速度輸出值會變化而Y軸保持不變。

——當以勻速旋轉裝置的時候，注意陀螺儀的哪個通道輸出值變化了，其他輸出應該保持不變。

——在陀螺儀繞Y軸旋轉（在XZ平面内旋轉）的時候，輸出值變化的就是AdcGyroXZ，用于計算RateAxz。

------最後一步，确認旋轉的方向是否和我們的模型對應，因為陀螺儀和加速度的位置關系，有時候你可能把RateAxz值反向

——重複上面的測試，将裝置繞Y軸旋轉，這次檢視加速度計的X軸輸出（也就是AdcRx）。如果，AdcRx增大（從水準位置開始旋轉的第一個90度），那AdcGyroXZ應當減小。這是因為我們觀察的是重力矢量，當裝置朝一個方向旋轉時矢量會朝相反的方向旋轉（相對坐标系運動）。是以，如果你不想反轉RateAxz，你可以再公式3中引入正負号來解決這個問題：

其中：

或者

。

同樣的方法可以用來測試RateAyz，将裝置繞X軸旋轉，你就能測出陀螺儀的哪個輸出對應于RateAyz，以及它是否需要反轉。一旦你确定了InvertAyz，你就能可以用下面的公式來計算RateAyz:

如果對Acc Gyro闆進行這些測試，你會得到下面的這些結果：

——RateAxz的輸出管腳是GX4，InvertAxz =1

——RateAyz的輸出管腳是GY4，InvertAyz=1

_____________________________________________________

______________________________________________________

從現在開始我們認為你已經設定好了IMU子產品并能計算出正确的Axr，Ayr,Azr值（在第一部分加速度計中定義）以及RateAxz，RateAyz(在第二部分陀螺儀中)。下一步，我們分析這些值之間的關系并得到更準确的裝置和地平面之間的傾角。

你可能會問自己一個問題，如果加速度計已經告訴我們Axr，Ayr，Azr的傾角，為什麼還要費事去得到陀螺儀的資料？答案很簡單：加速度計的資料不是100%準确。有幾個原因，還記加速度計測量的是慣性力，這個力可以由重力引起（理想情況隻受重力影響），當也可能由裝置的加速度（運動）引起。是以，就算加速度計處于一個相對比較平穩的狀态，它對一般的震動和機械噪聲很敏感。這就是為什麼大部分的IMU系統都需要陀螺儀來使加速度計的輸出更平滑。但是怎麼辦到這點呢？陀螺儀不受影響嗎？

陀螺儀也會有噪聲，但由于它檢測的是旋轉，是以對線性機械運動沒那麼敏感，不過陀螺儀有另外一種問題，比如漂移（當選擇停止的時候電壓不會回到零速率電壓）。然而，通過計算加速度計和陀螺儀的平均值我們能得到一個相對更準确的目前裝置的傾角值，這比單獨使用加速度計更好。

接下來的步驟我會介紹一種算法，算法手卡爾曼濾波中的一些思想啟發，但是它更簡單并且更容易在嵌入式裝置中實作。在此之前。讓我們先看看我們需要算法計算什麼值。所要算的就是重力矢量

，它可由其他值推導出來，如

Axr，Ayr，Azr或者cosX，cosY，cosZ，由這些值我們能得到裝置相對于地平面的傾角值，這些關系我們再第一部分已經讨論過。有人可能會說-根據第一部分的公式2，我們不是已經得到Rx，Ry，Rz的值了嗎？是的，但是記住，這些值隻是由加速度計資料推導出來的，如果你直接将它們用于你的程式你會得到難以忍受的噪聲。為了避免進一步的混亂。我們重新定義加速度計的測量值：

Racc-是由加速度計測量到得慣性力矢量，它可分解為下面的分量（在XYZ軸上的投影）：

現在我們得到了一組隻來自加速度計ADC的值。我們把這組資料叫做"vector"，并使用下面的符号：

因為這些Racc的分量可由加速度計資料得到，我們可以把它當做算法的輸入。

請注意Racc測量的是重力，如果你得到的矢量長度約等于1g那麼你就是正确的：

，

但是請确定把矢量轉換成下面的矢量非常重要：

。

這可以確定保準化Racc始終是1。

接下來我們引進一個新的向量：

這就是算法的輸出值，它經過陀螺儀資料的修正和基于上一次估算的值。

這是算法所做的事：

——加速度計告訴我們："你現在的位置是Racc"。

我們回答："謝謝，但讓我确認一下"。

—--然後根據陀螺儀的資料和上一次的Rest值修正這個值并輸出新的估算值Rest。

我們認為Rest是目前裝置姿态的”最佳值“。

讓我們看看它是怎麼實作的。

數列的開始，我們先認為加速度值正确并指派：

Rest和Racc是向量，是以上面的式子可以用3個簡單的式子代替，注意别重複了：

接下來我們再每個等時間間隔T秒做一次測量，得到新的測量值，并定義為Racc(1)，Racc(2)，Racc(3)等。同時，在每個時間間隔我們也計算出新的估計值Rest(1)，Rest(2)，Rest(3)，等等。

假設我們再第n步。我們有兩列已知的值可以用，即Rest(n-1)和Racc(n)。

Rest(n-1)代表前一個估算值，Rest(0)=Racc(0)，Racc(n)代表加速度計測量值。

在計算Rest(n)前，我們先引進一個新的值，它可由陀螺儀和前一個估算值得到，叫做Rgyro，同樣它是個矢量并由3個分量組成：

我們分别計算這個矢量的分量，從RxGyro開始。

首先觀察陀螺儀模型中下面的關系，根據由Rz和Rxz組成的直角三角形我們能推出：

，

，

你肯能從未用過atan2這個函數，它和atan類似，但atan傳回值範圍是(-PI/2,PI/2)，atan2傳回值範圍是（-PI，PI），并且他有個參數。它能将Rx，Rz值轉換成360（-PI，PI）内的角度。更多資訊檢視atan2

是以，知道了RxEst（n-1）和RzEst(n-1)，我們發現：

。

記住，陀螺儀測量的是Axz角度變化率，是以，我們可以按如下方法估算新的角度Axz(n):

同理可得：

，

好了。現在我們有了Axz(n),Ayz(n)。現在我們如何推導出RxGyro/RyGyro?根據公式1，我們可以把Rgyro長度寫成下式：

同時，因為我們已經将Racc标準化，我們可以認為它的長度是1并且旋轉後保持不變，是以寫成下面的方式相對比較安全：

我們暫時采用更短的符号進行下面的計算：

根據上面的關系可得：

，

分子分母除以

，得到：

，

注意

，

是以：

，

注意：

，

是以最後可得：

，

替換成原來的符号可得：

，

同理可得：

。

提示：這個公式還可以更進一步簡化。分式兩邊同除以

，可得：

，

繼續簡化得：

上面這個公式隻用了2個三角函數，計算量低。

—————————————————————————————————————————————————————————————————————————————

現在我們發現：

其中，當

時，

,當

時，

。

一個簡單的估算方法：

。

在實際應用中，當心

趨近于0時。這時候你可以跳過整個陀螺儀階段并指派：

。

可以用作計算

和

傾角的參考，當它趨近于0時，它可能會溢出并引發不好的結果。這時你會得到很大的浮點資料，并且

和

函數得到的結果會缺乏精度。

現在我們回顧一下已經得到的結果，我們再算法中的第n步，并計算出了下面的值：

代表加速度計讀取的目前值

代表根據

和目前陀螺儀讀取值所得

我們根據哪個值來更新

呢？你可能已經猜到，兩者都采用。我們用一個權重平均值，得：

分子分母同除以

，公式可簡化為：

現在，再次标準化矢量：

現在，可以再次進行下一輪循環了。