原创四元数，欧拉角及姿态矩阵的相互转换

 2010-5-10 20:12  15386 6 11 分类: 测试测量

网上太多的将转换的了，翻来覆去转载没有意义。。奉上源码，TC下直接编译即可～～在附上编译好了的exe可以直接下载运行～～
../upload/2010/5/10/6c4dc8dd-9abf-4c27-b068-d2f03f1eb92e.rar
～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～～
～～～～～～～～不华丽的分割～～以下是源码～～～～～～～～～～～～～～～～～～～～～～
/* 输入欧拉角，能看到四元数，以及再转换回去成欧拉角
    Yaw范围（-180~180）
    Pitch范围（-90~90）
    Roll范围（-180~180）
*/

#include "stdio.h"
#include "math.h"
#include "conio.h"

main()
{
float theta_z , theta_y ,theta_x ;

float cos_z_2;
float cos_y_2;
float cos_x_2;

float sin_z_2;
float sin_y_2;
float sin_x_2;

float Pitch;
float Roll;
float Yaw;
float Q[4];
float   T[3][3];
do{
printf("\nYaw = ");
scanf("%f",&theta_z);
printf("\nPitch = ");
scanf("%f",&theta_y);
printf("\nRoll = ");
scanf("%f",&theta_x);

theta_z = theta_z*3.1416/180;
theta_y = theta_y*3.1416/180;
theta_x = theta_x*3.1416/180;

cos_z_2 = cos(0.5*theta_z);
cos_y_2 = cos(0.5*theta_y);
cos_x_2 = cos(0.5*theta_x);

sin_z_2 = sin(0.5*theta_z);
sin_y_2 = sin(0.5*theta_y);
sin_x_2 = sin(0.5*theta_x);

Q[0] = cos_z_2*cos_y_2*cos_x_2 + sin_z_2*sin_y_2*sin_x_2;
Q[1] = cos_z_2*cos_y_2*sin_x_2 - sin_z_2*sin_y_2*cos_x_2;
Q[2] = cos_z_2*sin_y_2*cos_x_2 + sin_z_2*cos_y_2*sin_x_2;
Q[3] = sin_z_2*cos_y_2*cos_x_2 - cos_z_2*sin_y_2*sin_x_2;

printf("\nQ=[ %f %f %f %f]\n\n",Q[0],Q[1],Q[2],Q[3]) ;
printf("alpha = %f\n\n",acos(Q[0])*2*180/3.1416) ;

    T[0][0] =   Q[0]*Q[0]+Q[1]*Q[1]-Q[2]*Q[2]-Q[3]*Q[3] ;
    T[0][1] =                    2*(Q[1]*Q[2]-Q[0]*Q[3]);
    T[0][2] =                    2*(Q[1]*Q[3]+Q[0]*Q[2]);

    T[1][0] =                    2*(Q[1]*Q[2]+Q[0]*Q[3]);
    T[1][1] =   Q[0]*Q[0]-Q[1]*Q[1]+Q[2]*Q[2]-Q[3]*Q[3] ;
    T[1][2] =                    2*(Q[2]*Q[3]-Q[0]*Q[1]);

    T[2][0] =                    2*(Q[1]*Q[3]-Q[0]*Q[2]);
    T[2][1] =                    2*(Q[2]*Q[3]+Q[0]*Q[1]);
    T[2][2] =   Q[0]*Q[0]-Q[1]*Q[1]-Q[2]*Q[2]+Q[3]*Q[3] ;

    printf("T[0][0] = %9f,T[0][1] = %9f,T[0][2] = %9f\n",T[0][0],T[0][1],T[0][2]);
    printf("T[1][0] = %9f,T[1][1] = %9f,T[1][2] = %9f\n",T[1][0],T[1][1],T[1][2]);
    printf("T[2][0] = %9f,T[2][1] = %9f,T[2][2] = %9f\n\n",T[2][0],T[2][1],T[2][2]);

    Pitch = asin(-T[2][0]);
    Roll = atan( T[2][1]/T[2][2]);
    Yaw   = atan( T[1][0]/T[0][0]);

    if(T[2][2]<0)
    {
        if(Roll < 0)
        {
           Roll = Roll+3.1416;
        }
        else
        {
           Roll = Roll-3.1416;
        }
    }

    if(T[0][0]<0)
    {
        if(T[1][0]>0)
        {
            Yaw = Yaw + 3.1416;
        }
        else
        {
            Yaw = Yaw - 3.1416;
        }
    }

    printf("Yaw   = %f\nPitch = %f\nRoll = %f\n",Yaw*180/3.1416,Pitch*180/3.1416,Roll*180/3.1416) ;
}while(1);
    printf("Hello, world\n");
    getch();
}

写原创有奖励！2024面包板原创奖励正在进行中

上一篇：四元数，欧拉角和旋转矩阵的各自的优点用途
下一篇：关于卡尔曼滤波

IIC Shenzhen - 2024国际集成电路展览会暨研讨会（深圳，11.5-6）

2024 AI& Connect 是德创新技术峰会（深圳，10.22）

第六届意法半导体工业峰会2024（深圳，10.29）

2024 Arm Tech Symposia 年度大会（上海/深圳，11月19/21日）

文章评论（5条评论）

登录后参与讨论

用户377235 2015-3-23 10:43

谢谢分享，好贴

回复

用户377235 2014-11-24 12:51

谢谢！学习中

回复

用户377235 2013-9-7 05:58

perfect,thanks

回复

用户1632169 2011-6-11 16:56

刚刚接触陀螺仪，向你学习！附件好像下载不了！

回复

用户230612 2010-9-27 10:01

你好，我想下载下你的源码学习下，不过好像下不了了呢。。。

回复

查看更多评论

相关推荐阅读

用户47232 2010-05-12 18:10: 加速度计，磁场计的标定困难; 由参数那篇文章可知，传感器最重要的两个参数是零点和灵敏度~~这种场传感器的零点和灵敏度怎么标定呢。。。首先。。灵敏度的标定需要在零点确定以后才有可能进行~~以加速度计为例，我在地球这个环境里。。任何地...

用户47232 2010-05-10 19:58: 四元数，欧拉角和旋转矩阵的各自的优点用途; 四元数表示姿态：四元数表示姿态主要是用在捷连惯导系统陀螺运算姿态（山寨的动态姿态就是这样算出来的），3D游戏（因为运算简单，不像欧拉角需要大量的三角函数，但是这个领域我不太懂），是用4个数来表示一个姿...