Mahony算法常见的姿态融合算法-德赢Vwin官网网

1 前言

Mahony算法常见的姿态融合算法，根据加速度计、陀螺仪、以及磁力计，融合计算机体四元数，计算速度快、精度较高。本文介绍六轴融合，即根据加速度计和陀螺仪数据，计算姿态。
我们需要计算的是机体的姿态。计算角度可以通过角速度积分，也可以通过加速度正交分解，但这两种方法都存在缺陷。角速度的误差会随着积分不断增大，而加速度存在高频噪声，因此希望融合两种数据。

2 算法

2.1 重力对齐误差

首先要指出的是，Mahony算法假设加速度计测量的加速度完全由重力提供，即物体本体运动产生的加速度可忽略不计。在这一假设下，我们假设当前时刻机体的姿态为，则将重力向量的表示转到机体坐标系下，应该为：，这里表示四元数对应的旋转矩阵：

进一步地，带入，得到

我们计加速度计测量得到的加速度，如果此时没有误差，应该有，但实际两个向量并不重合，存在一定的误差。

为表示出，可以利用向量的叉乘：。因为叉乘的定义为：，当归一化为单位向量时，反应的就是角度。这里更准确的写为，下一时刻{t+1}时的误差为：

其中为根据当前{t}时刻估计的角度四元数。再记这个误差的积分量为：

误差的积分量也参与了后续计算。

2.2 角速度融合

此时已经计算出加速度计观测出的误差了，记陀螺仪提供的角速度为，则把陀螺仪角速度的误差加上上述的误差，采用控制中常用的比例-积分控制器思想，

得到纠正的角速度。

讨论：为什么用叉乘？

陀螺仪由于本身精度问题，测量的角速度存在误差，在积分过程中这个误差会一直累加，我们要做的就是去消除或是补偿这个误差，因为加速度计长期的测量值是准确的，所以可以用加速度计来进行修正。如何找到一个另一个角速度量纲的值来修正陀螺仪的角速度值呢？这里明明只有陀螺仪可以测量角速度！这时候前面提到的向量叉积得到的误差向量就帮上大忙了，这个误差向量不就是反映出了角度变化量吗。算法巧妙的将加速度相关量转化为角度相关量，因而可以用这个角度值乘一个系数来修正陀螺仪的角速度，因为在偏差角度很小的情况下，我们可以将陀螺仪角速度误差和加速度计求得的角度差看做正比的关系，也就说明陀螺仪积分误差和向量叉积存在正比关系。[2]

欢迎关注微信公众号「3D视觉工坊」，加群/文章投稿/课程主讲，请加微信：QYong2014，添加时请备注：加群/投稿/主讲申请

2.3 计算下一时刻四元数

此时我们已经获取了下一时刻纠正后的角速度，这时候需要计算下一时刻的角度。
我们知道[3]四元数对时间的导数与角速度的关系为，即有

此时，再采用欧拉积分[4]，即可得到下一时刻姿态与当前时刻姿态的关系：

从而完成了下一时刻姿态的计算。

3 核心代码解析

我们以Matlab代码为例，结合上述内容进行介绍：

functionobj=UpdateIMU(obj,Gyroscope,Accelerometer)
q=obj.Quaternion;%当前时刻的四元数
%归一化加速度计测量数据
if(norm(Accelerometer)==0),return;end%handleNaN
Accelerometer=Accelerometer/norm(Accelerometer);%normalisemagnitude

