旋转矩阵、欧拉角、四元数

表示数字姿态刚体的姿态（attitude）有很多种表示方法，关于这个话题有一篇十分出名的综述[1]，也是这篇文章的主要资料来源。

这篇文章从二维旋转开始，会讨论旋转矢量、旋转矩阵、四元数、欧拉角等旋转的表示方法。

在开始讨论前，需要明确的一点是刚体的姿态具有三个自由度，但使用三个参数对姿态进行全局的、没有奇异性的描述已经被证明是不可能的[2]，这也是近来IMU姿态估计的方法大多使用四元数或旋转矩阵的原因。

1.二维旋转的表示相比在三维空间中的旋转，二维旋转十分直观且易于理解。

二维旋转指将二维平面上的一个向量 \bm{v} （起点为原点）绕原点旋转一个角度 \theta ，得到一个新的向量\bm{v}_{\theta} 。

角度 \theta 可以完全描述这个旋转操作，因此是最直接的二维旋转的表示方法。

但需要注意的是，因为角度具有周期性，任何相差 2\pi 的整数倍的两个角度所代表的旋转是相同的（只考虑旋转的结果），所以表示角度的空间不是实数集 \mathbb{R} ，而是一个商空间\mathbb{R}/2\pi 。

这个商空间可以和平面上的单位圆做一一对应：\theta\leftrightarrow(\cos{\theta},\sin{\theta}) ，也就是说，我们可以认为单位圆上的每一个点，对应了一个独特的旋转操作，这也是二维旋转的第二种表示方法。

最后，如果我们把旋转后得到的向量写成坐标的形式，可以得到：\bm{v}_{\theta}=\left[\begin{matrix}\cos{\theta}&-\sin{\theta}\\\sin{\theta}&\cos{\theta}\end{matrix}\ri ght]\bm{v}=\mathbf{R}(\theta)\bm{v} (1)其中 \mathbf{R}(\theta) 被称为二维旋转矩阵，可以和\mathbb{R}/2\pi 作一一对应，是二维旋转的第三种表示方法。

旋转矩阵、旋转向量、欧拉⾓、四元数的关系向量的矩阵形式有两个向量：→a =(a 1,a 2,a 3)→b =(b 1,b 2,b 3)叉乘的结果表⽰⼀个向量，这个向量向量垂直于a,b 向量构成的平⾯。

→a ×→b =‖e 1e 2e 3a 1a 2a 3b 1b 2b 3‖=a 2b 3−a 3b 2a 3b 1−a 1b 3a 1b 2−a 2b 1=0−a 3a 2a 30−a 1−a 2a 10b 1b 2b 3=a ∧b将向量a 对应的矩阵表⽰出来，为⼀个反对称矩阵，每⼀个向量都对应着⼀个反对称矩阵。

这就引出向量的矩阵形式。

a ∧=0−a 3a 2a 30−a 1−a 2a 1坐标变换的易混点在齐次变换中p 1=T 12·p 2p 2=T 23·p 3T 12表⽰，把坐标系{2}的向量变换到坐标系{1}中，T 23同理，如果把坐标系{3}下的向量变换到坐标系{1}中为：p 1=T 12·T 23·p 3旋转向量和欧拉⾓: SO(3)的旋转矩阵有9个量，但是只有3个⾃由度，同理SE(3)有16个量，但是也只有6个⾃由度。

在实际的旋转中，任意的旋转都可⽤⼀个旋转轴和⼀个旋转⾓来表⽰，我们使⽤⼀个向量，⽅向与旋转轴⼀致，长度等于旋转⾓，这样只需要⼀个三维向量即可描述旋转。

对于SE(3),⽤⼀个旋转向量和⼀个平移向量即可表达，恰好⾃由度为6.如果⽤旋转向量来描述R ：旋转轴为⼀个单位长度的向量n,⾓度为θ，那么θn 可以表⽰这个旋转。

旋转矩阵R 和旋转向量θn 的转换过程为罗德⾥格斯变换：R =cos θI +(1−cos θ)nn T +sin θn ∧此处末尾的n ∧ 如上⾯所⽰，代表矩阵表⽰的向量。

