如何解决我的四元数方法与我计算偏航、俯仰和滚转的简单方法不符
目标:使用角速度通过四元数计算 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 举报,一经查实,本站将立刻删除。