四元数与欧拉角－－－wii 的坐标系转换

From：http://blog.ednchina.com/zhouusong/1616965/message.aspx #

四元数表示姿态：四元数表示姿态主 要是用在捷连惯导系统陀螺运算姿态（山寨的动态姿态就是这样算出来的），3D游戏（因为运算简单，不像欧拉角需要大量的三角函数，但是这个领域我不太 懂），是用4个数来表示一个姿态，这四个数的平方和为1（曾经我认为，既然欧拉角能用3个数表示，现在用四个数来表示，是不是会有过定义的情况，同样有这 个想法的看客可以打消，因为这四个数是相互约束的，不像欧拉角三个角度是独立的），四元数通常简写为 Q0，Q1，Q2，Q3，其中Q0表示一个旋转角度（后面有描述），Q1，Q2，Q3表示的是一个空间向量，四元数的物理意义就，物体从姿态原点（可认为 是三个欧拉角都是0的姿态），围绕向量（Q1，Q2，Q3）旋转一个角度f(Q0) (这里注意，旋转的是一个和Q0有关的角度)，这个物理意义我是看了好久领悟了好久在一天凌晨2点刷牙时才领悟出来的，不知道看客能领悟这个意义不~~
欧拉角表示姿态：据我查资料，欧拉角在航空领域又被叫做泰特-布莱恩特，由pitch（俯仰角），roll（横滚角），yaw（导航角）构成，把被观测物 体看作飞机，pitch的范围是（-90～90），roll和yaw的范围都是（-180～180）,该姿态角在东北天坐标系（在地面，以东面为X轴的正 方向，北面为Y轴的正方向，天（上面）为Z的正方向）中，按照右手定则，逆时针方向为增加～很多教材很多资料在这个领域都不是很统一，我只根据著名的 xsens公司的ahrs来定义的，我的山寨ahrs是按照这个范围来做的，以后的讨论也是按照这个规则来（我看过很多书，很多网上资料，这些定义都不尽 相同，虽然相互可以转化，但是我还是喜欢xsens这个定义）但是，欧拉表示有个致命的弱点，就是万向节死锁，有意的同志可以去搜索下

姿态矩阵（旋转矩阵）表示姿态：姿态矩阵是一个3x3的矩阵，姿态矩阵通常用在工业机器人这样的一些有级联旋转机构的东西上，例如，机器人的手臂此刻处于 零姿态，一级手臂末端点空间坐标为（X0，Y0，Z0），这相当于一个已知数，现在手臂旋转运动到某一姿态，这个时候的一级手臂末端点空间坐标为 （X1，Y1，Z1），请问我怎么求的现在这个坐标（X1，Y1，Z1）？这个时候姿态矩阵就起到作用了～～因为机械臂的旋转是由电机驱动的，电机旋转了 多少角度可以通过编码器知晓，那么，就能找到这个时候手臂的欧拉角姿态（以后分析怎么得到），通过欧拉角就可以算出旋转矩阵，在用（X0，Y0，Z0）的 转置（3行1列的向量）去乘以这个矩阵，得到的一个3行1列的向量，就能得到（X1，Y1，Z1），就是这么的，同理手臂的第二级，第三级。。。都可以这 样求得~~~

 

/* 输入欧拉角，能看到四元数，以及再转换回去成欧拉角 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(); }