为什么四元数产生错误的角度？

问题描述

我在这里为AHRS / MARG / IMU写了一些代码。 目的是将加速度计（重力）和磁力计（地球磁场）矢量结合起来以产生四元数，该四元数表示身体框架如何从全局框架旋转，我正在关注本文https://www.ncbi.nlm.nih.gov/pmc/articles/PMC4570372/。这是通过将Qacc乘以Qmag（等式36），将Qacc乘以俯仰和滚动（等式25）以及将Qmag乘以偏航（等式35）来实现的。问题是我分别获得了Qacc（+-80以上的俯仰角除外）和Qmag的正确角度，但是当我将Qacc乘以Qmag时，俯仰角，横滚角和偏航角都变得不正确了。在我看来，这一定是因为我的四元数乘法，但是我仔细检查了代码，乘法似乎是正确的... 这是我的短代码：

void UpdateAccMagQuaternion(){

    float ax = 0,ay = 0,az = 0,mx = 0,my = 0,mz = 0;
    float recipNorm,cosAngle,sinAngle;
    float XZprojection = 0;
    float sensorBuffer[3] = {0};
    float axis[3] = {0,0};


    LSM303DLHC_AccReadXYZ(sensorBuffer);                                                                                                                            //Acceleration vector
    ax = sensorBuffer[0];
    ay = sensorBuffer[1];
    az = sensorBuffer[2];
    
    //Normalize gravity acceleration vector
    recipNorm = invSqrt(ax * ax + ay * ay + az * az);                                                                                                   
        ax *= recipNorm;
        ay *= recipNorm;
        az *= recipNorm;
    
    //Qacc
    if(az < 0) {
        Qacc.w = -ay/sqrt(2*(1-az));
        Qacc.x = sqrt((1-az)/2);
        Qacc.y = 0;
        Qacc.z = ax/(sqrt(2*(1-az)));
    }
    else {
        Qacc.w = sqrt((1+az)/2);
        Qacc.x = -ay/sqrt(2*(1+az));
        Qacc.y = ax/(sqrt(2*(1+az)));
        Qacc.z = 0;
    }


    LSM303DLHC_MagReadXYZ(sensorBuffer);
    mx = (sensorBuffer[0]);                                                                                                         //Magnetometer offset and scale calibration
    my = (sensorBuffer[1]);
    mz = (sensorBuffer[2]);
    
    // Normalize earth magnetic field vector
    recipNorm = invSqrt(mx * mx + my * my + mz * mz);                                                                                                   
    mx *= recipNorm;
    my *= recipNorm;
    mz *= recipNorm;
    
    //Qmag
    XZprojection = mx*mx + my*my;
    if (mx < 0){
        Qmag.w = my/(sqrt(2)*sqrt(XZprojection-mx*sqrt(XZprojection)));
        Qmag.x = 0;
        Qmag.y = 0;
        Qmag.z = sqrt(XZprojection-mx*sqrt(XZprojection))/sqrt(2*XZprojection);
    }
    else {
        Qmag.w = sqrt(XZprojection+mx*sqrt(XZprojection))/sqrt(2*XZprojection);
        Qmag.x = 0;
        Qmag.y = 0;
        Qmag.z = my/(sqrt(2)*sqrt(XZprojection+mx*sqrt(XZprojection)));
    }


    //Total Q = Qacc X Qmag
    Q.w = (Qacc.w*Qmag.w - Qacc.x*Qmag.x - Qacc.y*Qmag.y - Qacc.z*Qmag.z);
    Q.x = (Qacc.w*Qmag.x + Qacc.x*Qmag.w + Qacc.y*Qmag.z - Qacc.z*Qmag.y);
    Q.y = (Qacc.w*Qmag.y - Qacc.x*Qmag.z + Qacc.y*Qmag.w + Qacc.z*Qmag.x);
    Q.z = (Qacc.w*Qmag.z + Qacc.x*Qmag.y - Qacc.y*Qmag.x + Qacc.z*Qmag.w);

}

这是我的将四元数转换为欧拉角的代码：

roll  = atan2(2.0 * (Q.z * Q.y + Q.w * Q.x),1.0 - 2.0 * (Q.x * Q.x + Q.y * Q.y))*57.2958;
pitch = asin(2.0 * (Q.y * Q.w - Q.z * Q.x))*57.2958;
yaw = atan2(2.0 * (Q.z * Q.w + Q.x * Q.y),- 1.0 + 2.0 * (Q.w * Q.w + Q.x * Q.x))*57.2958;

解决方法

暂无找到可以解决该程序问题的有效方法，小编努力寻找整理中！

如果你已经找到好的解决方法，欢迎将解决方案带上本链接一起发送给小编。

小编邮箱:dio#foxmail.com (将#修改为@）