三步教你結(jié)合加速度傳感器和陀螺儀算法設(shè)計

這篇文章主要介紹加速度計和陀螺儀的數(shù)學(xué)模型和基本算法，以及如何融合這兩者，側(cè)重算法、思想的討論.

介紹

本指南旨在向興趣者介紹慣性MEMS（微機電系統(tǒng)）傳感器，特別是加速度計和陀螺儀以及其他整合IMU（慣性測量單元）設(shè)備。

IMU單元例子：上圖中MCU頂端的ACC Gyro 6DOF，名為USBThumb，支持USB/串口通信

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

- 加速度計（accelerometer）檢測什么

- 陀螺儀（gyroscope，也稱作 gyro）檢測什么

- 如何將傳感器ADC讀取的數(shù)據(jù)轉(zhuǎn)換為物理單位（加速度傳感器的單位是g，陀螺儀的單位是 度/秒）

- 如何結(jié)合加速度傳感器和陀螺儀的數(shù)據(jù)以得到設(shè)備和地平面之間的傾角的準(zhǔn)確信息

在整篇文章中我盡量將數(shù)學(xué)運算降低到最少。如果你知道什么是正弦、余弦、正切函數(shù)，那無論你的項目使用哪種平臺你應(yīng)該都會明白和運用這篇文章中的思想，這些平臺如Arduino、Propeller、Basic Stamp、Ateml芯片、PIC芯片等等。總有些人認(rèn)為使用IMU單元需要復(fù)雜的數(shù)學(xué)運算（復(fù)雜的FIR或IIR濾波，如卡爾曼濾波，Parks-McClellan濾波等）。你如果研究這些會得到很棒且很復(fù)雜的結(jié)果。我解釋事情的方式，只需要基本的數(shù)學(xué)。我非常堅信簡單的原則。我認(rèn)為一個簡單的系統(tǒng)更容易操作和監(jiān)控，另外許多嵌入式設(shè)備并不具備能力和資源去實現(xiàn)需要進行矩陣運算的復(fù)雜算法。

我會用我設(shè)計的一個新IMU模塊——Acc_Gyro Accelerometer + Gyro IMU作為例子。在下面的例子中我們會使用這個設(shè)備的參數(shù)。用這個模塊作為介紹非常合適，因為它由3個設(shè)備組成：

- LIS331AL (datasheet) – 3軸 2G 模擬加速度計

- LPR550AL (datasheet) – 雙軸（俯仰、翻滾） 500°/s 加速度傳感器

- LY550ALH (datasheet) –單軸（偏航）陀螺儀 最后這個設(shè)備在這篇介紹中不使用，不過他在?DCM Matrix implementation中有重要作用

它們一起組成了一個6自由度的慣性測量單元。這是個花哨的名字！然而，在花哨的名字后面是個非常有用的設(shè)備組合，接下來我們會詳細(xì)介紹之。

第一部分 加速度計

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

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

如果我們把模型放在地球上，球會落在Z-墻面上并對其施加一個1g的力，見下圖：

在這種情況下盒子沒有移動但我們?nèi)稳蛔x取到Z軸有-1g的值。球在墻壁上施加的壓力是由引力造成的。在理論上，它可以是不同類型的力量 - 例如，你可以想象我們的球是鐵質(zhì)的，將一個磁鐵放在盒子旁邊那球就會撞上另一面墻。引用這個例子只是為了說明加速度計的本質(zhì)是檢測力而非加速度。只是加速度所引起的慣性力正好能被加速度計的檢測裝置所捕獲。

雖然這個模型并非一個MEMS傳感器的真實構(gòu)造，但它用來解決與加速度計相關(guān)的問題相當(dāng)有效。實際上有些類似傳感器中有金屬小球，它們稱作傾角開關(guān)，但是它們的功能更弱，只能檢測設(shè)備是否在一定程度內(nèi)傾斜，卻不能得到傾斜的程度。

到目前為止，我們已經(jīng)分析了單軸的加速度計輸出，這是使用單軸加速度計所能得到的。三軸加速度計的真正價值在于它們能夠檢測全部三個軸的慣性力。讓我們回到盒子模型，并將盒子向右旋轉(zhuǎn)45度。現(xiàn)在球會與兩個面接觸：Z-和X-，見下圖：

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