那么反过来通过旋转矩阵获取转⾓ θ;θ=arccostr (R )−12tr(R)为矩阵R 的迹。

对于转轴n,Rn=n;表⽰为转轴绕⾃⾝转动不⽣改变，从数学来说n 是矩阵R 特征值为1时对应的特征向量。

旋转矩阵、欧拉⾓、四元数理论及其转换关系1. 概述旋转矩阵、欧拉⾓、四元数主要⽤于表⽰坐标系中的旋转关系，它们之间的转换关系可以减⼩⼀些算法的复杂度。

本⽂主要介绍了旋转矩阵、欧拉⾓、四元数的基本理论及其之间的转换关系。

2、原理2.1 旋转矩阵对于两个三维点p1(x1,y1,z1)，p2(x2,y2,z2)，由点 p1 经过旋转矩阵 R 旋转到 p2，则有注：旋转矩阵为正交矩阵RR^T=E任意旋转矩阵：任何⼀个旋转可以表⽰为依次绕着三个旋转轴旋三个⾓度的组合。

这三个⾓度称为欧拉⾓。

三个轴可以指固定的世界坐标系轴，也可以指被旋转的物体坐标系的轴。

三个旋转轴次序不同，会导致结果不同。

2.2 欧拉⾓欧拉⾓有两种：静态：即绕世界坐标系三个轴的旋转，由于物体旋转过程中坐标轴保持静⽌，所以称为静态。

动态：即绕物体坐标系三个轴的旋转，由于物体旋转过程中坐标轴随着物体做相同的转动，所以称为动态。

使⽤动态欧拉⾓会出现万向锁现象；静态欧拉⾓不存在万向锁的问题。

对于在三维空间⾥的⼀个参考系，任何坐标系的取向，都可以⽤三个欧拉⾓来表现。

参考系⼜称为实验室参考系，是静⽌不动的。

⽽坐标系则固定于刚体，随着刚体的旋转⽽旋转。

如图1，设定xyz-轴为参考系的参考轴。

称xy-平⾯与XY-平⾯的相交为交点线，⽤英⽂字母（N）代表。

zxz顺规的欧拉⾓可以静态地这样定义：α是x-轴与交点线的夹⾓，β是z-轴与Z-轴的夹⾓，γ是交点线与X-轴的夹⾓。

图中三个欧拉⾓分别为：(α,β,γ)；蓝⾊的轴为：xyz轴红⾊的轴为：XYZ轴绿⾊的线为交线：Nα∈[0,2π],β∈[0,π],γ∈[0,2π]很可惜地，对于夹⾓的顺序和标记，夹⾓的两个轴的指定，并没有任何常规。

科学家对此从未达成共识。

每当⽤到欧拉⾓时，我们必须明确的表⽰出夹⾓的顺序，指定其参考轴。

实际上，有许多⽅法可以设定两个坐标系的相对取向。

欧拉⾓⽅法只是其中的⼀种。

此外，不同的作者会⽤不同组合的欧拉⾓来描述，或⽤不同的名字表⽰同样的欧拉⾓。

三维旋转矩阵与自身相乘1. 介绍在计算机图形学和计算机视觉领域，三维旋转矩阵是一种用于描述三维空间中物体旋转的数学工具。

它可以通过矩阵相乘的方式将一个向量或者一个矩阵进行旋转变换。

本文将详细介绍三维旋转矩阵的定义、性质以及如何将一个三维旋转矩阵与自身相乘的操作。

2. 三维旋转矩阵的定义三维旋转矩阵是一个3x3的正交矩阵，表示一个物体在三维空间中的旋转变换。

它的每一列向量都是一个单位向量，且互相垂直。

三维旋转矩阵可以通过欧拉角、四元数或旋转矩阵的形式进行表示。

2.1 欧拉角表示欧拉角表示法使用三个角度来描述旋转变换。

常用的欧拉角顺序有XYZ、XZY、YXZ、YZX、ZXY和ZYX六种。

