大家好,我是学电子的小白白~
上一篇我们讲了陀螺仪与加速度计的数据融合,本篇文章是互补滤波的续篇,接着讲陀螺仪与磁力计的数据融合。
1)为什么需要磁力计的数据
先回忆一下上一篇内容,加速度计是如何修正陀螺仪的呢?
我们是假定了在大部分时间里,载体是静止或者匀速直线运动的,所以可以认为加速度计的测量值,就是重力加速度。但是,在载体水平时,重力加速度的方向与Z轴相同,是无法反映出偏航角yaw的(因为偏航角是绕Z轴旋转,即使偏航角变化,Z轴方向仍然不变,也就是说加速度计无法感知Z轴的旋转)。
而磁力计可以测出地球的磁场,能够得到水平的北向方位,所以引入磁力计的数据,可以用来修正陀螺仪的偏航角。
2)陀螺仪与磁力计融合方法
我们仍然先回忆一下上一篇的内容,陀螺仪与加速度计是怎样融合的,会有助于本篇方法的理解。
陀螺仪与加速度计融合时,我们先是通过当前时刻的姿态转移矩阵,算出了一个重力加速度的方向vx、vy、vz(这个方向包含了陀螺仪的姿态信息);再将这个方向与加速度计测量到的重力加速度方向ax、ay、az对比,求出误差ex、ey、ez,再用误差去修正下一时刻的陀螺仪角速度gx、gy、gz。
本篇中,陀螺仪与磁力计融合时,我们也用类似的方法,先通过姿态转移矩阵的变换,利用当前的姿态信息求出一个磁场的方向;再用它与磁力计测到的磁场方向比对,求出误差,最后用这个误差去修正下一时刻的角速度。
但是,需要注意的是,利用加速度计姿态融合时,由于重力加速的方向只有z向有,x、y向都为0,所以我们可以认为它(方向)是已知的;而地球的磁场并不是完全的水平指向北极,在竖直方向上也有一定的大小,所以它不是个已知量;因此,具体实现的时候,磁力计数据的融合与加速度计有点不同,主要在利用姿态信息求磁场方向的这一步。
假定我们需要修正的姿态,是已经被加速度计修正过的,那么此时的姿态与真实的载体姿态就只存在偏航角yaw的偏移。
修正的主要步骤如下:
a)磁力计测到的地球磁场数值为mx、my、mz,是在未修正偏航角的载体坐标系下的,可以通过当前的姿态转移矩阵转换到地理坐标系下(n系),转换后的磁力计数据为hx、hy、hz,这组磁力计的数据,与真实的磁场数据,只存在偏航角的偏移:
b)在n系中,假定真实的磁场数据为bx、by、bz,由于z向的磁场数据与偏航角不相关,所以有bz=hz;如果n系的x轴指北,则by=0,bx=√(hx2+hy2),这就是真实的磁场数据。(如果n系y轴指北,则bx=0,by=√(hx2+hy2))。
c)我们得到了真实的磁场数据后(就想当于加速度计融合时,已知的重力加速度),再通过未修正偏航角的姿态转移矩阵变回到载体系,得到一组载体系下的磁场数据wx、wy、wz,这组数据中就包含了偏航角的偏差信息:
d)然后就可以用wx、wy、wz与磁力计测到的载体系下真实的mx、my、mz叉乘(求外积),求出偏差。
e)最后,使用这组偏差去修正陀螺仪的角速度,仍然使用与加速度计数据融合相同的方法,PI后修正。
具体的实现框图如下:
融合磁力计数据的主要的代码实现如下:
(这段代码里也包括了加速度计的数据融合,计算误差时,将加速计的误差和磁力计的误差相加后,再求PI去修正陀螺仪的角速度。完整工程文件可以在文末关注公众号找到下载地址)
3)测试验证
使用时,只要周期性地获取陀螺仪、加速度计、磁力计的数据,调用互补滤波函数,更新四元数,计算姿态角即可(与上一篇加速度计数据融合用法一样)。
测试一下互补滤波的效果,使用的硬件平台是MPU6050和HMC5883L,处理器是stm32f103c8t6。
初始化时,需要平放几秒钟让加速度计和陀螺仪自行校准。平放时俯仰角和滚转角都接近于0,偏航角依据与地磁北向的夹角有一定初始值。经过较长时间后,俯仰角、滚转角、偏航角都可以保持正确,不会产生误差累积和漂移;姿态变化后,在回到初始位置,也基本不会有偏差,说明陀螺仪的数据得到了修正;磁力计对偏航角的修正是有效的:
好了,关于互补滤波姿态融合的知识,就讲到这里了。由于篇幅所限,文中的代码只是截取了核心片段进行讲解,完整的工程代码可以免费提供下载交流。
如果觉得有用可以关注作者微 信公.众号“小白白学电子”,有更多内容分享,在公众号可以找到所有代码和资料下载地址。
如果觉得《陀螺仪与磁力计的姿态融合——互补滤波(续)》对你有帮助,请点赞、收藏,并留下你的观点哦!