在上一個模型中我們引入了重力并旋轉(zhuǎn)了盒子。在最后的兩個例子中我們分析了盒子在兩種情況下的輸出值，力矢量保持不變。雖然這有助于理解加速度計是怎么和外部力相互作用的，但如果我們將坐標(biāo)系換為加速度的三個軸并想象矢量力在周圍旋轉(zhuǎn)，這會更方便計算。

請看看在上面的模型，我保留了軸的顏色，以便你的思維能更好的從上一個模型轉(zhuǎn)到新的模型中。想象新模型中每個軸都分別垂直于原模型中各自的墻面。矢量R是加速度計所檢測的矢量（它可能是重力或上面例子中慣性力的合成）。RX，RY，RZ是矢量R在X，Y，Z上的投影。請注意下列關(guān)系：

，R ^ 2 = RX ^ 2 + RY ^ 2 + RZ ^ 2（公式1）

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

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

1 ^ 2 =（SQRT（1/2））^ 2 + 0 ^ 2 +（SQRT（1/2））^ 2在公式1中簡單的取代： R=1, Rx = -SQRT(1/2), Ry = 0 , Rz = -SQRT(1/2)

經(jīng)過一大段的理論序言后，我們和實際的加速度計很靠近了。RX，RY，RZ值是實際中加速度計輸出的線性相關(guān)值，你可以用它們進行各種計算。

在我們運用它之前我們先討論一點獲取加速度計數(shù)據(jù)的方法。大多數(shù)加速度計可歸為兩類：數(shù)字和模擬。數(shù)字加速度計可通過I2C，SPI或USART方式獲取信息，而模擬加速度計的輸出是一個在預(yù)定范圍內(nèi)的電壓值，你需要用ADC（模擬量轉(zhuǎn)數(shù)字量）模塊將其轉(zhuǎn)換為數(shù)字值。我將不會詳細(xì)介紹ADC是怎么工作的，部分原因是這是個很廣的話題，另一個原因是不同平臺的ADC都會有差別。有些MCU具有內(nèi)置ADC模塊，而有些則需要外部電路進行ADC轉(zhuǎn)換。不管使用什么類型的ADC模塊，你都會得到一個在一定范圍內(nèi)的數(shù)值。例如一個10位ADC模塊的輸出值范圍在0 .. 1023間，請注意，1023 = 2 ^ 10 -1。一個12位ADC模塊的輸出值范圍在0 .. 4095內(nèi)，注意，4095 = 2 ^ 12-1。

我們繼續(xù)，先考慮下一個簡單的例子，假設(shè)我們從10位ADC模塊得到了以下的三個軸的數(shù)據(jù)：

AdcRx = 586AdcRy = 630

AdcRz = 561

每個ADC模塊都有一個參考電壓，假設(shè)在我們的例子中，它是3.3V。要將一個10位的ADC值轉(zhuǎn)成電壓值，我們使用下列公式：

VoltsRx = AdcRx * VREF / 1023

小注：8位ADC的最大值是255 = 2 ^ 8 -1，12位ADC最大值是4095 = 2 ^ 12 -1。

將3個軸的值代入上式，得到：

VoltsRx = 586 * 3.3 / 1023 =～1.89V（結(jié)果取兩位小數(shù)）VoltsRy = 630 * 3.3 / 1023 =～2.03V

VoltsRz = 561 * 3.3 / 1023 =～1.81V

每個加速度計都有一個零加速度的電壓值，你可以在它的說明書中找到，這個電壓值對應(yīng)于加速度為0g。通過計算相對0g電壓的偏移量我們可以得到一個有符號的電壓值。比方說，0g電壓值 VzeroG= 1.65V，通過下面的方式可以得到相對0g電壓的偏移量:

DeltaVoltsRx = 1.89V - 1.65V = 0.24VDeltaVoltsRy = 2.03V - 1.65V = 0.38V

DeltaVoltsRz = 1.81V - 1.65V = 0.16V