以XYZ顺序为例，旋转矩阵可以表示为：R = Rz * Ry * Rx其中Rx、Ry和Rz分别表示绕X轴、Y轴和Z轴的旋转矩阵。

2.2 四元数表示四元数是一种扩展了复数的数学工具，可以用来表示旋转变换。

一个四元数可以表示为：q = w + xi + yj + zk其中w、x、y和z是实数，i、j和k是虚数单位。

通过四元数可以构造一个旋转矩阵，表示为：R = | 1-2y^2-2z^2 2xy-2wz 2xz+2wy || 2xy+2wz 1-2x^2-2z^2 2yz-2wx || 2xz-2wy 2yz+2wx 1-2x^2-2y^2 |2.3 旋转矩阵表示旋转矩阵可以直接表示为一个3x3的矩阵，其中每一列向量都是一个单位向量，且互相垂直。

例如，绕X轴旋转θ角度的旋转矩阵可以表示为：R = | 1 0 0 || 0 cosθ -si nθ || 0 sinθ cosθ |3. 三维旋转矩阵的性质三维旋转矩阵具有一些重要的性质，这些性质对于理解旋转矩阵的操作非常重要。

3.1 正交性旋转矩阵的每一列向量都是单位向量，且互相垂直。

这意味着旋转矩阵是一个正交矩阵，满足以下等式：R^T * R = I其中R^T表示R的转置，I表示单位矩阵。

旋转知识点总结旋转是一种常见的几何变换，它改变了物体的方向、位置和角度。

在计算机图形学、几何学、物理学和工程学等领域都有广泛的应用。

下面是对旋转相关知识点的一些总结：1. 旋转的定义：旋转是一种刚体运动，它将物体绕着特定的轴线转动一定的角度。

旋转由旋转中心、旋转轴和旋转角度三个要素来描述。

2. 旋转的方向：旋转可以是顺时针方向或逆时针方向。

在三维空间中，右手法则可以确定旋转的方向。

3. 旋转角度的表示：旋转角度可以用弧度制或角度制来表示。

弧度制是使用弧长与半径的比值来表示角度，角度制则是使用度数来表示。

4. 旋转矩阵：旋转可以用旋转矩阵来表示。

旋转矩阵是一个二维矩阵，其中每个元素表示旋转后的坐标与旋转前的坐标之间的关系。

5. 旋转轴的表示：旋转轴可以用向量来表示，向量的方向和大小决定了旋转轴的方向和旋转角度的大小。

6. 旋转的基本性质：旋转具有一些基本的性质，包括不变性、可逆性、可叠加性等。

这些性质对于旋转的应用非常重要。

7. 旋转的合成：旋转可以进行合成，即先进行一个旋转，再进行另一个旋转。

合成旋转可以通过旋转矩阵的乘法来实现。

8. 旋转的变换：旋转可以用来进行物体的变换，包括位置的变换、形状的变换和姿态的变换等。

旋转变换可以通过矩阵乘法来实现。

9. 欧拉角和四元数：欧拉角和四元数是常用的旋转表示方法。

欧拉角使用三个独立的角度来表示旋转，而四元数使用一个四维向量来表示旋转。

10. 旋转的应用：旋转在计算机图形学中有广泛的应用，包括三维建模、动画、物理模拟等。

旋转也被广泛应用于机器人学、飞行控制、游戏开发等领域。

11. 旋转的误差：由于测量误差和计算误差等原因，旋转变换可能会引入一定的误差。

为了减少误差，可以使用数值方法和优化算法等技术来进行旋转估计和校正。

12. 旋转的性能优化：旋转的计算通常比较复杂，对于大规模的数据和复杂的模型，旋转计算可能会成为性能瓶颈。

为了提高性能，可以使用并行计算、SIMD指令、快速算法等技术来加速旋转计算。

