关于磁力计一阶互补修正偏航角的一点体会

什么又被压力

于 2024-03-19 00:02:46 发布

阅读量2.4k

点赞数 31

文章标签：算法

本文链接：https://blog.csdn.net/m0_64257804/article/details/136824263

版权

近期笔者想使用九轴陀螺仪做姿态解算，在使用磁力计修正偏航角时，踩了很多“坑”，有些困惑到现在还很迷糊，想写篇文章记录一下，由于笔者水平及其有限，如果文章内容有误，还请大家谅解，欢迎在评论区更正、探讨。

笔者这里的欧拉角求解使用的是Mahony+Madgwick，也就是用理想加速度和实际加速度做差修正角速度，梯度算子修正四元数，通过一阶龙格库塔求解四元数微分方程，最后通过旋转矩阵得到欧拉角；也就是说想用磁力计修正偏航角Yaw，需要让磁力计和陀螺仪在同一坐标系下才可以。最好安装磁力计时坐标系和陀螺仪坐标系“重合”，如此写代码时就不会出现莫名其妙的负号、把俯仰角和横滚角互换······等等情况。笔者使用的九轴陀螺仪（imu963ra）二者坐标系是重合的，（相同坐标系下，平放imu963ra，此时az是正最大值，mz是负最大值）。下面所有的公式、代码都是基于坐标系是重合的情况。

下面直接从代码的角度去实现（水平有限，原理笔者也不太懂），先放一张从别的文章piao的图：

从图中可以看到，只要我们有加速度和三轴磁力计数据就可以得到磁力计算出的偏航角，由于通过加速度计算的夹角受运动加速度影响较大，笔者选择使用欧拉角替代，下面贴出代码：

float magYaw =0;        //磁力计计算的偏航角
float gyroINS_Yaw =0;    //世界系下的Z轴角速度
void YawMag_Observe(float mx, float my, float mz)   //根据磁力计观测Yaw轴角度
{
    Vector_3f bMag ;
    float gama = IMU.Roll*DEG_TO_RAD;//atan(IMU.Acc_AHRS.y/IMU.Acc_AHRS.z);
    float theta = IMU.Pitch*DEG_TO_RAD;//asin(-IMU.Acc_AHRS.x/sqrt(IMU.Acc_AHRS.y*IMU.Acc_AHRS.y+IMU.Acc_AHRS.z*IMU.Acc_AHRS.z));
    //这里文章中公式的意思就是用加速度直接求欧拉角，此处的公式是一样的效果
    bMag.y = my*cos(gama) - mz*sin(gama);
    bMag.x = mx*cos(theta) + my*sin(theta)*sin(gama) + mz*sin(theta)*cos(gama);
    magYaw = -atan2(bMag.y,bMag.x)*RAD_TO_DEG;//这里 负号 是为了和后面 一阶互补融合时角速度的方向一致
    if(magYaw < 0) magYaw += 360;

    gyroINS_Yaw = -sin(IMU.Pitch*DEG_TO_RAD)*IMU.Gyro_Uint.x + sin(IMU.Roll*DEG_TO_RAD)*cos(IMU.Pitch*DEG_TO_RAD)*IMU.Gyro_Uint.y
            + cos(IMU.Roll*DEG_TO_RAD)*cos(IMU.Pitch*DEG_TO_RAD)*IMU.Gyro_Uint.z;

}

这里就是笔者遇到的一个坑，网上很多文章给出的公式是角度用的是θ φ Φ 或者pitch roll yaw，但是欧拉角使用的旋转方式有多种，常用的有Z-Y-X和Z-X-Y，而文章中的加速度用的坐标系是右手东-北-天坐标系，如果欧拉角使用的是Z-Y-X的旋转方式对应的右手坐标系就变成了北-东-天，即pitch是我们实际用的roll， roll是实际我们用的pitch。

如果你欧拉角计算使用的是这个公式，那需要把图片中的pitch角和roll角对换后（从上到下为roll-pitch-yaw），就可以用计算的欧拉角代替文章中的gama 和 theta（即代码中所写）。如此我们就可以得到由磁力计计算的偏航角观测值。

笔者在这里想补充一下，可能观点有误，逛了很多篇文章，个人认为，通常情况下，roll表示绕着x轴旋转（常用φ表示），pitch表示绕着y轴旋转（常用θ表示），yaw表示绕着z轴旋转（常用Φ表示）。也就是说如果你认为右手围绕着大拇指的方向转动是俯仰角（pitch），那此时大拇指所指方向为y轴正向(并非x轴)，目前还不知道此结论是否适用于惯性导航中，但是根据世界坐标系下Z轴角速度的波形来看，应该没问题······如果笔者此处理解有误欢迎大家批评指出，相互交流!

废话不多说，得到了Yaw轴观测值后，可以和Z轴角速度积分制一阶互补滤波，即可得到稳定的偏航角（Yaw)。下面贴出代码：

    //计算欧拉角
    IMU.Pitch= atan2(2.0f * q[2] * q[3] + 2.0f * q[0] * q[1], -2.0f * q[1] * q[1]- 2.0f * q[2]* q[2] + 1.0f) * RAD_TO_DEG;
    IMU.Roll= asin(2.0f * q[0]* q[2]-2.0f * q[1] * q[3]) * RAD_TO_DEG;
#if QUAT_MAGNETIC_MODE      //有磁力计时一阶互补
    IMU.Yaw += gyroINS_Yaw*D_T;
    IMU.Yaw = IMU.Yaw* 0.975f + magYaw*0.025;
#else                       //无磁力计直接计算
    IMU.Yaw = atan2(2.0f * q[1] * q[2] + 2.0f * q[0] * q[3] , -2.0f * q[3] * q[3] - 2.0f* q[2] * q[2] +1.0f) * RAD_TO_DEG;
#endif

此处的角速度必须为世界坐标系下的Z轴角速度，通过方向余弦矩阵计算得出，上文函数中已经计算过了。

有一个问题希望读者可以思考一下，问什么角速度有漂移，而此处直接用角速度直接乘以一个权重直接积分累加了之后，最终的偏航角就不飘了呢？

——答案是乘以这个权重（0.975）再乘以Δt后，角速度积分的角度就不漂了，再加上一个不漂移的值（ magYaw*0.025），最后的结果当然是不漂移的啦。其实也很容易理解，陀螺仪的零漂本就是百分之几，比如2000°/s，零漂大概±30，也就1.5%，这里参数直接省略了后面2.5%的“漂移的部分”。

以上就是笔者的一点心得体会，希望能对读者和以后的自己有所帮助……