現(xiàn)在我們得到了加速度計的電壓值，但它的單位還不是g（9.8m/s^2），最后的轉(zhuǎn)換，我們還需要引入加速度計的靈敏度（Sensitivity），單位通常是 mV/g。比方說，加速度計的靈敏度 Sensitivity= 478.5mV / g = 0.4785V /g。靈敏度值可以在加速度計說明書中找到。要獲得最后的單位為g的加速度，我們使用下列公式計算：

RX = DeltaVoltsRx /SensitivityRX = 0.24V / 0.4785V / G =～0.5gRY = 0.38V / 0.4785V / G =～0.79g

RZ = 0.16V / 0.4785V / G =～0.33g

當(dāng)然，我們可以把所有的步驟全部放在一個式子里，但我想通過介紹每一個步驟以便讓你了解怎么讀取一個ADC值并將其轉(zhuǎn)換為單位為g的矢量力的分量。

Rx = (AdcRx * Vref / 1023 – VzeroG) / Sensitivity(公式2)Ry = (AdcRy * Vref / 1023 – VzeroG) / Sensitivity

Rz = (AdcRz * Vref / 1023 – VzeroG) / Sensitivity

現(xiàn)在我們得到了慣性力矢量的三個分量，如果設(shè)備除了重力外不受任何外力影響，那我們就可以認(rèn)為這個方向就是重力矢量的方向。如果你想計算設(shè)備相對于地面的傾角，可以計算這個矢量和Z軸之間的夾角。如果你對每個軸的傾角都感興趣，你可以把這個結(jié)果分為兩個分量：X軸、Y軸傾角，這可以通過計算重力矢量和X、Y軸的夾角得到。計算這些角度比你想象的簡單，現(xiàn)在我們已經(jīng)算出了Rx，Ry，Rz的值，讓我們回到我們的上一個加速度模型，再加一些標(biāo)注上去：

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

cos(Axr) = Rx / R , 類似的：cos(Ayr) = Ry / R

cos(Azr) = Rz / R

從公式1我們可以推導(dǎo)出 R = SQRT( Rx^2 + Ry^2 + Rz^2)

通過arccos()函數(shù)（cos()的反函數(shù)）我們可以計算出所需的角度：

Axr = arccos(Rx/R)

Ayr = arccos(Ry/R)

Azr = arccos(Rz/R)

我們花了大段的篇幅來解釋加速度計模型，最后所要的只是以上這幾個公式。根據(jù)你的應(yīng)用場合，你可能會用到我們推導(dǎo)出來的幾個過渡公式。我們接下來要介紹陀螺儀模塊，并向大家介紹怎么融合加速度計和陀螺儀的數(shù)據(jù)以得到更精確的傾角值。

但在此之前，我們再介紹幾個很常用的公式：cosX = cos(Axr) = Rx / RcosY = cos(Ayr) = Ry / RcosZ = cos(Azr) = Rz / R

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

SQRT（cosX ^ 2 + COSY ^ 2 + cosZ ^ 2）= 1

這是個很好的性質(zhì)，因為它避免了我們一直檢測R向量的模（長度）。通常如果我們只是對慣性力的方向感興趣，那標(biāo)準(zhǔn)化模長以簡化其他計算是個明智的選擇。

第二部分陀螺儀

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

陀螺儀的每個通道檢測一個軸的旋轉(zhuǎn)。例如，一個2軸陀螺儀檢測繞X和Y軸的旋轉(zhuǎn)。為了用數(shù)字來表達(dá)這些旋轉(zhuǎn)，我們先引進一些符號。首先我們定義：

Rxz – 慣性力矢量R在XZ平面上的投影Ryz – 慣性力矢量R在YZ平面的上投影在由Rxz和Rz組成的直角三角形中，運用勾股定理可得：Rxz^2 = Rx^2 + Rz^2 ，同樣：Ryz^2 = Ry^2 + Rz^2同時注意：R^2 = Rxz^2 + Ry^2 ，這個公式可以公式1和上面的公式推導(dǎo)出來，也可由R和Ryz所組成的直角三角形推導(dǎo)出來R ^ 2 = Ryz ^ 2 + RX ^ 2

