我的四元数方法与我计算偏航、俯仰和滚转的简单方法不符

问题描述

目标：使用角速度通过四元数计算 YAW、PITCH 和 ROLL 值。

问题：我的四元数方法与我计算YAW、PITCH和ROLL的简单方法不匹配。

#define M_PI 3.14159265358979323846

//Angular Velocties in radians
float angvx=(M_PI/2);
float angvy=0;
float angvz=0;        
// change in time
float dt=1;

int main()
{

    //"Simple" METHOD: yaw,pitch roll calc
    pitch=dt*angvx;
    roll=dt*angvy;
    yaw=dt*angvz;
    
    //Quaternion Method
    //ang veLocity vector of all 3 
    omega = sqrt(pow(angvx,2)+pow(angvy,2)+pow(angvz,2)); 
    //length of angular veLocity vector
    theta = omega*dt;
    
    printf("omega: %f\n",omega);
    printf("theta: %f\n",theta);
    
    v_x = angvx / omega;
    //normalized orientation of angular veLocity vector
    v_y = angvy / omega;  
    v_z = angvz / omega;
    
    printf("v_x:%f v_y:%f v_z:%f\n",v_x,v_y,v_z);
    
    
    qw=cos(theta/2);
    qx= v_x * sin(theta/2);
    qy= v_y * sin(theta/2);
    qz= v_z * sin(theta/2);
    
    printf("qw:%f qx:%f qy:%f qz:%f\n",qw,qx,qy,qz);
    
    //formula from 
    //https://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/
    qyaw = atan2(2*qy*qw-2*qx*qz,1 - 2*pow(qy,2) - 2*pow(qz,2));
    qpitch = asin(2*qx*qy + 2*qz*qw);
    qroll = atan2(2*qx*qw-2*qy*qz,1 - 2*pow(qx,2));
    
    
    printf("qyaw:%f qpitch:%f qroll:%f\n",qyaw,qpitch,qroll);
    printf("yaw:%f pitch:%f roll:%f\n",yaw,pitch,roll);
    
    return 0;
}

当 angvx = M_PI (pi) 示例 1 简单的方法产生pitch = 3.141593。四元数收益率 qroll:-3.141593

当 angvz=M_PI (pi) 示例 2 简单的方法产生yaw= 3.141593。四元数收益率 **qroll:-3.141593 qyaw:3.141593 **

我知道在 qroll 计算中 qx 是示例 1 中唯一的非零项。 我知道在 qroll & qyaw 计算中 qz 是 Example2 中唯一的非零项。

但是这些公式似乎得到了社区的支持，所以我不明白我做错了什么？

公式 qroll、qyaw、qpitch 来源：

https://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/

Extracting Yaw from a Quaternion

进一步问题

当我同时指定 angvx,angvz,angvy 时，俯仰、偏航和滚转与简单方法有很大不同。

解决方法

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

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

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