经典的滤波算法:Madgwick滤波算法

描述

前言

之前的一次推送介绍了Mahony姿态解算算法(IMU姿态滤波算法——Mahony算法:原理与代码),这次介绍另一个经典的滤波算法:Madgwick滤波。

Madgwick滤波算法根据加速度计、陀螺仪、以及磁力计,融合计算机体四元数,计算速度快、精度较高。本文详细介绍六轴融合,即根据加速度计和陀螺仪数据,计算IMU的姿态。

 

磁力计

算法

2.1 重力方向对齐优化

首先要指出的是,Madgwick算法假设加速度计测量的加速度完全由重力提供,即物体本体运动产生的加速度可忽略不计。这一点和Mahony算法是一样的。

假设world系中一个向量在world下的表示为 ,在传感器下的观测为 ,传感器位姿以四元数表示记为 。如果此时的姿态是准确的,应该有:

然而由于姿态不准,等式并不成立。因此,我们定义目标函数:

令目标函数取到0(在四元数对应的参数空间上),则得到了最优的姿态,即。这可以当作一个优化问题进行求解。而优化问题的求解思路之一,就是采用梯度下降法。记的梯度为,那么在迭代优化求解时,下一次取值应该为当前取值减去当前梯度方向走一个步长,即

其中,

其中, 为 的雅可比(具体的形式可参考文献[1][2])。

可以将步长 视作加速度计产生的数据对姿态的收敛速度,同时考虑到角速度在当前时刻会产生一个角速度的增量,因此加速度这部分的收敛必须大于角速度变化速率,即 ,其中 。

对于一般的优化,我们需要不断迭代求解直到收敛,但Madgwick为了保证算法的实时性,只进行一次梯度下降,也能取得差不多的精度。

具体地,如果我们把world系下的这个向量 取重力 ,姿态传感器对应的测量 即为IMU的加速度计读数 ,此时有:

2.2 角速度融合

另一方面,我们可以通过角速度计提供的角速度积分,得到姿态,即:

这里我们对四元数进一步的区分。我们对上一时刻滤波后的姿态(四元数)记为 ,当前时刻角速度计的读数即角速度为 ,角速度计计算得到的姿态为 。

对2.1中优化后得到的四元数记为 ,即加速度计计算得到的姿态。

那么,当前时刻估计的姿态取做两个姿态的加权平均:

至于 如何取值,下一小节进行讨论。

2.3 权重取值

由于角速度存在漂移,我们定义角速度的发散速率为 。这里如果加速度计对姿态贡献的收敛速度,等于角速度的发散速度,则由式(5)融合的结果依旧是准确的姿态。即需要有:

由于 中对 没有限制,因此取 为无限大,此时,综合(1)(5)(6),有:

定义估计的角速度 ,则最终有:

再看式(7),可以将视作,下一时刻的姿态,等于上一时刻姿态,加上角速度计的积分,减去一个与角速度计噪声水平 相关的增量,这个增量的与加速度计的优化时的梯度有关。最终,Madgwick滤波算法的参数只有一个。

代码

Matlab完整代码[3]如下:

function obj = UpdateIMU(obj, Gyroscope, Accelerometer)

    q = obj.Quaternion;

    if(norm(Accelerometer) == 0), return; end
    Accelerometer = Accelerometer / norm(Accelerometer); % 归一化加速度计数据

    % 式(2)和(3)
    F = [2*(q(2)*q(4) - q(1)*q(3)) - Accelerometer(1)
        2*(q(1)*q(2) + q(3)*q(4)) - Accelerometer(2)
        2*(0.5 - q(2)^2 - q(3)^2) - Accelerometer(3)];
    J = [-2*q(3), 2*q(4),    -2*q(1), 2*q(2)
        2*q(2),     2*q(1),     2*q(4), 2*q(3)
        0,         -4*q(2),    -4*q(3), 0    ];
    step = (J'*F);  
    step = step / norm(step); % 式(1)中的减号后面的部分,即修正量

    % 式(8)中的修正角速度
    qDot = 0.5 * quaternProd(q, [0 Gyroscope(1) Gyroscope(2) Gyroscope(3)]) - obj.Beta * step';     

    % 式(7)(8),即迭代到下一步。
    q = q + qDot * obj.SamplePeriod;
    obj.Quaternion = q / norm(q); % normalise quaternion
end

与Mahony算法的比较

Madgwick算法与Mahony算法相比,最大的不同之处是如何对待加速度计估计的误差。Mahony是利用叉乘,Madgwick是利用优化;

Mahony可以视作一个PI(比例-积分)控制器,Madgwick是一个P(比例)控制器;

Madgwick比Mahony的精度稍高一丢丢,但Mahony的计算速度略快[4];

Mahony与Madgwick都需要假设加速度测的只是重力,因此在加速度变化剧烈情况下表现不佳。

编辑:黄飞

 

打开APP阅读更多精彩内容
声明:本文内容及配图由入驻作者撰写或者入驻合作网站授权转载。文章观点仅代表作者本人,不代表电子发烧友网立场。文章及其配图仅供工程师学习之用,如有内容侵权或者其他违规问题,请联系本站处理。 举报投诉

全部0条评论

快来发表一下你的评论吧 !

×
20
完善资料,
赚取积分