在這篇文章中我們不會用到這些公式，但知道模型中的那些數(shù)值間的關(guān)系有助于理解。

相反，我們按如下方法定義Z軸和Rxz、Ryz向量所成的夾角：AXZ - Rxz（矢量R在XZ平面的投影）和Z軸所成的夾角AYZ - Ryz（矢量R在YZ平面的投影）和Z軸所成夾角

現(xiàn)在我們離陀螺儀要測量的東西又近了一步。陀螺儀測量上面定義的角度的變化率。換句話說，它會輸出一個與上面這些角度變化率線性相關(guān)的值。為了解釋這一點，我們先假設(shè)在t0時刻，我們已測得繞Y軸旋轉(zhuǎn)的角度（也就是Axz），定義為Axz0，之后在t1時刻我們再次測量這個角度，得到Axz1。

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

RateAxz = (Axz1 – Axz0) / (t1 – t0).

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

在實際運用中，陀螺儀一般都不會直接給你一個單位為度/秒的值（除非它是個特殊的數(shù)字陀螺儀）。就像加速度計一樣，你會得到一個ADC值并且要用類似公式2的式子將其轉(zhuǎn)換成單位為 度/秒的值。讓我們來介紹陀螺儀輸出值轉(zhuǎn)換中的ADC部分（假設(shè)使用10位ADC模塊，如果是8位ADC，用1023代替255，如果是12為ADC用4095代替1023）。

RateAxz = (AdcGyroXZ * Vref / 1023 – VzeroRate) / Sensitivity公式3RateAyz = (AdcGyroYZ * Vref / 1023 – VzeroRate) / Sensitivity

AdcGyroXZ，AdcGyroYZ - 這兩個值由ADC讀取，它們分別代表矢量R的投影在XZ和YZ平面內(nèi)里的轉(zhuǎn)角，也可等價的說，旋轉(zhuǎn)可分解為單獨繞Y和X軸的運動。

Vref – ADC的參考電壓，上例中我們使用3.3V

VzeroRate – 是零變化率電壓，換句話說它是陀螺儀不受任何轉(zhuǎn)動影響時的輸出值，對Acc Gyro板來說，可以認(rèn)為是1.23V（此值通?？梢栽谡f明書中找到——但千萬別相信這個值，因為大多數(shù)的陀螺儀在焊接后會有一定的偏差，所以可以使用電壓計測量每個通道的輸出值，通常這個值在焊接后就不會改變，如果有跳動，在設(shè)備使用前寫一個校準(zhǔn)程序?qū)ζ溥M行測量，用戶應(yīng)當(dāng)在設(shè)備啟動的時候保持設(shè)備靜止以進行校準(zhǔn)）。

Sensitivity –陀螺儀的靈敏度，單位mV/(deg/s)，通常寫作mV/deg/s,它的意思就是如果旋轉(zhuǎn)速度增加1°/s,陀螺儀的輸出就會增加多少mV。Acc_Gyro板的靈敏度值是2mV/deg/s或0.002V/deg/s讓我們舉個例子，假設(shè)我們的ADC模塊返回以下值：AdcGyroXZ = 571AdcGyroXZ = 323用上面的公式，在代入Acc Gyro板的參數(shù)，可得：RateAxz = (571 * 3.3V / 1023 – 1.23V) / ( 0.002V/deg/s) =~ 306 deg/sRateAyz = (323 * 3.3V / 1023 – 1.23V) / ( 0.002V/deg/s) =~ -94 deg/s換句話說設(shè)備繞Y軸（也可以說在XZ平面內(nèi)）以306°/s速度和繞X軸（或者說YZ平面內(nèi)）以-94°/s的速度旋轉(zhuǎn)。請注意，負(fù)號表示該設(shè)備朝著反方向旋轉(zhuǎn)。按照慣例，一個方向的旋轉(zhuǎn)是正值。一份好的陀螺儀說明書會告訴你哪個方向是正的，否則你就要自己測試出哪個旋轉(zhuǎn)方向會使得輸出腳電壓增加。最好使用示波器進行測試，因為一旦你停止了旋轉(zhuǎn)，電壓就會掉回零速率水平。如果你使用的是萬用表，你得保持一定的旋轉(zhuǎn)速度幾秒鐘并同時比較電壓值和零速率電壓值。如果值大于零速率電壓值那說明這個旋轉(zhuǎn)方向是正向。

