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.参考文献

  1. 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.

Madgwick算法详细解读的更多相关文章

  1. 和textrank4ZH代码一模一样的算法详细解读

    前不久做了有关自动文摘的学习,采用方法是TextRank算法,整理和大家分享. 一. 关于自动文摘 利用计算机将大量的文本进行处理,产生简洁.精炼内容的过程就是文本摘要,人们可通过阅读摘要来把握文本主 ...

  2. A*寻路算法详细解读

    文章目录 A*算法描述 简化搜索区域 概述算法步骤 进一步解释 具体寻路过程 模拟需要更新F值的情况 Lua代码实现 在学习A*算法之前,很好奇的是A*为什么叫做A*.在知乎上找到一个回答,大致意思是 ...

  3. 相机IMU融合四部曲(一):D-LG-EKF详细解读

    相机IMU融合四部曲(一):D-LG-EKF详细解读 极品巧克力 前言 前两篇文章<Google Cardbord的九轴融合算法>,<Madgwick算法详细解读>,讨论的都是 ...

  4. MemCache超详细解读

    MemCache是什么 MemCache是一个自由.源码开放.高性能.分布式的分布式内存对象缓存系统,用于动态Web应用以减轻数据库的负载.它通过在内存中缓存数据和对象来减少读取数据库的次数,从而提高 ...

  5. MemCache超详细解读 图

    http://www.cnblogs.com/xrq730/p/4948707.html   MemCache是什么 MemCache是一个自由.源码开放.高性能.分布式的分布式内存对象缓存系统,用于 ...

  6. MemCache详细解读

    MemCache是什么 MemCache是一个自由.源码开放.高性能.分布式的分布式内存对象缓存系统,用于动态Web应用以减轻数据库的负载.它通过在内存中缓存数据和对象来减少读取数据库的次数,从而提高 ...

  7. 为你详细解读HTTP请求头的具体含意

    当我们打开一个网页时,浏览器要向网站服务器发送一个HTTP请求头,然后网站服务器根据HTTP请求头的内容生成当次请求的内容发送给浏览器.你明白HTTP请求头的具体含意吗?下面一条条的为你详细解读,先看 ...

  8. VINS-mono详细解读

    VINS-mono详细解读 极品巧克力 前言 Vins-mono是香港科技大学开源的一个VIO算法,https://github.com/HKUST-Aerial-Robotics/VINS-Mono ...

  9. SVO详细解读

    SVO详细解读 极品巧克力 前言 接上一篇文章<深度滤波器详细解读>. SVO(Semi-Direct Monocular Visual Odometry)是苏黎世大学Scaramuzza ...

随机推荐

  1. JavaFX 之窗口拖动(三)

    一.问题场景 在上一篇中,我们将窗口的默认标题栏隐藏从而导致鼠标点击窗体无法进行拖动. 二.解决思路 给组件添加鼠标按下事件监听器和鼠标拖动事件监听器. 三.代码实现 /** * 程序入口 * @au ...

  2. 每30秒运行一下shell脚本

    cd /usr/local/sbin/     //存放shell脚本 目录. vim guoguosql.sh      //每30秒运行一个php文件.   文件路径为 vim /home/www ...

  3. PES包格式

    PES是Packetized Elementary Stream的简称,是将原始ES流打包后形成的,再将PES经过不同的打包方式可以组成MPEG program stream 和 MPEG trans ...

  4. GOF23设计模式之桥接模式(bridge)

    一.桥接模式概述 桥接模式核心要点: 处理多层继承结构,处理多维度变化的场景,将各个维度设计成独立的继承结构,使各个维度可以独立的扩展在抽象层建立关联. 二.桥接模式场景提出与存在问题 商城系统中常见 ...

  5. 在Windows下搭建基于nginx的视频直播和点播系统

    http://my.oschina.net/gaga/blog/478480 一.软件准备 由于nginx原生是为linux服务的,因此官方并没有编译好的windows版本可以下载,要在windows ...

  6. 怎么使用ping命令进行连通性测试

    关于ping命令的作用: ping 命令有助于验证网络层的连通性!一般进行网络故障排除时,可以使用ping 命令向目标计算机或IP地址发送ICMP回显请求,目标计算机会返回回显应答,如果目标计算机不能 ...

  7. Repeater,ItemDataBound事件,获取绑定列的值,给指定列添加js方法

    protected void rp_bf_ItemDataBound(object sender, RepeaterItemEventArgs e) { if (e.Item.ItemType == ...

  8. HTTP客户端之使用request方法向其他网站请求数据

    在node中,可以很轻松的向任何网站发送请求并读取该网站的响应数据. var req=http.request(options,callback); options是一个字符串或者是对象.如果是字符串 ...

  9. GIL线程与进程选择

    #!/usr/bin/env python # encoding: utf-8  # Date: 2018/6/18 # 计算密集型.应该启动多进程# from multiprocessing imp ...

  10. 03——微信小程序官方demo讲解——page部分

    一个page由一个文件夹以及文件夹下四个文件组成. 比如一个页面叫index.则需要在pages目录下新建一个index目录,且包含由index+类型(js\wxml\wxss\json)为名组成的若 ...