Madgwick算法詳細解讀

Madgwick算法詳細解讀

極品巧克力

前言

接上一篇文章《Google Cardboard的九軸融合算法》。

Madgwick算法是另一種九軸融合的方法，普遍應用在旋翼飛行器上，效果也蠻不錯的。網上已經有不少madgwick算法的源代碼了。

本文結合參考文獻，按照我本身的理解，把Madgwick算法的前因後果講清楚。

1.加權融合

因爲姿態都是相對的，用來表示水平朝北靜止時的物體，表明運動後的物體。因此，運動時的物體相對水平朝北靜止時的物體的姿態能夠用四元數來表示。

主要基於Madgwick算法對傳感器數據進行融合。Madgwick算法的本質是加權整合t時刻的陀螺儀算出的姿態和加速度計磁場計共同算出的姿態，從而獲得最終的姿態。其加權公式以下。

其中，和是加權係數，它們是由各自的偏差佔整體偏差的比重所決定的，偏差所佔的比重越小則加權係數越大。設採樣時間間隔爲。陀螺儀的單位時間的偏差能夠經過查陀螺儀的手冊獲得，通常是一個很小的值，因此陀螺儀的偏差爲。而加速度計磁場計共同算出的姿態的偏差是由計算方法決定的，計算方法如梯度降低法、高斯牛頓迭代法、牛頓法、共軛梯度法等，因爲採用的方法是梯度降低法，因此其偏差爲梯度降低法中所選取的步長，步長越長則其計算結果的偏差越大。因此，整體偏差爲。

是陀螺儀算出的姿態的加權係數。

是加速度計磁場計所算出的姿態的加權係數。

因而，接下來須要獲得的是陀螺儀計算出的姿態和加速度計磁場計共同算出的姿態。

2.陀螺儀姿態估計

用陀螺儀的數據來計算出姿態。

三軸陀螺儀返回的數據是自身分別繞軸、軸、軸的角速度，這三個角速度分別用、、來表示。則陀螺儀返回的數據能夠當作實部爲零的四元數，用來表示。

而姿態四元數變化的速度與當前的姿態和角速度有關，其計算公式以下。

如今已知時刻的四元數和角速度，以及時刻的角速度，系統採樣間隔爲，求時刻的四元數。相似於常微分方程，套用改進的歐拉公式。

這裏用改進的歐拉方法，其實就是近似估計物體四元數在時刻到時刻之間的平均變化速度。而在實際過程當中，每每並不能獲得時刻的準確的四元數，而是一個最優的估計值。

從而獲得，

另外，在實際過程當中，受傳感器特性的影響，陀螺儀、加速度計、磁場計的最大采樣速度不同。系統採樣間隔是爲了知足這三個傳感器中最低的採樣速度，使得能同時採樣。而實際上，陀螺儀的採樣速度能夠比另外兩個傳感器的速度快不少。因此，若是爲了追求更高的精度的話，能夠分開採樣。假設陀螺儀的採樣間隔是，則可用代替上面公式中的，用歐拉公式一步一步地日後計算，重複次，直到，就獲得了高精度的下一個系統採樣時刻的四元數，而且這個四元數與另外兩個傳感器同步。

3.加速度計磁場計姿態估計

要用加速度計和磁場計來共同算出姿態。

當物體水平靜止朝北時，加速度計的理論輸出和磁場計的理論輸出分別以下，均可以當作是實部爲0的四元數。

將和的虛部組成一個向量。

而在物體運動以後，在時刻，上述的兩個傳感器的數據都發生了變化。加速度傳感器的歸一化後的數據和磁場計的歸一化後的數據分別以下，均可以當作是實部爲0的四元數。

將和的虛部組成一個向量。

設這個時候的物體的姿態爲，則按照四元數的矢量旋轉性質可創建和的關係，和的關係，獲得以下方程。

分別用旋轉矩陣表示。

將組合成一個新的矩陣。

因而，和的關係，用矩陣來表示。

在上式中，和是已知的，因此能夠由上式再反過來去求出。即求出一個，，由此轉換成，使得偏差平方和最小。

其中，是一個多元向量函數，而求多元向量函數的極值問題，在計算機中通常採用數值解法，如梯度降低法、高斯牛頓迭代法、牛頓法、共軛梯度法等。採用梯度降低法。

設的初值爲，爲4行1列的矩陣，則偏差平方和爲。假設這個初值四元數須要修正的量爲，則修正後的偏差平方和爲。

由泰勒公式展開可得，

其中，爲4行1列的矩陣。爲1行4列的矩陣，以下所示。

則梯度大小以下。

其中，爲向量與單位向量的夾角。因此，當爲時，上式有最小值，即偏差平方和降得最快。

若要讓爲，則要讓上述兩個向量的方向相同，即要知足以下的等式關係。

