欧拉角与四元数.docVIP

四元数与旋转 一．四元组基础 Q(x,y,z,w)，其中x，y，z用来确定旋转轴，w为旋转的角度 Q=w+xi+yj+zk，i，j，k为三个虚轴的单位分量 I*j=k J*k=i; K*i=j; 叉乘： c=a × b= | i j k| |a1 b1 c1| |a2 b2 c2| =(b1c2-b2c1,c1a2-a1c2,a1b2-a2b1) c也为一个向量，且c的长度为|a||b|sin(theta)，垂直于a和b所在的平面，方向由右手法则来判定，用右手的四指先表示向量a的方向，然后手指朝着手心的方向摆动到向量b的方向，大拇指所指的方向就是向量c的方向 1． 四元组相乘： Q1=w1+x1i+y1j+z1k=(w1,v1) Q2=w2+x2i+y2j+z2k=(w2,v2) Q1*Q2=(w1*w2-v1,v2,w1*v2+w2*v1+v1xv2) ( w1+x1i+y1j+z1k)*( w2+x2i+y2j+z2k) =w1*w2-x1*x2-y1*y2-z1*z2+ (W1*x2+x1*w2+y1*z2-z1-y2)i+ (y1*w2+w1*y2+z1*x2-x1*z2)j+ (w1*z2+z1*w2+x1*y2-y1*x2)k 对于其中的轴部分，假如v1//v2，则有v1 x v2=0（平行向量的叉乘结果为0） 2． 四元组的点乘，点乘积为数值： Q1.*Q2=w1*w2+v1,v2=w1*w2+x1*x2+y1*y2+z1*z2; 3． 数乘 s为一实数，q为四元组，则有sq=qs 4． 共轭 p=(w,v)，则p*=(w,-v) (pq)*=q*p* N(q)=w2+x2+y2+z2 q-1=q*/N(q)---------------à显然可得qq-1=(1,0) 二．使用四元数旋转向量 假如有一表示向量的四元组q=(w,v)，对其应用旋转量p后的结果为： q’=pqp-1=(w,v’) 从上可以看出，计算的结果q’的实部和q的实部是相等的，并且有N(v)=N(v’) 如果N(q)=1，则可以令q=(cosa,usina)，u也为一个单位向量，则q’是q绕u旋转2a个弧度的结果 假如S(q)表示q的实部，则有2S(q)=q+q* 2S(pqp-1)= pqp-1+( pqp-1)*=pqp*+(pqp*)*=pqp*+pq*p*=p(q+q*)p*=2S(q) （这里由于p是单位四元数，所以有p-1等于p*） 欧拉角到四元数的转换 定义pitch, yaw, roll分别为绕X轴、Y轴、Z轴的旋转弧度 float p = pitch * PIOVER180 / 2.0; float y = yaw * PIOVER180 / 2.0; float r = roll * PIOVER180 / 2.0; float sinp = sin(p); float siny = sin(y); float sinr = sin(r); float cosp = cos(p); float cosy = cos(y); float cosr = cos(r); this-x = sinr * cosp * cosy - cosr * sinp * siny; this-y = cosr * sinp * cosy + sinr * cosp * siny; this-z = cosr * cosp * siny - sinr * sinp * cosy; this-w = cosr * cosp * cosy + sinr * sinp * siny; normalise(); 三．使用matlab进行相关计算 计算两个向量v1和v2之间的旋转量四元数p，使得v1应用p后到达v2 假如v1转到v2的旋转轴为v，旋转角为theta，则q=[v*cos(theta/2) sin(theta/2)] Matlab代码： function q=vector2q(v1,v2) %..normalize.... len1=sqrt(v1*v1); len2=sqrt(v2*v2); v1=v1/len1; v2=v2/len2; angle=v1*v2; axis=cross(v1,v2); alen=sqrt(axis*axis); axis=axis/alen; t=acos(angle); t=t/2; q(1)=axis(1)*sin(t); q