您的位置:首页 > 其它

crazyflie2.0中欧拉角和四元数之间转换的问题

2015-12-27 15:34 375 查看
crazyflie2.0中从四元数获取欧拉角的程序中,看到如下程序:

void sensfusion6GetEulerRPY(float* roll, float* pitch, float* yaw)
{
float gx, gy, gz; // estimated gravity direction

gx = 2 * (q1*q3 - q0*q2);
gy = 2 * (q0*q1 + q2*q3);
gz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

if (gx>1) gx=1;
if (gx<-1) gx=-1;

*yaw = atan2f(2*(q0*q3 + q1*q2), q0*q0 + q1*q1 - q2*q2 - q3*q3) * 180 / M_PI_F;
*pitch = asinf(gx) * 180 / M_PI_F; //Pitch seems to be inverted
*roll = atan2f(gy, gz) * 180 / M_PI_F;
}

但是在看邓正隆的惯性技术这本书中,计算出来的应该是如下公式,其中Z轴Yaw偏航,X轴Pitch俯仰,Y轴Roll翻滚;使用如下公式计算之后,从crazyflie PC client上看到X轴和Y轴交换了,很奇怪,不知道为什么?先记录下结果

void sensfusion6GetEulerRPYNormal(float* roll, float* pitch, float* yaw)
{
*yaw = -atan2f(2*(-q0*q3 + q1*q2), q0*q0 + q1*q1 - q2*q2 - q3*q3) * 180 / M_PI_F;
*pitch = asinf(2*q2*q3 + 2*q0*q1) * 180 / M_PI_F; //Pitch seems to be inverted
*roll = -atan2f((2*q1*q3-2*q0*q2), q0*q0 + q3*q3 - q1*q1 - q2*q2) * 180 / M_PI_F;
}
内容来自用户分享和网络整理,不保证内容的准确性,如有侵权内容,可联系管理员处理 点击这里给我发消息
标签: