1 前言

Mahony算法常見的姿態(tài)融合算法，根據(jù)加速度計(jì)、陀螺儀、以及磁力計(jì)，融合計(jì)算機(jī)體四元數(shù)，計(jì)算速度快、精度較高。本文介紹六軸融合，即根據(jù)加速度計(jì)和陀螺儀數(shù)據(jù)，計(jì)算姿態(tài)。
我們需要計(jì)算的是機(jī)體的姿態(tài)。計(jì)算角度可以通過角速度積分，也可以通過加速度正交分解，但這兩種方法都存在缺陷。角速度的誤差會(huì)隨著積分不斷增大，而加速度存在高頻噪聲，因此希望融合兩種數(shù)據(jù)。

2 算法

2.1 重力對齊誤差

首先要指出的是，Mahony算法假設(shè)加速度計(jì)測量的加速度完全由重力提供，即物體本體運(yùn)動(dòng)產(chǎn)生的加速度可忽略不計(jì)。在這一假設(shè)下，我們假設(shè)當(dāng)前時(shí)刻機(jī)體的姿態(tài)為，則將重力向量的表示轉(zhuǎn)到機(jī)體坐標(biāo)系下，應(yīng)該為：，這里表示四元數(shù)對應(yīng)的旋轉(zhuǎn)矩陣：

進(jìn)一步地，帶入，得到

我們計(jì)加速度計(jì)測量得到的加速度，如果此時(shí)沒有誤差，應(yīng)該有，但實(shí)際兩個(gè)向量并不重合，存在一定的誤差 。

為表示出，可以利用向量的叉乘：。因?yàn)椴娉说亩x為：，當(dāng)歸一化為單位向量時(shí)，反應(yīng)的就是角度。這里更準(zhǔn)確的寫為，下一時(shí)刻{t+1}時(shí)的誤差為：

其中 為根據(jù)當(dāng)前{t}時(shí)刻估計(jì)的角度四元數(shù)。再記這個(gè)誤差的積分量為：

誤差的積分量也參與了后續(xù)計(jì)算。

2.2 角速度融合

此時(shí)已經(jīng)計(jì)算出加速度計(jì)觀測出的誤差了，記陀螺儀提供的角速度為，則把陀螺儀角速度的誤差加上上述的誤差，采用控制中常用的比例-積分控制器思想，

得到糾正的角速度。

討論：為什么用叉乘？

陀螺儀由于本身精度問題，測量的角速度存在誤差，在積分過程中這個(gè)誤差會(huì)一直累加，我們要做的就是去消除或是補(bǔ)償這個(gè)誤差，因?yàn)榧铀俣扔?jì)長期的測量值是準(zhǔn)確的，所以可以用加速度計(jì)來進(jìn)行修正。如何找到一個(gè)另一個(gè)角速度量綱的值來修正陀螺儀的角速度值呢？這里明明只有陀螺儀可以測量角速度！這時(shí)候前面提到的向量叉積得到的誤差向量就幫上大忙了，這個(gè)誤差向量不就是反映出了角度變化量嗎。算法巧妙的將加速度相關(guān)量轉(zhuǎn)化為角度相關(guān)量，因而可以用這個(gè)角度值乘一個(gè)系數(shù)來修正陀螺儀的角速度，因?yàn)樵谄罱嵌群苄〉那闆r下，我們可以將陀螺儀角速度誤差和加速度計(jì)求得的角度差看做正比的關(guān)系，也就說明陀螺儀積分誤差和向量叉積存在正比關(guān)系。[2]

歡迎關(guān)注微信公眾號「3D視覺工坊」，加群/文章投稿/課程主講，請加微信：QYong2014，添加時(shí)請備注：加群/投稿/主講申請

方向主要包括：3D視覺領(lǐng)域各細(xì)分方向，比如相機(jī)標(biāo)定|三維點(diǎn)云|三維重建|視覺/激光SLAM|感知|控制規(guī)劃|模型部署|3D目標(biāo)檢測|TOF|多傳感器融合|AR|VR|編程基礎(chǔ)等。

2.3 計(jì)算下一時(shí)刻四元數(shù)

此時(shí)我們已經(jīng)獲取了下一時(shí)刻糾正后的角速度 ，這時(shí)候需要計(jì)算下一時(shí)刻的角度。
我們知道[3]四元數(shù)對時(shí)間的導(dǎo)數(shù)與角速度的關(guān)系為 ，即有

此時(shí)，再采用歐拉積分[4]，即可得到下一時(shí)刻姿態(tài)與當(dāng)前時(shí)刻姿態(tài)的關(guān)系：

從而完成了下一時(shí)刻姿態(tài)的計(jì)算。

3 核心代碼解析

我們以Matlab代碼為例，結(jié)合上述內(nèi)容進(jìn)行介紹：

functionobj=UpdateIMU(obj,Gyroscope,Accelerometer)
q=obj.Quaternion;%當(dāng)前時(shí)刻的四元數(shù)
%歸一化加速度計(jì)測量數(shù)據(jù)
if(norm(Accelerometer)==0),return;end%handleNaN
Accelerometer=Accelerometer/norm(Accelerometer);%normalisemagnitude

%計(jì)算重力在當(dāng)前四元數(shù)位姿下的分量，即上述公式(2)
v=[2*(q(2)*q(4)-q(1)*q(3))
2*(q(1)*q(2)+q(3)*q(4))
q(1)^2-q(2)^2-q(3)^2+q(4)^2];

%計(jì)算重力分量與加速度計(jì)的測量誤差，上述公式(3)
e=cross(Accelerometer,v);
if(obj.Ki>0)
obj.eInt=obj.eInt+e*obj.SamplePeriod;%計(jì)算誤差的積分，公式(4)
else
obj.eInt=[000];
end

%角速度融合，公式(5)
Gyroscope=Gyroscope+obj.Kp*e+obj.Ki*obj.eInt;

%公式(6)
qDot=0.5*quaternProd(q,[0Gyroscope(1)Gyroscope(2)Gyroscope(3)]);

%歐拉積分計(jì)算下一時(shí)刻四元數(shù)，公式(7)
q=q+qDot*obj.SamplePeriod;
obj.Quaternion=q/norm(q);%結(jié)果歸一化
end

4 完整代碼獲取

官方C++/Matlab/C#代碼：https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/
官方python代碼：https://github.com/xioTechnologies/Fusion/tree/main/Python
第三方python姿態(tài)解算庫：https://ahrs.readthedocs.io/en/latest/filters/mahony.html