其中，爲一個大於零的比例係數。上式中的關於四元數的偏導，能夠計算以下。

其中，表明中第行第列的元素。的表達式以下。

而就是關於四元數的偏導，即其雅克比矩陣。

因此， 的計算能夠轉換成，

進行歸一化，除以其2範數，獲得梯度的方向，表達式以下。

而後再乘以步長，就獲得了各個自變量要改變的值。各個自變量現有的值加上要修正的量，獲得了新的對的估計，以下面公式所示。

梯度降低法是一個不停迭代的過程。用上一時刻的姿態做爲初值，即已知上一次的估計，代入上述公式中。重複以前的步驟，進行屢次迭代，直到小於某一閾值，此時，爲的最佳估計值。或者通過屢次迭代，直到知足，此時，爲的最佳估計值。

步長須要經過人爲來設定。步長越小，則最終結果的精度越高，但迭代的次數也會越多。步長越大，則最終結果的偏差越大，但迭代的次數也會越少。

用來表示最終經過梯度降低法融合加速度計和磁場計來共同估計出的姿態，即上述的最佳估計值。

4.算法簡化

由前面的陀螺儀積分的結果和加速度計磁場計優化的結果加權，就能夠獲得高精度的融合結果。

可是，在實際工程中，須要權衡計算精度和計算速度。在梯度降低法的迭代過程，雖然以前的方法可使得結果的精度更高，可是也增長了計算量。在實際工程中，若是要追求速度的話，能夠對這些地方進行簡化。

在時刻到時刻之間的變化速度能夠用以下公式近似。

因此，的計算公式也可簡化以下。

而梯度降低法的迭代過程，也能夠只用一步來簡化，即認爲一步就能夠近似達到最佳估計值。那就是要設置步長，使得迭代一次就能最接近最佳估計值。

其中，是一個根據實際狀況調節的量，用來彌補加速度計和磁場計的測量偏差。上式中的，若是要追求更高的精度，仍可以使用以前的計算公式。若是追求速度，採用簡化公式，因此，簡化後的計算公式以下。

最終，將和加權融合，獲得最佳的姿態估計。

其中，。在上式的分母中，因爲，因此，因而上式能夠進一步簡化成，

至關因而陀螺儀計算出來的四元數變化速度與加速度計磁場計計算出來的變化速度加權整合，前者的權重是1，後者的權重是。

而後，對進行歸一化，獲得。

5.磁場計修正

因爲物體是在不停地運動之中的，物體周圍的磁場容易受環境變化影響，即水平靜止朝北時的磁場計理論輸出可能會因爲環境的變化而發生改變。而水平靜止朝北時的加速度計理論輸出則幾乎不受環境影響。因此，爲了能獲得一個更加精確的姿態估計，不能像那樣採用一個固定值，而須要實時修正。

在傳感器剛開始運行的時候，即第一幀的時候，傳感器可能處於任意一種姿態，幾乎不會是水平靜止朝北的，因此磁場計的輸出幾乎不會是。因此，是未知的。而這時候，能夠用加速度計的輸出來計算出物體的姿態。

用旋轉矩陣表示。

在上式中，和是已知的，因此能夠由上式再反過來去求出第一幀的姿態。即求出一個，轉換成，使得最小。

用高斯牛頓迭代法來尋找這個最佳的四元數。先計算其雅克比矩陣，

假設當前四元數各個元素的偏差爲4行1列的矩陣，則。用最小二乘法來計算出。

因此，現有的四元數的值減去偏差，獲得新的四元數。

的初值能夠設爲以下。

重複上述公式，迭代屢次，直到達到最小值。

因而就獲得加速度計估計出的第一幀的姿態。

因此，根據四元數的座標系旋轉性質，能夠把座標系轉到水平的位置上，但並不能保證朝北。對於向量來說，座標系逆着四元數轉回去，就至關因而向量順着四元數繼續轉，獲得在這個水平座標系中的磁場的向量。

其中，和是在這個座標系中的軸和軸上的份量，因此能夠獲得。

以上是第一幀的時候獲得的方法。

再將加速度計估計出來的姿態做爲初值，將、、、代入到以前公式中，用梯度降低法迭代，獲得高精度的第一幀的姿態。

當物體發生運動以後，因爲周圍環境的影響，每一幀都要對進行修正。假設已經用以前的方法獲得上一幀的最佳的姿態估計，則這一幀的計算以下。

6.求讚揚

您以爲，本文值多少？ 

7.參考文獻

1. Madgwick S O H, Harrison A J L, Vaidyanathan R. Estimation of IMU and MARG orientation using a gradient descent algorithm[C]//Rehabilitation Robotics (ICORR), 2011 IEEE International Conference on. IEEE, 2011: 1-7.