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

旋转变换

旋转变换最为直观的表示方法是“轴-角”：绕着某一个过原点轴，旋转某一角度。
轴可以用一个单位长度的点[w 1 ,w 2 ,w 3 ] 表示：原点到该点的射线即为此轴。
使用右手坐标系，拇指指向轴方向，四指方向即为旋转的方向。
一个旋转变换可以用用欧拉角、四元数或者旋转矩阵表示。以下讨论不同表示方法之间的关系，以及旋转变换的合成、取逆等操作。

旋转矩阵

旋转可以看做一种特殊的坐标变换，而坐标变换可以用用3×3 矩阵R 来表示。对一个坐标施加旋转的结果是x ′ =Rx 。
旋转矩阵可以在不同坐标系之间进行变换，但不能进行“反演”，即不能在左手系和右手系之间进行变换。
旋转矩阵是正交矩阵，即|R|=1 ，旋转变换不改变向量的长度。

欧拉角的物理意义

任何一个旋转可以表示为依次绕着三个旋转轴旋三个角度的组合。这三个角度称为欧拉角。
三个轴可以指固定的世界坐标系轴，也可以指被旋转的物体坐标系的轴。三个旋转轴次序不同，会导致结果不同。
本文中提到的欧拉角指：绕着世界坐标系的x,y,z轴，依次旋转的结果。其取值范围如下：

θ x ∈(−π,π),θ y ∈(−π2 ,π2 ),θ z ∈(−π,π) 

欧拉角→ 旋转矩阵

单独绕一个轴旋转θ 角度的旋转矩阵为：

如果依次绕x轴、y轴、z轴旋转，该变换的旋转矩阵为：

R=R z ⋅R y ⋅R x  

记三个轴欧拉角的正弦和余弦函数为s x ,c x ,s y ,c y ,s z ,c z  。使用matlab的syms功能可以轻松推导旋转矩阵R ：

⎡ ⎣ ⎢ c y c z c y s z −s y  c z s x s y −c x s z c x c z +s x s y s z c y s x  s x s z +c x c z s y c x s y s z −c z s x c x c y  ⎤ ⎦ ⎥  

旋转矩阵→ 欧拉角

设旋转矩阵i行j列元素为r ij  。根据旋转矩阵的表达式，利用三角函数可以推导出欧拉角取值：

θ x =atan2(r 32 ,r 33 ) 

θ y =atan2(−r 31 ,r 2 32 +r 2 33  − − − − − − −  √ ) 

θ z =atan2(r 21 ,r 11 ) 

四元数的物理意义

设有一个通过原点[0,0,0] 的旋转轴，该轴上单位长度的点为[w 1 ,w 2 ,w 3 ] 。绕此轴旋转θ 角的变换可以用一个向量表示：

[cosθ2 ,w 1 sinθ2 ,w 2 sinθ2 ,w 3 sinθ2 ] 

也记为q=[q 0 ,q 1 ,q 2 ,q 3 ] ，或者q=q 0 +q 1 i+q 2 j+q 3 k 。四元数的模长为1：

q 2 0 +q 2 1 +q 2 2 +q 2 3 =cos 2 θ2 +sin 2 θ2 (w 2 1 +w 2 2 +w 3 3 )=cos 2 θ2 +sin 2 θ2 =1 

四元数→ 旋转矩阵

利用罗德里格旋转公式可以获得绕某过原点一轴[w 1 ,w 2 ,w 3 ] 旋转某一角度θ 的旋转矩阵R ：

⎡ ⎣ ⎢ cosθ+w 2 1 (1−cosθ)w 1 w 2 (1−cosθ)−w 2 sinθ w 1 w 2 (1−cosθ)cosθ+w 2 2 (1−cosθ)w 1 sinθ w 2 sinθ−w 1 sinθcosθ ⎤ ⎦ ⎥  

代入四元组的表示方法，可得：

⎡ ⎣ ⎢ ⎢ 1−2q 2 2 −2q 2 3 2q 1 q 2 +2q 3 q 0 2q 1 q 3 −2q 2 q 0  2q 1 q 2 −2q 3 q 0 1−2q 2 1 −2q 2 3 2q 2 q 3 +2q 1 q 0  2q 1 q 3 +2q 2 q 0 2q 2 q 3 −2q 1 q 0 1−2q 2 1 −2q 2 2  ⎤ ⎦ ⎥ ⎥  

旋转矩阵→ 四元数

根据旋转矩阵的表达式，利用三角函数性质，可以由旋转矩阵得到四元数：

q 0 =1+r 11 +r 22 +r 33  − − − − − − − − − − − − − −  √ 2  

q 1 =r 32 −r 23 4q 0   

q 2 =r 13 −r 31 4q 0   

q 3 =r 21 −r 12 4q 0   

开根号要求1+Tr(R)>0 （其中Tr 表示矩阵的迹，等于对角元素之和，也等于特征值之和），分式要求q 0 ≠0 。
某些情况下（例如θ=π ），q 0  接近零，Tr(R) 接近-1，需要使用以下方式求解（来源于此处）。

如果r 11 ,r 22 ,r 33  中，r 11  最大：

S=1+r 11 −r 22 −r 33  − − − − − − − − − − − − − −  √  

q 0 =r 32 −r 23 S  

q 1 =S/4 

q 2 =r 12 +r 21 S  

q 3 =r 21 +r 12 S  

如果r 11 ,r 22 ,r 33  中，r 22  最大：

S=1−r 11 +r 22 −r 33  − − − − − − − − − − − − − −  √  

q 0 =r 13 −r 31 S  