第三部分 將它們綜合起來。融合加速度計和陀螺儀的數(shù)據(jù)

如果你在閱讀這篇文章你可能已經(jīng)有了或準(zhǔn)備購買一個IMU設(shè)備，或者你準(zhǔn)備用獨立的加速度計和陀螺儀搭建一個。

在使用整合了加速度計和陀螺儀的IMU設(shè)備時，首先要做的就是統(tǒng)一它們的坐標(biāo)系。最簡單的辦法就是將加速度計作為參考坐標(biāo)系。大多數(shù)的加速度計技術(shù)說明書都會指出對應(yīng)于物理芯片或設(shè)備的XZY軸方向。例如，下面就是Acc Gyro板的說明書中給出的XYZ軸方向：

接下來的步驟是：

- 確定陀螺儀的輸出對應(yīng)到上述討論的RateAxz，RateAyz值。

- 根據(jù)陀螺儀和加速度計的位置決定是否要反轉(zhuǎn)輸出值

不要設(shè)想陀螺儀陀的輸出有XY，它會適應(yīng)加速度計坐標(biāo)系里的任何軸，盡管這個輸出是IMU模塊的一部分。最好的辦法就是測試。

接下來的示例用來確定哪個陀螺儀的輸出對應(yīng)RateAxz。

- 首先將設(shè)備保持水平。加速度計的XY軸輸出會是零加速度電壓（Acc Gyro板的值是1.65V）

- 接下來將設(shè)備繞Y軸旋轉(zhuǎn)，換句話說就是將設(shè)備在XZ平面內(nèi)旋轉(zhuǎn)，所以X、Z的加速度輸出值會變化而Y軸保持不變。

-當(dāng)以勻速旋轉(zhuǎn)設(shè)備的時候，注意陀螺儀的哪個通道輸出值變化了，其他輸出應(yīng)該保持不變。

- 在陀螺儀繞Y軸旋轉(zhuǎn)（在XZ平面內(nèi)旋轉(zhuǎn)）的時候輸出值變化的就是AdcGyroXZ，用于計算RateAxz

-最后一步，確認(rèn)旋轉(zhuǎn)的方向是否和我們的模型對應(yīng)，因為陀螺儀和加速度的位置關(guān)系，有時候你可能要把RateAxz值反向

-重復(fù)上面的測試，將設(shè)備繞Y軸旋轉(zhuǎn)，這次查看加速度計的X軸輸出（也就是AdcRx）。如果AdcRx增大（從水平位置開始旋轉(zhuǎn)的第一個90°），那AdcGyroXZ應(yīng)當(dāng)減小。這是因為我們觀察的是重力矢量，當(dāng)設(shè)備朝一個方向旋轉(zhuǎn)時矢量會朝相反的方向旋轉(zhuǎn)（相對坐標(biāo)系運動）。所以，如果你不想反轉(zhuǎn)RateAxz，你可以在公式3中引入正負(fù)號來解決這個問題：

RateAxz = InvertAxz * (AdcGyroXZ * Vref / 1023 – VzeroRate) / Sensitivity ,其中InvertAxz= 1 或-1

同樣的方法可以用來測試RateAyz，將設(shè)備繞X軸旋轉(zhuǎn)，你就能測出陀螺儀的哪個輸出對應(yīng)于RateAyz，以及它是否需要反轉(zhuǎn)。一旦你確定了InvertAyz，你就能可以用下面的公式來計算RateAyz：

RateAyz = InvertAyz * (AdcGyroYZ * Vref / 1023 – VzeroRate) / Sensitivity

如果對Acc Gyro板進行這些測試，你會得到下面的這些結(jié)果：

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

- RateAyz輸出管腳是GY4，InvertAyz = 1

