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.