继之前研究了一些飞行姿态理论方面的问题后,又找到了之前很流行的一段外国大神写的代码,来分析分析。 第二篇文章的最后,讲到了文章中的算法在实际使用中有重大缺陷。
大家都知道,分析算法理论的时候很多情况下我们没有考虑太多外界干扰的情况,原因是很多情况下,传感器的精度以及受到的干扰并不会特别大,而显著的影响到算法。但是在IMU系统中,有点不同。由于地磁场十分微弱,而我们生活中有大量使用电子设备,使得磁场非常的混乱,以至于地磁传感器非常容易受到干扰。
由于以上算法把地磁传感器一同加入到姿态的测定中,并基本给予了地磁传感器与加速度传感器同样的加权,导致地磁传感器一旦被干扰,会对姿态产生地球重力突然被干扰一样的结果。。。对姿态的测量是毁灭性的。
综上,考虑到磁场的不稳定性,必须对地磁传感器进行降权处理,使得他对姿态的影响变小。
于是设计了以下的算法。
将磁场传感器的数据在姿态角度中剔除,更新姿态的俯仰角(PITCH)以及横滚角(ROLL)的时候只使用加速度传感器以及陀螺仪(角速度传感器)。
只在计算偏航角的(YAW)的时候使用磁场传感器。也就是只使用磁场传感器作为一个电子指南针,定位整个姿态在水平面旋转的角度。这样设计,让磁场传感器只影响姿态中的一个数值,减少了磁场的权重,即使磁场收到干扰,也不会导致姿态骤变,使得四轴坠机。
在对YAW进行计算的时候使用了如下函数。
eulerAngleRaw.yaw = 0.9 * (eulerAngleRaw.yaw - gzF*2*halfT) + 0.1 * angleMagYaw;
此处的0.9和0.1是可以变动的但他们相加应该为1,此处为一个最简单的1阶低通滤波器,增加0.1则是增大截止频率。
算法的流程图是这样的:
新的姿态更新算法是这样的
voidAHRSupdate(float gxf, float gyf, float gzf, float axf, float ayf, float azf, float mxf, float myf, float mzf)
{