從現(xiàn)在開始我們認(rèn)為你已經(jīng)設(shè)置好了IMU模塊并能計算出正確的Axr，Ayr，Azr值（在第一部分加速度計中定義）以及RateAyz，RateAyz（在第二部分陀螺儀中）。下一步，我們分析這些值之間的關(guān)系并得到更準(zhǔn)確的設(shè)備和地平面之間的傾角。

你可能會問自己一個問題，如果加速度計已經(jīng)告訴我們Axr，Ayr，Azr的傾角，為什么還要費事去得到陀螺儀的數(shù)據(jù)？答案很簡單：加速度計的數(shù)據(jù)不是100%準(zhǔn)確的。有幾個原因，還記加速度計測量的是慣性力，這個力可以由重力引起（理想情況只受重力影響），當(dāng)也可能由設(shè)備的加速度（運動）引起。因此，就算加速度計處于一個相對比較平穩(wěn)的狀態(tài)，它對一般的震動和機械噪聲很敏感。這就是為什么大部分的IMU系統(tǒng)都需要陀螺儀來使加速度計的輸出更平滑。但是怎么辦到這點呢？陀螺儀不受噪聲影響嗎？

陀螺儀也會有噪聲，但由于它檢測的是旋轉(zhuǎn)，因此對線性機械運動沒那么敏感，不過陀螺儀有另外一種問題，比如漂移（當(dāng)選擇停止的時候電壓不會回到零速率電壓）。然而，通過計算加速度計和陀螺儀的平均值我們能得到一個相對更準(zhǔn)確的當(dāng)前設(shè)備的傾角值，這比單獨使用加速度計更好。

接下來的步驟我會介紹一種算法，算法受卡爾曼濾波中的一些思想啟發(fā)，但是它更簡單并且更容易在嵌入式設(shè)備中實現(xiàn)。在此之前，讓我們先看看我們需要算法計算什么值。所要算的就是重力矢量R=[Rx,Ry,Rz]，它可由其他值推導(dǎo)出來，如Axr，Ayr，Azr或者cosX，cosY，cosZ，由這些值我們能得到設(shè)備相對地平面的傾角值，這些關(guān)系我們在第一部分已經(jīng)討論過。有人可能會說-根據(jù)第一部分的公式2我們不是已經(jīng)得到Rx，Ry，Rz的值了嗎？是的，但是記住，這些值只是由加速度計數(shù)據(jù)推導(dǎo)出來的，如果你直接將它們用于你的程序你會得到難以忍受的噪聲。為了避免進一步的混亂，我們重新定義加速度計的測量值：

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

RxAcc = (AdcRx * Vref / 1023 – VzeroG) / SensitivityRyAcc = (AdcRy * Vref / 1023 – VzeroG) / Sensitivity

RzAcc = (AdcRz * Vref / 1023 – VzeroG) / Sensitivity

現(xiàn)在我們得到了一組只來自于加速度計ADC的值。我們把這組數(shù)據(jù)叫做“vector”，并使用下面的符號：

Racc = [RxAcc,RyAcc,RzAcc]

因為這些Racc的分量可由加速度計數(shù)據(jù)得到，我們可以把它當(dāng)做算法的輸入。

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

|Racc| = SQRT(RxAcc^2 +RyAcc^2 + RzAcc^2),

但是請確定把矢量轉(zhuǎn)換成下面的矢量非常重要：

Racc(normalized) = [RxAcc/|Racc| , RyAcc/|Racc| , RzAcc/|Racc|].

這可以確保標(biāo)準(zhǔn)化Racc始終是1。

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

Rest = [RxEst,RyEst,RzEst]

這就是算法的輸出值，它經(jīng)過陀螺儀數(shù)據(jù)的修正和基于上一次估算的值。

這是算法所做的事：

-加速度計告訴我們：“你現(xiàn)在的位置是Racc”

我們回答：“謝謝，但讓我確認(rèn)一下”

-然后根據(jù)陀螺儀的數(shù)據(jù)和上一次的Rest值修正這個值并輸出新的估算值Rest。

-我們認(rèn)為Rest是當(dāng)前設(shè)備姿態(tài)的“最佳值”。

讓我們看看它是怎么實現(xiàn)的。

數(shù)列的開始，我們先認(rèn)為加速度值正確并賦值：

