1 前言

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

本文作者：小L | 來源：微信公眾號「3D視覺工坊」

討論：為什么用叉乘？

陀螺儀由于本身精度問題，測量的角速度存在誤差，在積分過程中這個誤差會一直累加，我們要做的就是去消除或是補償這個誤差，因為加速度計長期的測量值是準確的，所以可以用加速度計來進行修正。如何找到一個另一個角速度量綱的值來修正陀螺儀的角速度值呢？這里明明只有陀螺儀可以測量角速度！這時候前面提到的向量叉積得到的誤差向量就幫上大忙了，這個誤差向量不就是反映出了角度變化量嗎。算法巧妙的將加速度相關量轉(zhuǎn)化為角度相關量，因而可以用這個角度值乘一個系數(shù)來修正陀螺儀的角速度，因為在偏差角度很小的情況下，我們可以將陀螺儀角速度誤差和加速度計求得的角度差看做正比的關系，也就說明陀螺儀積分誤差和向量叉積存在正比關系。[2]

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

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

3 核心代碼解析

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

function?obj?=?UpdateIMU(obj,?Gyroscope,?Accelerometer)????q?=?obj.Quaternion;?????????????%?當前時刻的四元數(shù)????%?歸一化加速度計測量數(shù)據(jù)????if(norm(Accelerometer)?==?0),?return;?end???%?handle?NaN????Accelerometer?=?Accelerometer?/?norm(Accelerometer);?%?normalise?magnitude????%?計算重力在當前四元數(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];????%?計算重力分量與加速度計的測量誤差，上述公式(3)????e?=?cross(Accelerometer,?v);?????if(obj.Ki?>?0)????????obj.eInt?=?obj.eInt?+?e?*?obj.SamplePeriod;?????%?計算誤差的積分，公式(4)????else????????obj.eInt?=?[0?0?0];????end????????%?角速度融合，公式(5)????Gyroscope?=?Gyroscope?+?obj.Kp?*?e?+?obj.Ki?*?obj.eInt;????????????????????%?公式(6)????qDot?=?0.5?*?quaternProd(q,?[0?Gyroscope(1)?Gyroscope(2)?Gyroscope(3)]);????%?歐拉積分計算下一時刻四元數(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

參考資料：

[1] Robert Mahony, Tarek Hamel, and Jean-Michel Pflimlin. Nonlinear Complementary Filters on the Special Orthogonal Group. IEEE Transactions on Automatic Control, 2008, 53(5), pp.1203-1217.
[2] https://www.dgrt.cn/a/1551295.html
[3] https://zhuanlan.zhihu.com/p/395511536
[4] https://zhuanlan.zhihu.com/p/422074759

關注知乎「3D視覺工坊」(https://www.zhihu.com/people/qiyong1993),及時獲取每日3D視覺行業(yè)最前沿學術(shù)研究。

點擊加入「SLAM」(https://mp.weixin.qq.com/s/MKi8k8P3PQqPsgCnpryjSQ)技術(shù)交流群