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.參考文獻

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.

轉載于:https://www.cnblogs.com/ilekoaiq/p/8849217.html

總結

以上是生活随笔為你收集整理的Madgwick算法详细解读的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。