这里的四元数算法来自这里,我没有直接使用这个算法而是自己写了个程序验证其正确性.下面是代码:
(博客园 : 梦工厂2012)
// Quaternion_test.cpp : Defines the entry point for the console application. // /*所有方法体者经过了验证*/ #include "stdafx.h" #include "math.h" #define pi 3.1415926 /*四元数的元素，代表估计方向 */ float q0 = 1, q1 = 0, q2 = 0, q3 = 0; float q0_inc,q1_inc,q2_inc,q3_inc; /*用于对四元进行更新,角增量,不是真实的欧拉角*/ float Roll_inc,Pitch_inc,Yaw_inc; float Roll,Pitch,Yaw; /*真实欧拉角*/ void FromEulerAngle(float Roll_add,float Pitch_add,float Yaw_add) ; void ToEulerAngle(); void Quaternion_nor(); void Multiply(float q0_n,float q1_n,float q2_n,float q3_n); int main(int argc, char* argv[]) { Roll_inc=00.0/180*pi; Pitch_inc=0.0/180*pi; Yaw_inc=10.0/180*pi; printf("第一次旋转\n"); printf("yaw_inc=%f\n",Yaw_inc*57.3); printf("pitch_inc=%f\n",Pitch_inc*57.3); printf("roll_inc=%f\n",Roll_inc*57.3); FromEulerAngle(Roll_inc,Pitch_inc,Yaw_inc) ; /*更新四元 */ Multiply(q0_inc,q1_inc,q2_inc,q3_inc); printf("q0=%f\n",q0); printf("q1=%f\n",q1); printf("q2=%f\n",q2); printf("q3=%f\n",q3); ToEulerAngle(); printf("yaw=%f\n",Yaw*57.3); printf("pitch=%f\n",Pitch*57.3); printf("roll=%f\n",Roll*57.3); printf("第二次旋转\n"); Roll_inc=0.0/180*pi; Pitch_inc=10.0/180*pi; Yaw_inc=0.0/180*pi; /*角度增量*/ printf("yaw_inc=%f\n",Yaw_inc*57.3); printf("pitch_inc=%f\n",Pitch_inc*57.3); printf("roll_inc=%f\n",Roll_inc*57.3); /*欧拉角转四元数, 增量四元数*/ FromEulerAngle(Roll_inc,Pitch_inc,Yaw_inc) ; /*更新四元数*/ Multiply(q0_inc,q1_inc,q2_inc,q3_inc); printf("第二次旋转的四元数\n"); printf("q0=%f\n",q0); printf("q1=%f\n",q1); printf("q2=%f\n",q2); printf("q3=%f\n",q3); /*转成载体欧拉角*/ ToEulerAngle(); printf("yaw=%f\n",Yaw*57.3); printf("pitch=%f\n",Pitch*57.3); printf("roll=%f\n",Roll*57.3); /*第三次旋转*/ printf("第三次旋转\n"); Roll_inc=10.0/180*pi; Pitch_inc=0.0/180*pi; Yaw_inc=0.0/180*pi; /*角度增量*/ printf("yaw_inc=%f\n",Yaw_inc*57.3); printf("pitch_inc=%f\n",Pitch_inc*57.3); printf("roll_inc=%f\n",Roll_inc*57.3); /*欧拉角转四元数, 增量四元数*/ FromEulerAngle(Roll_inc,Pitch_inc,Yaw_inc) ; /*更新四元数*/ Multiply(q0_inc,q1_inc,q2_inc,q3_inc); printf("第三次旋转的四元数\n"); printf("q0=%f\n",q0); printf("q1=%f\n",q1); printf("q2=%f\n",q2); printf("q3=%f\n",q3); /*转成载体欧拉角*/ ToEulerAngle(); printf("yaw=%f\n",Yaw*57.3); printf("pitch=%f\n",Pitch*57.3); printf("roll=%f\n",Roll*57.3); return 0; } /*欧拉角转四元,其它坐标系 */ /*这里是否采用小角近似???*/ void FromEulerAngle(float Roll_add,float Pitch_add,float Yaw_add) /*这里只是机体转角近似成欧拉角 */ { /*在俊的姿态解算中,用的是小角近似 q=[1,Ω*t/2,Ω*t/2,Ω*t/2]T*/ float fCosHRoll = (float)cos(Roll_add * .5f); float fSinHRoll = (float)sin(Roll_add * .5f); float fCosHPitch = (float)cos(Pitch_add * .5f); float fSinHPitch = (float)sin(Pitch_add * .5f); float fCosHYaw = (float)cos(Yaw_add * .5f); float fSinHYaw = (float)sin(Yaw_add * .5f); /*回来看看这三角函数运算用了多长时间*/ /*下面这个运算要根据坐标第进行修改*/ q0_inc = fCosHRoll * fCosHPitch * fCosHYaw + fSinHRoll * fSinHPitch * fSinHYaw; q1_inc = fSinHRoll * fCosHPitch * fCosHYaw - fCosHRoll * fSinHPitch * fSinHYaw; q2_inc = fCosHRoll * fSinHPitch * fCosHYaw + fSinHRoll * fCosHPitch * fSinHYaw; q3_inc = fCosHRoll * fCosHPitch * fSinHYaw - fSinHRoll * fSinHPitch * fCosHYaw; } /*四元数转欧拉角*/ void ToEulerAngle() { Roll=atan2(2 * (q0 * q1 + q2 * q3) , 1 - 2 * (q1 * q1 + q2 * q2)); Yaw = atan2(2 * (q0 * q3 + q1 * q2) , 1 - 2 * (q2 * q2 + q3 * q3)); Pitch = asin(2*(q0*q2-q3*q1)) ; } /*更新四元数*/ /* q_n 这里代表新来的四元数,这里指的是增量 */ void Multiply(float q0_n,float q1_n,float q2_n,float q3_n) { float q0_temp=q0*q0_n -q1*q1_n -q2*q2_n -q3*q3_n; float q1_temp=q0*q1_n +q1*q0_n +q2*q3_n -q3*q2_n; float q2_temp=q0*q2_n -q1*q3_n +q2*q0_n +q3*q1_n; float q3_temp=q0*q3_n +q1*q2_n -q2*q1_n +q3*q0_n; q0=q0_temp; q1=q1_temp; q2=q2_temp; q3=q3_temp;
float s=sqrt(q0*q0+q1*q1+q2*q2+q3*q3); //这里重新进行规范化,避免的累积误差
q0=q0/s;
q1=q1/s;
q2=q2/s;
q3=q3/s;
}
网页中给的欧拉角的转动顺序为 yaw pitch roll ,代码不是最重要的而是其验证的思想 .这里我介绍一下代码的思想 ,首先我们假设转动的顺序为 yaw pitch roll ,
这样的话我们首先只进行 yaw角度的旋转,pitch角度的旋转,roll角度的旋转 ,如果假设成立的话应该出现下面这结果,也就是说最终的欧拉角不相互影响:
进一步验证其正确性,其共有6种可能性,逐一进行验证,最终确定其转动顺序为 yaw pitch roll .和之前说的一致.
我利用这个算法 对陀螺仪数据 简单平均采样,限幅处理之后,直接积分的效果:
下面这个视频是 与加计互补滤波,可以全姿态
imu.c 与 互补滤波 四元数算法对比
博文为本人所写转载请表明出处 :验证网络上四元数的正确性