%计算重力在当前四元数位姿下的分量，即上述公式(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=[000];
end

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

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

%欧拉积分计算下一时刻四元数，公式(7)
q=q+qDot*obj.SamplePeriod;
obj.Quaternion=q/norm(q);%结果归一化
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姿态解算库：https://ahrs.readthedocs.io/en/latest/filters/mahony.html

审核编辑：彭静

声明：本文内容及配图由入驻作者撰写或者入驻合作网站授权转载。文章观点仅代表作者本人，不代表德赢Vwin官网网立场。文章及其配图仅供工程师学习之用，如有内容侵权或者其他违规问题，请联系本站处理。举报投诉

控制器

控制器

+关注

关注
112

文章
16332

浏览量
177795
数据

数据

+关注

关注
8

文章
7002

浏览量
88937
计算机

计算机

+关注

关注
19

文章
7488

浏览量
87846

原文标题：IMU姿态滤波算法——Mahony算法：原理与代码

文章出处：【微信号：3D视觉工坊，微信公众号：3D视觉工坊】欢迎添加关注！文章转载请注明出处。

导航及无人机姿态检测融合算法很难？用sensor fusion来搞定！

简单来说，姿态融合算法就是融合多种运动传感器数据（一般需要3轴加速度， 3轴陀螺仪或者3轴地磁感应传感器），通过数字滤波算法容错补偿，实现当前姿态

发表于 09-05 09:47 •2.6w次阅读

传感器数据融合算法python代码

传感器数据融合算法是一种将多个传感器收集到的数据合并到一个一致和准确的表示中的技术。这种算法的目的是提高数据精确性和可靠性，从而增强对环境或目标的理解和控制。在本文中，我们将详细讨论传感器数据融合算法

发表于 12-15 10:28 •1436次阅读

Spring Boot和飞腾派融合构建的农业物联网系统-改进自适应加权融合算法

接上集：上次实验我们已经完成多传感器融合算法的设计，本次实验注意完成改进自适应加权融合算法设计。一、背景通过利用温室控制系统来管理温室内的各种设备并控制温度、湿度，可以更好地探索和掌握环境因素

发表于 01-06 12:18

姿态融合算法是什么

作者：Joy Yang1.什么是姿态融合算法简单来说，姿态融合算法就是融合多种运动传感器数据（一般需要3轴加速度， 3轴陀螺仪或者3轴地磁感

发表于 07-19 06:47

常用的无线传感器网络数据融合算法有什么优缺点？

本文介绍了几类常用的无线传感器网络数据融合算法，并比较了其优缺点。

发表于 06-03 06:41

一种实用的数据融合算法

摘要:文章分析了目前一些数据融合算法中对先验信息要求苛刻，定义数据间支持度中门限预先设定对融合结果的不利影响，提出了一种实用的数据融合算法，该算法中定义了一种新

发表于 01-18 23:20 •16次下载

多传感器异步航迹融合算法与仿真

针对分布式多传感器数据融合系统，提出了一种多传感器异步航迹融合算法。由于不同传感器的采样时间各不相同，融合算法首先利用最小二乘法将局部航迹统一到融合中心的

发表于 08-07 09:47 •12次下载

基于COM的数据融合算法测试平台开发

基于COM的数据融合算法测试平台开发建立了一个基于COM技术的数据融合算法测试平台,把数据融合算法封装到组件中,并定义了一种COM接口规范,为算法调用提供了一

发表于 02-22 15:45 •8次下载

聚类算法及聚类融合算法研究

聚类算法及聚类融合算法研究首先对聚类算法的特点进行了分析，然后对聚类融合算法进行了挖掘。最后得出聚类融合算法比聚类

发表于 08-10 15:08 •33次下载

基于DCT的遥感图像融合算法_曹流

基于DCT的遥感图像融合算法_曹流

发表于 03-19 19:07 •1次下载

ST姿态检测融合算法技术介绍

　　姿态融合算法就是融合多种运动传感器数据（一般需要3轴加速度，3轴陀螺仪或者3轴地磁感应传感器），通过数字滤波算法容错补偿，实现当前姿态检

发表于 09-18 18:33 •11次下载

如何使用FPGA实现动态可重构的图像融合算法

一种基于FPGA动态可重构的图像融合算法。该方法对小波分解后的图像低频子带采用平均融合算子处理，在高频子带的融合中依据小波系数树状结构特点，提出了一种新的自适应融合方法，最后经过小波逆

发表于 02-02 17:12 •8次下载

何为多传感器融合算法？常用的融合算法包括哪些

因为多传感器的使用会产生大量需要处理的数据，因此通常通过融合算法来对数据进行优化。不同传感器采集到的信息可能相互之间可能会不同甚至是有矛盾，使用融合算法可以帮我们弄懂如何保证系统能够准确处理这些数据，使系统最终做出及时、正确的决策，这非常重要。

发表于 03-30 16:29 •2.2w次阅读

IMU姿态滤波算法——Mahony算法：原理与代码

首先要指出的是，Mahony算法假设加速度计测量的加速度完全由重力提供，即物体本体运动产生的加速度可忽略不计。在这一假设下，我们假设当前时刻机体的姿态为，则将重力向量的表示转到机体坐标系下，应该为：，这里表示四元数对应的旋转矩阵

发表于 05-22 09:18 •4441次阅读

Mahony滤波算法参数自动调节方法介绍

Mahony滤波算法参数自动调节方法是一种用于姿态估计的滤波算法。

发表于 12-06 09:45 •1127次阅读