q 1 =r 12 +r 21 S  

q 2 =S/4 

q 3 =r 23 +r 32 S  

如果r 11 ,r 22 ,r 33  中，r 33  最大：

S=1−r 11 −r 22 +r 33  − − − − − − − − − − − − − −  √  

q 0 =r 21 −r 12 S  

q 1 =r 13 +r 31 S  

q 2 =r 23 −r 32 S  

q 3 =S/4 

变换的逆

使用欧拉角表示时，必须颠倒三个旋转轴的顺序，同时对旋转角度取反。
使用旋转矩阵表示时，求矩阵的逆即可：R ∗ =R −1  。
使用四元组表示时，考虑其物理意义，对后三位取反即可：q −1 =[q 0 ,−q 1 ,−q 2 ,−q 3 ] 。

向量的叉乘

接下去讨论之前，需要先复习向量的叉乘。

三维空间中的一个点可以表示为向量[xi,yj,zk] ，其中i,j,k 是三个坐标轴：[1,0,0],[0,1,0],[0,0,1] 。
两个向量a,b 叉乘的结果是一个向量，其长度为|a||b|sinθ 。θ 表示从向量a 到b 的小于180°的角度。其方向垂直于a,b 所在平面，遵循右手定则：四指从a 转向b ，拇指方向为叉乘结果方向。

四元组作为一种向量，其叉乘涉及到虚数单位的乘法，遵循以下原则：

i×i=−1 

i×j=−j×i 

i×j=k,j×k=i,k×i=j 

叉乘满足反交换律：

a×b=−b×a 

联想叉乘的物理意义：交换了a,b 顺序，则拇指方向反转。

叉乘满足加法分配律：

(a+b)×c=a×c+b×c 

特别要注意，叉乘不满足结合律：

(a×b)×c≠a×(b×c) 

四元数的叉乘

四元数既不是矢量也不是标量。可以看做一个标量q 0  和一个三维矢量[q 1 ,q 2 ,q 3 ] 的结合体：q=q 0 +q 1 i+q 2 j+q 3 k 。
两个四元数a 1 +a 2 i+a 3 j+a 4 k ，b 1 +b 2 i+b 3 j+b 4 k ，相乘的结果q=a×b ：

q 0 =a 1 b 1 −a 2 b 2 −a 3 b 3 −a 4 b 4  

q 1 =a 1 b 2 +a 2 b 1 +a 3 b 4 −a 4 b 3  

q 2 =a 1 b 3 −a 2 b 4 +a 3 b 1 +a 4 b 2  

q 3 =a 1 b 4 +a 2 b 3 −a 3 b 2 +a 4 b 1  

四元数叉乘不满足交换律、结合律。

变换的组合

使用欧拉角表示时，很难直接组合两个变换。
使用旋转矩阵表示时，依次做矩阵相乘即可：R=R 2 R 1  。
使用四元组表示时，需要对两个四元组叉乘：q=q 2 ×q 1  。
注意，先发生的变换要放在乘号右侧。
由于叉乘不满足结合律，当有一系列变化q 1 ,q 2 ,q 3 ... 陆续发生时，要用括号保证乘法发生的顺序：

q=q 4 ×(q 3 ×(q 2 ×q 1 )) 

使用四元数

首先把待旋转的点(x,y,z) 表示成四元数形式：p=[0,x,y,z] 。这里的四元数不再表示一个旋转，所以模长不一定为1。
对该点p 施加旋转变换q ，通过四元组的两次叉乘实现：p ′ =q×p×q −1 =[0,x ′ ,y ′ ,z ′ ] 。

利用matlab的syms功能（参看附录），可以得到对三维点[x,y,z] 施加变换[q 0 ,q 1 ,q 2 ,q 3 ] 的结果：

x ′ =x(q 2 0 +q 2 1 −q 2 2 −q 2 3 )+2y(q 1 q 2 −q 0 q 3 )+2z(q 0 q 2 +q 1 q 3 ) 

y ′ =2x(q 0 q 3 +q 1 q 2 )+y(q 2 0 −q 2 1 +q 2 2 −q 2 3 )+2z(−q 0 q 1 +q 2 q 3 ) 

z ′ =2x(−q 0 q 2 +q 1 q 3 )+2y(q 0 q 1 +q 2 q 3 )+z(q 2 0 −q 2 1 −q 2 2 +q 2 3 ) 

附录

以下代码推导对三维点[x,y,z] 
施加变换[q 0 ,q 1 ,q 2 ,q 3 ] 的结果。

clear;clc;close all;syms a1 a2 a3 a4 b1 b2 b3 b4syms q0 q1 q2 q3syms x y z% a: rotation, b: 3D vectora1 = q0;a2 = q1;a3 = q2;a4 = q3;b1 = 0;b2 = x;b3 = y;b4 = z;% c = a (*) bc1 = a1*b1-a2*b2-a3*b3-a4*b4;c2 = a1*b2+a2*b1+a3*b4-a4*b3;c3 = a1*b3-a2*b4+a3*b1+a4*b2;c4 = a1*b4+a2*b3-a3*b2+a4*b1;% d = conj(a)d1 = a1;d2 = -a2;d3 = -a3;d4 = -a4;% e = a (*) b (*) conj(a) = c (*) de1 = c1*d1-c2*d2-c3*d3-c4*d4;e2 = c1*d2+c2*d1+c3*d4-c4*d3;e3 = c1*d3-c2*d4+c3*d1+c4*d2;e4 = c1*d4+c2*d3-c3*d2+c4*d1;expand(e1)expand(e2)expand(e3)expand(e4)