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

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

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

接下來的步驟是：

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

- 根據(jù)陀螺儀和加速度計的位置決定是否要反轉(zhuǎn)輸出值 不要設(shè)想陀螺儀陀的輸出有XY，它會適應(yīng)加速度計坐標系里的任何軸挣棕，盡管這個輸出是IMU模塊的一部分译隘。最好的辦法就是測試亲桥。

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

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

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

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

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

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

-重復(fù)上面的測試诱桂，將設(shè)備繞Y軸旋轉(zhuǎn)奥额，這次查看加速度計的X軸輸出（也就是AdcRx）。如果AdcRx增大（從水平位置開始旋轉(zhuǎn)的第一個90°）访诱，那AdcGyroXZ應(yīng)當減小。這是因為我們觀察的是重力矢量韩肝，當設(shè)備朝一個方向旋轉(zhuǎn)時矢量會朝相反的方向旋轉(zhuǎn)（相對坐標系運動）触菜。所以，如果你不想反轉(zhuǎn)RateAxz哀峻，你可以在公式3中引入正負號來解決這個問題： 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)在開始我們認為你已經(jīng)設(shè)置好了IMU模塊并能計算出正確的Axr缰冤，Ayr犬缨，Azr值（在第一部分加速度計中定義）以及RateAyz，RateAyz（在第二部分陀螺儀中）棉浸。下一步怀薛，我們分析這些值之間的關(guān)系并得到更準確的設(shè)備和地平面之間的傾角。

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

陀螺儀也會有噪聲象踊，但由于它檢測的是旋轉(zhuǎn)温亲，因此對線性機械運動沒那么敏感，不過陀螺儀有另外一種問題杯矩，比如漂移（當選擇停止的時候電壓不會回到零速率電壓）栈虚。然而，通過計算加速度計和陀螺儀的平均值我們能得到一個相對更準確的當前設(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) / Sensitivity

RyAcc = (AdcRy * Vref / 1023 – VzeroG) / Sensitivity

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

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

Racc = [RxAcc,RyAcc,RzAcc]

因為這些Racc的分量可由加速度計數(shù)據(jù)得到，我們可以把它當做算法的輸入料饥。 請注意Racc測量的是重力蒲犬，如果你得到的矢量長度約等于1g那么你就是正確的：

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

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

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

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

接來下我們引進一個新的向量： Rest = [RxEst,RyEst,RzEst]

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

-加速度計告訴我們：“你現(xiàn)在的位置是Racc” 我們回答：“謝謝，但讓我確認一下”

-然后根據(jù)陀螺儀的數(shù)據(jù)和上一次的Rest值修正這個值并輸出新的估算值Rest巡蘸。 -我們認為Rest是當前設(shè)備姿態(tài)的“最佳值”奋隶。 讓我們看看它是怎么實現(xiàn)的悦荒。

數(shù)列的開始唯欣，我們先認為加速度值正確并賦值： 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) – 當前加速度計測量值

在計算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)的角度匿刮。更多信息請閱讀 atan2.

所以僧凰，知道了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)速可由得到一個更準確的公式：

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標準化，我們可以認為它的長度是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^2

x = 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))^2

RxGyro = ?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).

其中堤结，當 RzGyro>=0時唆迁，Sign(RzGyro) = 1 , 當 RzGyro<0時，Sign(RzGyro) = -1 竞穷。 一個簡單的估算方法：

Sign(RzGyro) = Sign(RzEst(n-1))

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

現(xiàn)在我們回顧一下已經(jīng)得到的結(jié)果，我們在算法中的第n步菜皂，并計算出了下面的值： Racc – 加速度計讀取的當前值

Rgyro –根據(jù)Rest(-1)和當前陀螺儀讀取值所得

我們根據(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)重會隨著加速度計讀取的噪聲而改變蚪缀★牛卡爾曼濾波注重給你一個“最好”的理論結(jié)果，而此算法給你的是實際項目中“夠用”的結(jié)果询枚。你可以實現(xiàn)一個算法违帆，它能根據(jù)測量的噪聲而改變wGyro值，但對大部分應(yīng)用來說固定的權(quán)重也能工作的很好金蜀。 現(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)在刷后，再次標準化矢量：

R = SQRT(RxEst(n) ^2 + RyEst(n)^2 + ?RzEst(n)^2 )

RxEst(n) = RxEst(n)/R RyEst(n) = RyEst(n)/R

RzEst(n) = RzEst(n)/R

現(xiàn)在，可以再次進行下一輪循環(huán)了渊抄。

This guide originally appeared onstarlino.com, I've made a few light edits and re-posted it with permission. Thanks Starlino!