Rest(0) = Racc(0)

Rest和Racc是向量，所以上面的式子可以用3個簡單的式子代替，注意別重復(fù)了：

RxEst（0）= RxAcc（0）

RyEst（0）= RyAcc（0）

RzEst（0）= RzAcc（0）

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

假設(shè)我們在第n步。我們有兩列已知的值可以用：

Rest(n-1) – 前一個估算值，Rest(0) = Racc(0)

Racc(n) – 當(dāng)前加速度計測量值

在計算Rest（n）前，我們先引進一個新的值，它可由陀螺儀和前一個估算值得到。

叫做Rgyro，同樣它是個矢量并由3個分量組成：

Rgyro = [RxGyro,RyGyro,RzGyro]

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

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

tan(Axz) = Rx/Rz => Axz = atan2(Rx,Rz)

你可能從未用過atan2這個函數(shù)，它和atan類似，但atan返回值范圍是（-PI/2,PI/2），atan2返回值范圍是（-PI，PI），并且他有兩個參數(shù)。它能將Rx，Rz值轉(zhuǎn)換成360°（-PI，PI）內(nèi)的角度。

所以，知道了RxEst（n-1）和RzEst（n-1）我們發(fā)現(xiàn)：

Axz(n-1) = atan2( RxEst(n-1) , RzEst(n-1) ).

記住，陀螺儀測量的是Axz角度變化率，因此，我們可以按如下方法估算新的角度Axz（n）：

Axz(n) = Axz(n-1) + RateAxz(n) * T

請記住，RateAxz可由陀螺儀ADC讀取得到。通過使用平均轉(zhuǎn)速可由得到一個更準(zhǔn)確的公式：

RateAxzAvg =（RateAxz（N）+ RateAxz（N-1））/ 2

Axz(n) = Axz(n-1) + RateAxzAvg * T

同理可得：

Ayz(n) = Ayz(n-1) + RateAyz(n) * T

好了，現(xiàn)在我們有了Axz（n），Ayz（n）。現(xiàn)在我們?nèi)绾瓮茖?dǎo)出RxGyro/RyGyro?根據(jù)公式1我們可以把Rgyro長度寫成下式：

| Rgyro | = SQRT（RxGyro ^ 2 + RyGyro ^ 2 + RzGyro ^ 2）

同時，因為我們已經(jīng)將Racc標(biāo)準(zhǔn)化，我們可以認(rèn)為它的長度是1并且旋轉(zhuǎn)后保持不變，所以寫成下面的方式相對比較安全：

| Rgyro | = 1

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

x =RxGyro , y=RyGyro, z=RzGyro

根據(jù)上面的關(guān)系可得：

x = x / 1 = x / SQRT(x^2+y^2+z^2)分子分母同除以SQRT（X ^ 2 + Z ^ 2）x = ( x / SQRT(x^2 + z^2) ) / SQRT( (x^2 + y^2 + z^2) / (x^2 + z^2) )注意x / SQRT(x^2 + z^2) = sin(Axz), 所以:x = sin(Axz) / SQRT (1 + y^2 / (x^2 + z^2) )將SQRT內(nèi)部分式的分子分母同乘以z^2x = sin(Axz) / SQRT (1 + y^2* z ^2 / (z^2 * (x^2 + z^2)) )注意 z / SQRT(x^2 + z^2) = cos(Axz)， y / z = tan(Ayz), 所以最后可得:

x = sin(Axz) / SQRT (1 + cos(Axz)^2 * tan(Ayz)^2 )

替換成原來的符號可得：

RxGyro = sin(Axz(n)) / SQRT (1 + cos(Axz(n))^2 * tan(Ayz(n))^2 )

同理可得：

RyGyro = sin(Ayz(n)) / SQRT (1 + cos(Ayz(n))^2 * tan(Axz(n))^2 )

提示：這個公式還可以更進一步簡化。分式兩邊同除以sin（axz（你））可得：