1.轴、角转四元数公式：q = [cos(Q/2), sin(Q /2)v] v是旋转轴矢量，Q是旋转角度2.欧拉角转四元数:qroll = [cos (y/2), (sin(y/2), 0, 0)]qpitch = [cos (q/2), (0, sin(q/2), 0)]qyaw = [cos(f /2), (0, 0, sin(f /2)]pitch(俯仰），yaw(偏航），roll（滚转）3.四元数转旋转矩阵：| 1 - 2y2 - 2z2 2yz + 2wx 2xz - 2wy | Rm = | 2xy - 2wz 1 - 2x2 - 2z2 2yz - 2wx | | 2xz + 2wy 2yz - 2wx 1 - 2x2 - 2y2|4.矩阵转四元数代码：MatToQuat(float m[4][4], QUAT * quat){float tr, s, q[4];int i, j, k;int nxt[3] = {1, 2, 0};tr = m[0][0] + m[1][1] + m[2][2];// check the diagonalif (tr > 0.0){s = sqrt (tr + 1.0);quat->w = s / 2.0;s = 0.5 / s;quat->x = (m[1][2] - m[2][1]) * s;quat->y = (m[2][0] - m[0][2]) * s;quat->z = (m[0][1] - m[1][0]) * s;}else{// diagonal is negativei = 0;if (m[1][1] > m[0][0]) i = 1;if (m[2][2] > m[i][i]) i = 2;j = nxt[i];k = nxt[j];s = sqrt ((m[i][i] - (m[j][j] + m[k][k])) + 1.0); q[i] = s * 0.5;if (s != 0.0) s = 0.5 / s;q[3] = (m[j][k] - m[k][j]) * s;q[j] = (m[i][j] + m[j][i]) * s;q[k] = (m[i][k] + m[k][i]) * s;quat->x = q[0];quat->y = q[1];quat->z = q[2];quat->w = q[3];}}5.四元数转矩阵代码：QuatToMatrix(QUAT * quat, float m[4][4]){float wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;// calculate coefficientsx2 = quat->x + quat->x;y2 = quat->y + quat->y;z2 = quat->z + quat->z;xx = quat->x * x2;xy = quat->x * y2;xz = quat->x * z2;yy = quat->y * y2;yz = quat->y * z2;zz = quat->z * z2;wx = quat->w * x2;wy = quat->w * y2;wz = quat->w * z2;m[0][0] = 1.0 - (yy + zz);m[1][0] = xy - wz;m[2][0] = xz + wy;m[3][0] = 0.0;m[0][1] = xy + wz;m[1][1] = 1.0 - (xx + zz);m[2][1] = yz - wx;m[3][1] = 0.0;m[0][2] = xz - wy;m[1][2] = yz + wx;m[2][2] = 1.0 - (xx + yy);m[3][2] = 0.0;m[0][3] = 0;m[1][3] = 0;m[2][3] = 0;m[3][3] = 1;}6.欧拉角转四元数代码：EulerToQuat(float roll, float pitch, float yaw, QUAT * quat) {float cr, cp, cy, sr, sp, sy, cpcy, spsy;// calculate trig identitiescr = cos(roll/2);cp = cos(pitch/2);cy = cos(yaw/2);sr = sin(roll/2);sp = sin(pitch/2);sy = sin(yaw/2);cpcy = cp * cy;spsy = sp * sy;quat->w = cr * cpcy + sr * spsy;quat->x = sr * cpcy - cr * spsy;quat->y = cr * sp * cy + sr * cp * sy; quat->z = cr * cp * sy - sr * sp * cy; }四元数乘法QuatMul(QUAT *q1, QUAT *q2, QUAT *res){ float A, B, C, D, E, F, G, H;A = (q1->w + q1->x)*(q2->w + q2->x);B = (q1->z - q1->y)*(q2->y - q2->z);C = (q1->w - q1->x)*(q2->y + q2->z);D = (q1->y + q1->z)*(q2->w - q2->x);E = (q1->x + q1->z)*(q2->x + q2->y);F = (q1->x - q1->z)*(q2->x - q2->y);G = (q1->w + q1->y)*(q2->w - q2->z);H = (q1->w - q1->y)*(q2->w + q2->z);res->w = B + (-E - F + G + H) /2;res->x = A - (E + F + G + H)/2;res->y = C + (E - F + G - H)/2;res->z = D + (E - F - G + H)/2;}四元数插值QuatSlerp(QUAT * from, QUAT * to, float t, QUAT * res){float to1[4];double omega, cosom, sinom, scale0, scale1;// calc cosinecosom = from->x * to->x + from->y * to->y + from->z * to->z + from->w * to->w;// adjust signs (if necessary)if ( cosom <0.0 ){ cosom = -cosom; to1[0] = - to->x;to1[1] = - to->y;to1[2] = - to->z;to1[3] = - to->w;} else {to1[0] = to->x;to1[1] = to->y;to1[2] = to->z;to1[3] = to->w;}// calculate coefficientsif ( (1.0 - cosom) > DELTA ) {// standard case (slerp)omega = acos(cosom);sinom = sin(omega);scale0 = sin((1.0 - t) * omega) / sinom; scale1 = sin(t * omega) / sinom;} else {// "from" and "to" quaternions are very close// ... so we can do a linear interpolationscale0 = 1.0 - t;scale1 = t;}// calculate final valuesres->x = scale0 * from->x + scale1 * to1[0]; res->y = scale0 * from->y + scale1 * to1[1]; res->z = scale0 * from->z + scale1 * to1[2]; res->w = scale0 * from->w + scale1 * to1[3]; }。

三维旋转：旋转矩阵，欧拉⾓，四元数原⽂见我的，欢迎⼤家过去评论。

如何描述三维空间中刚体的旋转，是个有趣的问题。

具体地说，就是刚体上的任意⼀个点P(x, y, z)围绕过原点的轴(i, j, k)旋转θ，求旋转后的点P\'(x\', y\', z\')。

旋转矩阵旋转矩阵乘以点P的齐次坐标，得到旋转后的点P'，因此旋转矩阵可以描述旋转，$$\begin{bmatrix}x'\\ y'\\ z'\\ 1\end{bmatrix}=R\cdot \begin{bmatrix}x\\ y\\ z\\ 1\end{bmatrix}$$绕x，y，或z轴旋转θ的矩阵为：$$R_{x}(\theta)=\begin{bmatrix}1 & 0 & 0\\ 0 & \cos\theta & -\sin\theta\\ 0 & \sin\theta & \cos\theta\end{bmatrix}$$$$R_{y}(\theta)=\begin{bmatrix}\cos\theta & 0 & -\sin\theta\\ 0 & 1 & 0\\ \sin\theta & 0 & \cos\theta\end{bmatrix}$$$$R_{z}(\theta)=\begin{bmatrix}\cos\theta & -\sin\theta & 0\\ \sin\theta & \cos\theta & 0\\ 0 & 0 & 1\end{bmatrix}$$所以，绕任意轴旋转的矩阵为$$R_{x}(-p)\cdot R_{y}(-q)\cdot R_{z}(\theta)\cdot R_{y}(q)\cdot R_{x}(p)$$这表⽰：1. 绕x轴旋转⾓度p使指定的旋转轴在xz平⾯上2. 绕y轴旋转⾓度q使指定的旋转轴与z轴重合3. 绕z轴旋转⾓度θ4. 绕y轴旋转⾓度-q5. 绕x轴旋转⾓度-p其中，p和q的值需要⽤i,j,k计算出来。

三维空间旋转坐标表示
在三维空间中，旋转坐标通常用欧拉角、旋转矩阵或四元数来表示。

1. 欧拉角：欧拉角是用来描述三维空间中旋转的一种方法，它使用三个角度值（通常用α、β和γ表示）来表示一个物体的方位。

其中，α表示物体绕着垂直于纸面的轴线旋转的角度，β表示物体绕着平行于纸面的轴线旋转的角度，γ表示物体绕着通过旋转轴线的垂直轴线旋转的角度。

2. 旋转矩阵：旋转矩阵是一种用来描述三维空间中旋转的数学工具，它是一个3x3的方阵，可以表示一个绕着某个轴旋转一定角度的旋转操作。

旋转矩阵的表示方法有很多种，其中最常用的是单位矩阵和绕着Z轴旋转的矩阵。

单位矩阵表示不进行任何旋转，而绕着Z轴旋转的矩阵则可以用来表示绕着Z轴旋转一定角度的操作。

3. 四元数：四元数是一种用来描述三维空间中旋转的数学工具，它由一个实数和三个虚数组成，可以表示一个绕着某个轴旋转一定角度的旋转操作。

四元数的表示方法有很多种，其中最常用的是单位四元数和绕着Z轴旋转的四元数。

单位四元数表示不进行任何旋转，而绕着Z轴旋转的四元数则可以用来表示绕着Z轴旋转一定角度的操作。

总之，三维空间的旋转坐标表示方法有很多种，具体使用哪种方法取决于具体的应用场景和需求。

四元数、欧拉角、旋转矩阵四元数、欧拉角和旋转矩阵是三种常用的描述三维空间中物体旋转的方法。

它们在计算机图形学、物理模拟、机器人学等领域发挥着重要作用。

本文将分别介绍这三种描述方法的原理和应用以及它们之间的关系。

首先，我们来介绍四元数。

四元数是一种具有四个实数分量的数学工具，在三维空间中可以用来表示旋转。

一个四元数可以表示为q = a + bi + cj + dk，其中a、b、c和d都是实数，且满足单位长度条件a^2 + b^2 + c^2 + d^2 = 1。

四元数与旋转的关系可以通过四元数乘法来描述，即两个四元数p和q的乘积pq表示将p所表示的旋转应用到q所表示的向量上。

四元数旋转具有很好的插值性质和无歧义性，因此在计算机图形学等领域得到了广泛应用。

接下来，我们介绍欧拉角。

欧拉角是一种将旋转表示为一系列基本旋转的方法。

在三维空间中，常用的欧拉角包括绕X轴旋转的俯仰角（pitch）、绕Y轴旋转的偏航角（yaw）和绕Z轴旋转的滚转角（roll）。

欧拉角可以通过矩阵乘法来表示旋转，即将三个基本旋转矩阵按顺序相乘。

欧拉角相对直观，易于理解和可视化，但存在万向锁问题，即当某个旋转角接近90度时，会出现无法唯一表示旋转的情况。

最后，我们介绍旋转矩阵。

旋转矩阵是一个3x3的正交矩阵，它通过乘法作用在向量上实现旋转。

旋转矩阵具有正交性和行列式等于1的特点，因此可以保持向量的长度和直角关系。

旋转矩阵也可用于表示三维空间中的旋转，其旋转效果等价于欧拉角表示和四元数表示。

旋转矩阵相对简单，容易计算和处理，但在插值和融合等方面相对复杂。

三种旋转描述方法之间存在着数学上的对应关系。

欧拉角和旋转矩阵可以相互转换，通过旋转矩阵可以计算出对应的欧拉角，反之亦然。

四元数和旋转矩阵也可以相互转换，通过旋转矩阵可以计算出对应的四元数，反之亦然。

尽管存在转换关系，但在实际应用中，需要根据具体需求选择合适的旋转描述方法。

综上所述，四元数、欧拉角和旋转矩阵是描述三维空间物体旋转的常用方法。

四元数与旋转一．四元组基础Q(x,y,z,w)，其中x，y，z用来确定旋转轴，w为旋转的角度Q=w+xi+yj+zk，i，j，k为三个虚轴的单位分量I*j=kJ*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=qs4．共轭p=(w,v)，则p*=(w,-v)(pq)*=q*p*N(q)=w2+x2+y2+z2q-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(2)=axis(2)*sin(t);q(3)=axis(3)*sin(t);q(4)=cos(t);end计算出了q之后，可以获得对应的旋转矩阵，旋转矩阵的计算Matlab里面的矩阵是以列为主顺序的function r=q2rot(q)w=q(4);x=q(1);y=q(2);z=q(3);r=zeros(3,3);r(1,1)=1-2*y*y-2*z*z;r(1,2)=2*x*y+2*w*z;r(1,3)=2*x*z-2*w*y;r(2,1)=2*x*y-2*w*z;r(2,2)=1-2*x*x-2*z*z;r(2,3)=2*z*y+2*w*x;r(3,1)=2*x*z+2*w*y;r(3,2)=2*y*z-2*w*x;r(3,3)=1-2*x*x-2*y*y;r=r';end同时，也可以根据四元数来计算欧拉角function R=q2euler(q)w=q(4);x=q(1);y=q(2);z=q(3);t11=2*(w*x+y*z);t12=1-2*(x*x+y*y);R(1)=atan2(t11,t12);t2=2*(w*y-z*x);R(2)=asin(t2);t31=2*(w*z+x*y);t32=1-2*(y*y+z*z);R(3)=atan2(t31,t32);end计算出来的欧拉角rx，ry，rz，分别为绕X轴、Y轴和Z轴的旋转角，假如有：Rotq=q2rot(q)R=q2euler(q)[rotx roty rotz]=Rotation(R)可以发现Rotq==rotz*roty*rotx从这里可以看出，上面使用四元数这样计算出来的旋转矩阵的旋转顺序分别是X轴、Y轴和Z轴的ra=pi/4;qz=[0 0 -sin(ra) cos(ra)] %绕z旋转-90度qy=[0 sin(ra) 0 cos(ra) ] %绕y旋转90度qyz=qmult(qy,qz)r=q2euler(qyz)上面的r得出的结果为r = -1.5708 0.0000 -1.5708也就是说其几何意义变成先绕X轴旋转-90度，再绕Z轴旋转-90度，而根据qy和qz的相乘我们实际进行的操作却是先绕Z轴旋转-90度，再绕Y轴旋转90度，但是结果却是这两种操作等价，这说明由四元数到欧拉角可以有多个解两个四元数，假如它们的方向是相反的，用它们作用于向量得到的新向量的值仍然相等q1=[0.024666 -0.023954 0.504727 0.862594];arm=[-8.881719 6.037597 -2.36776];q2=-q1;rot1=q2rot(q1);rot2=q2rot(q2);v1=rot1*arm'v2=rot2*arm'上面计算出来的v1等于v2四元数的余弦值为它们的内积假如余弦值小于0，则需要将其中的一个取反，因为上面我们知道一个四元数和它的反方向的四元数对一个向量起相同的作用四元数的相乘，代表旋转的累积pq=p*q;rotp=q2rot(p);rotq=q2rot(q);rotpq=q2rot(pq);rotmul=rotp*rotq;这里rotpq与rotmul相等四． OGRE中Quaternion类的几个函数1.四元数到旋转向量void Quaternion::ToRotationMatrix (Matrix3& kRot) const1 - 2*qy2 -2*qz22*qx*qy -2*qz*qw2*qx*qz +2*qy*qw2*qx*qy + 2*qz*qw 1 - 2*qx2 -2*qz22*qy*qz -2*qx*qw2*qx*qz -2*qy*qw 2*qy*qz +2*qx*qw1 - 2*qx2 -2*qy22.旋转量到四元数根据1中的表格，有：4 *(1-qx2-qy2-qz2) = 1 + m00 + m11 + m22又qw2=1-qx2-qy2-qz2，可得4 *qw2= 1 + m00 + m11 + m22这里解qw必须保证1 + m00 + m11 + m22>=0，如果不是的话，就构造其他的等式来计算，OGRE中分成两种情况，一种是m00 + m11 + m22>=0，就可以直接先解出qw，否则的采用另外的等式计算3．Local axisVector3 xAixs(void) const;取得旋转矩阵的第一列，旋转矩阵和一个向量相乘的话，第一列的数据均和向量的x分量相乘Vecotr3 yAxis(void) const;取得旋转矩阵的第二列，旋转矩阵和一个向量相乘的话，第二列的数据均和向量的y分量相乘Vecotr3 zAxis(void) const;取得旋转矩阵的第三列，旋转矩阵和一个向量相乘的话，第三列的数据均和向量的z分量相乘。