RxGyro =1/ SQRT (1/ sin(Axz(n))^2+ cos(Axz(n))^2 / sin(Axz(n))^2* tan(Ayz(n))^2 )RxGyro =1/ SQRT (1/ sin(Axz(n))^2+ cot(Axz(n))^2* sin(Ayz(n))^2/ cos(Ayz(n))^2 )現(xiàn)在加減 cos(Axz(n))^2/sin(Axz(n))^2 = cot(Axz(n))^2RxGyro =1/ SQRT (1/ sin(Axz(n))^2-cos(Axz(n))^2/sin(Axz(n))^2 + cot(Axz(n))^2* sin(Ayz(n))^2/ cos(Ayz(n))^2+ cot(Axz(n))^2 )

綜合條件1、2和3、4可得：

RxGyro =1/ SQRT (1+ cot(Axz(n))^2 * sec(Ayz(n))^2 ), 其中cot(x) = 1 / tan(x)， sec(x) = 1 / cos(x)

這個公式只用了2個三角函數(shù)并且計算量更低。如果你有Mathematica程序，通過使用 FullSimplify [Sin[A]^2/ ( 1 + Cos[A]^2 * Tan[B]^2)]你可以驗證這個公式。

現(xiàn)在我們發(fā)現(xiàn)：

RzGyro=Sign(RzGyro)*SQRT(1 – RxGyro^2 – RyGyro^2).

其中，當(dāng) RzGyro>=0時，Sign(RzGyro) = 1 , 當(dāng) RzGyro<0時，Sign(RzGyro) = -1 。

一個簡單的估算方法：Sign(RzGyro) = Sign(RzEst(n-1))在實際應(yīng)用中，當(dāng)心RzEst（n-1）趨近于0。這時候你可以跳過整個陀螺儀階段并賦值：Rgyro=Rest（n-1）。Rz可以用作計算Axz和Ayz傾角的參考，當(dāng)它趨近于0時，它可能會溢出并引發(fā)不好的后果。這時你會得到很大的浮點數(shù)據(jù)，并且tan()/atan()函數(shù)得到的結(jié)果會缺乏精度。

現(xiàn)在我們回顧一下已經(jīng)得到的結(jié)果，我們在算法中的第n步，并計算出了下面的值：

Racc – 加速度計讀取的當(dāng)前值Rgyro –根據(jù)Rest(-1)和當(dāng)前陀螺儀讀取值所得我們根據(jù)哪個值來更新Rest（n）呢？你可能已經(jīng)猜到，兩者都采用。我們會用一個加權(quán)平均值，得：Rest(n) = (Racc * w1 + Rgyro * w2 ) / (w1 + w2)分子分母同除以w1，公式可簡化成：Rest(n) = (Racc * w1/w1 + Rgyro * w2/w1 ) / (w1/w1 + w2/w1)令w2=w1=wGyro，可得：Rest(n) = (Racc + Rgyro * wGyro ) / (1 + wGyro)在上面的公式中，wGyro表示我們對加速度計和陀螺儀的相信程度。這個值可以通過測試確定，根據(jù)經(jīng)驗值5-20之間會得到一個很好的結(jié)果。此算法和卡爾曼濾波最主要的差別是它的權(quán)重是相對固定的，而卡爾曼濾波中的權(quán)重會隨著加速度計讀取的噪聲而改變?？柭鼮V波注重給你一個“最好”的理論結(jié)果，而此算法給你的是實際項目中“夠用”的結(jié)果。你可以實現(xiàn)一個算法，它能根據(jù)測量的噪聲而改變wGyro值，但對大部分應(yīng)用來說固定的權(quán)重也能工作的很好?，F(xiàn)在得到最新的估算值還差一步：RxEst(n) = (RxAcc + RxGyro * wGyro ) / (1 + wGyro)RyEst(n) = (RyAcc + RyGyro * wGyro ) / (1 + wGyro)RzEst(n) = (RzAcc + RzGyro * wGyro ) / (1 + wGyro)現(xiàn)在，再次標(biāo)準(zhǔn)化矢量：R = SQRT(RxEst(n) ^2 + RyEst(n)^2 +RzEst(n)^2 )RxEst(n) = RxEst(n)/RRyEst(n) = RyEst(n)/RRzEst(n) = RzEst(n)/R現(xiàn)在，可以再次進行下一輪循環(huán)了。