旋转矩阵、欧拉角、四元数理论及其转换关系

author@jason_ql（lql0716）
http://blog.csdn.net/lql0716

1、概述

旋转矩阵、欧拉角、四元数主要用于表示坐标系中的旋转关系，它们之间的转换关系可以减小一些算法的复杂度。

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

2、原理

2.1 旋转矩阵

• 对于两个三维点 p1(x1,y1,z1) p 1 ( x 1 , y 1 , z 1 ) ， p2(x2,y2,z2) p 2 ( x 2 , y 2 , z 2 ) ，由点 p1 p 1 经过旋转矩阵 R R 旋转到 p2 p 2 ，则有：

R=⎡⎣⎢r11r21r31r12r22r32r13r23r33⎤⎦⎥ R = [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ]

⎡⎣⎢x2y2z2⎤⎦⎥=R⎡⎣⎢x1y1z1⎤⎦⎥ [ x 2 y 2 z 2 ] = R [ x 1 y 1 z 1 ]

注：旋转矩阵为正交矩阵 RRT=E R R T = E

• 绕x轴旋转：

Rx(θ)=⎡⎣⎢1000cosθsinθ0−sinθcosθ⎤⎦⎥ R x ( θ ) = [ 1 0 0 0 c o s θ − s i n θ 0 s i n θ c o s θ ]

• 绕y轴旋转：

Ry(θ)=⎡⎣⎢cosθ0−sinθ010sinθ0cosθ⎤⎦⎥ R y ( θ ) = [ c o s θ 0 s i n θ 0 1 0 − s i n θ 0 c o s θ ]

• 绕z轴旋转：

Rz(θ)=⎡⎣⎢cosθsinθ0−sinθcosθ0001⎤⎦⎥ R z ( θ ) = [ c o s θ − s i n θ 0 s i n θ c o s θ 0 0 0 1 ]

• 任意旋转矩阵：
  任何一个旋转可以表示为依次绕着三个旋转轴旋三个角度的组合。这三个角度称为欧拉角。
  三个轴可以指固定的世界坐标系轴，也可以指被旋转的物体坐标系的轴。三个旋转轴次序不同，会导致结果不同。

Rx(−θ1)Ry(−θ2)Rz(θ)Ry(θ2)Rx(θ1) R x ( − θ 1 ) R y ( − θ 2 ) R z ( θ ) R y ( θ 2 ) R x ( θ 1 )

2.2 欧拉角

• 欧拉角有两种：
  静态：即绕世界坐标系三个轴的旋转，由于物体旋转过程中坐标轴保持静止，所以称为静态。
  动态：即绕物体坐标系三个轴的旋转，由于物体旋转过程中坐标轴随着物体做相同的转动，所以称为动态。
  使用动态欧拉角会出现万向锁现象；静态欧拉角不存在万向锁的问题。

• 对于在三维空间里的一个参考系，任何坐标系的取向，都可以用三个欧拉角来表现。参考系又称为实验室参考系，是静止不动的。而坐标系则固定于刚体，随着刚体的旋转而旋转。

• 如图1，设定xyz-轴为参考系的参考轴。称xy-平面与XY-平面的相交为交点线，用英文字母（N）代表。zxz顺规的欧拉角可以静态地这样定义：

  α α 是x-轴与交点线的夹角，
  β β 是z-轴与Z-轴的夹角，
  γ γ 是交点线与X-轴的夹角。

• 图1：
  
  图中三个欧拉角分别为： (α,β,γ) ( α , β , γ ) ；
  蓝色的轴为： xyz轴 x y z 轴
  红色的轴为： XYZ X Y Z 轴
  绿色的线为交线： N N
  α∈[0,2π],β∈[0,π],γ∈[0,2π] α ∈ [ 0 , 2 π ] , β ∈ [ 0 , π ] , γ ∈ [ 0 , 2 π ]

• 很可惜地，对于夹角的顺序和标记，夹角的两个轴的指定，并没有任何常规。科学家对此从未达成共识。每当用到欧拉角时，我们必须明确的表示出夹角的顺序，指定其参考轴。

• 实际上，有许多方法可以设定两个坐标系的相对取向。欧拉角方法只是其中的一种。此外，不同的作者会用不同组合的欧拉角来描述，或用不同的名字表示同样的欧拉角。因此，使用欧拉角前，必须先做好明确的定义。

• 欧拉角的基本思想是将角位移分解为绕三个互相垂直轴的三个旋转组成的序列。所以，欧拉旋转的三个角，可以对应于三个旋转矩阵。
  Yaw（偏航）：欧拉角向量的y轴
  Pitch（俯仰）：欧拉角向量的x轴
  Roll（翻滚）： 欧拉角向量的z轴

• Unity3D中，欧拉旋转是按照ZYX的顺序旋转的。（不同的旋转顺序最终得到的结果是不一样的，要引起重视。给定一组欧拉角角度值，比如yaw=45度，pitch=30度，roll=60度，按照yaw-pitch-roll的顺序旋转和按照yaw-roll-pitch的顺序旋转，最终刚体的朝向是不同的！换言之，若刚体需要按照两种不同的旋转顺序旋转到相同的朝向，所需要的欧拉角角度值则是不同的！）

• 欧拉角的缺点：

  1、 欧拉角的表示方式不唯一。给定某个起始朝向和目标朝向，即使给定yaw、pitch、roll的顺序，也可以通过不同的yaw/pitch/roll的角度组合来表示所需的旋转。这其实主要是由于万向锁(Gimbal Lock)引起的；

  2、欧拉角的插值比较难；

  3、计算旋转变换时，一般需要转换成旋转矩阵，这时候需要计算很多sin, cos，计算量较大；

2.2.1 由欧拉角求旋转矩阵

设三个轴x，y，z的欧拉角分别为 θx,θy,θz θ x , θ y , θ z ，正弦值、余弦值分别为 sx,cx,sy,cy,sz,cz s x , c x , s y , c y , s z , c z 那么旋转矩阵为：

R(θx,θy,θz)=Rz(θz)Ry(θy)Rx(θx)=⎡⎣⎢cyczcysz−syczsxsy−cxszcxcz+sxsyszcysxsxsz+cxczsycxsysz−czszcxcy⎤⎦⎥ R ( θ x , θ y , θ z ) = R z ( θ z ) R y ( θ y ) R x ( θ x ) = [ c y c z c z s x s y − c x s z s x s z + c x c z s y c y s z c x c z + s x s y s z c x s y s z − c z s z − s y c y s x c x c y ]

2.2.2 由旋转矩阵求欧拉角

R=⎡⎣⎢r11r21r31r12r22r32r13r23r33⎤⎦⎥=⎡⎣⎢cyczcysz−syczsxsy−cxszcxcz+sxsyszcysxsxsz+cxczsycxsysz−czszcxcy⎤⎦⎥ R = [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ] = [ c y c z c z s x s y − c x s z s x s z + c x c z s y c y s z c x c z + s x s y s z c x s y s z − c z s z − s y c y s x c x c y ]

解方程可得：

θx=atan2(r32,r33) θ x = a t a n 2 ( r 32 , r 33 )

θy=atan2(−r31,r232+r233−−−−−−−√) θ y = a t a n 2 ( − r 31 , r 32 2 + r 33 2 )

θz=atan2(r21,r11) θ z = a t a n 2 ( r 21 , r 11 )

注：atan2()为C++中的函数，atan2(y，x) 求的是y/x的反正切，其返回值为[-pi,+pi]之间的一个数。

2.3 四元数

四元数（Quaternions）是由爱尔兰数学家哈密顿（William Rowan Hamilton）在1843年提出。

三维空间的任意旋转，都可以用绕三维空间的某个轴旋转过某个角度来表示，即所谓的Axis-Angle表示方法。这种表示方法里，Axis可用一个三维向量 (x,y,z) ( x , y , z ) 来表示， θ θ 可以用一个角度值来表示，直观来讲，一个四维向量 (θ,x,y,z) ( θ , x , y , z ) 就可以表示出三维空间任意的旋转。注意，这里的三维向量 (x,y,z) ( x , y , z ) 只是用来表示axis的方向朝向，因此更紧凑的表示方式是用一个单位向量来表示方向axis，而用该三维向量的长度来表示角度值 θ θ 。这样以来，可以用一个三维向量 (θ∗x,θ∗y,θ∗z) ( θ ∗ x , θ ∗ y , θ ∗ z ) 就可以表示出三维空间任意的旋转，前提是其中 (x,y,z) ( x , y , z ) 是单位向量。这就是旋转向量(Rotation Vector)的表示方式，OpenCV里大量使用的就是这种表示方法来表示旋转(见OpenCV相机标定部分的rvec)。

• 单位向量 (x,y,z) ( x , y , z ) 旋转 θ θ 角度后的四元数：

  (cosθ2,x∗sinθ2,y∗sinθ2,z∗sinθ2) ( c o s θ 2 , x ∗ s i n θ 2 , y ∗ s i n θ 2 , z ∗ s i n θ 2 )

• 对于三维坐标的旋转，可以通过四元数乘法直接操作，与旋转矩阵操作可以等价，但是表示方式更加紧凑，计算量也可以小一些。

2.3.1 四元数基本概念

• 四元数的复数定义：

  q=q0+q1i+q2j+q3k=[s,v] q = q 0 + q 1 i + q 2 j + q 3 k = [ s , v ]

  • 其中 q0,q1,q2,q3 q 0 , q 1 , q 2 , q 3 均为实数, s=q0,v=[q1,q2,q3] s = q 0 , v = [ q 1 , q 2 , q 3 ] ， i2=j2=k2=−1 i 2 = j 2 = k 2 = − 1
  • 对于 i,j,k i , j , k 本身的几何意义可以理解为一种旋转，其中 i i 代表 x x 轴与 y y 轴相交平面中 x x 轴正向向 y y 轴正向的旋转， j j 旋转代表 z z 轴与 x x 轴相交平面中 z z 轴正向向 x x 轴正向的旋转， k k 旋转代表 y y 轴与 z z 轴相交平面中 y y 轴正向向 z z 轴正向的旋转， −i,−j,−k − i , − j , − k 分别代表 i,j,k i , j , k 的反向旋转。

• 四元数的模

  |q|=q20+q21+q22+q23−−−−−−−−−−−−−−√ | q | = q 0 2 + q 1 2 + q 2 2 + q 3 2

• 四元数的优点：

  • 非奇异表达（和欧拉角之类的表示相比）
  • 比矩阵更紧凑（更快速）
  • 单位四元数的对可以表示四维空间中的一个旋转

• 四元数与群

  所有单位四元数的集合组成一个三维球S3和在乘法下的一个群（李群）。S3是行列式为1的实正交3×3正交矩阵的群SO(3,R)的双面覆盖，因为每两个单位四元数通过上述关系对应于一个转动。群S3和SU(2)同构，SU(2)是行列式为1的复酉2×2矩阵的群。

  设 A={ q=q0+q1i+q2j+q3k|q0,q1,q2,q3均为实数} A = { q = q 0 + q 1 i + q 2 j + q 3 k | q 0 , q 1 , q 2 , q 3 均 为 实 数 } ，则 A A 是一个环，并且是一个格， A A 中存在24个四元数，而它们是施莱夫利符号为 { 3,4,3} { 3 , 4 , 3 } 的正二十四胞体的顶点。

• 四元数的运算：

  p1=[s1,v1]=(a1b1c1d1)T p 1 = [ s 1 , v 1 ] = ( a 1 b 1 c 1 d 1 ) T

  p2=[s2,v2]=(a2b2c2d2)T p 2 = [ s 2 , v 2 ] = ( a 2 b 2 c 2 d 2 ) T

  • 四元数的加法

  p1+p2=s1+s2+v1+v2=(a1+a2)+(b1+b2)i+(c1+c2)j+(d1+d2)k p 1 + p 2 = s 1 + s 2 + v 1 + v 2 = ( a 1 + a 2 ) + ( b 1 + b 2 ) i + ( c 1 + c 2 ) j + ( d 1 + d 2 ) k

  • 四元数的乘法

  p1p2=s1s2−v1v2+s1v2+s2v1+v1×v2 p 1 p 2 = s 1 s 2 − v 1 v 2 + s 1 v 2 + s 2 v 1 + v 1 × v 2

  
  p1p2=⎛⎝⎜⎜⎜a1a2−b1b2−c1c2−d1d2a1b2+b1a2+c1d2−d1c2a1c2−b1d2+c1a2+d1b2a1d2+b1c2−c1b2+d1a2⎞⎠⎟⎟⎟ p 1 p 2 = ( a 1 a 2 − b 1 b 2 − c 1 c 2 − d 1 d 2 a 1 b 2 + b 1 a 2 + c 1 d 2 − d 1 c 2 a 1 c 2 − b 1 d 2 + c 1 a 2 + d 1 b 2 a 1 d 2 + b 1 c 2 − c 1 b 2 + d 1 a 2 )

  注：四元数不满足交换律，故 p1p2≠p2p1 p 1 p 2 ≠ p 2 p 1 ，四元数的乘法不等同于外积。

  • 四元数的点积（内积）
    点积也叫欧几里得内积，四元数的点积等同于一个四维向量的点积。点积的值是 p1,p2 p 1 , p 2 中相应元素的数值的乘积的和。

  p1⋅p2=s1s2+v1⋅v2=a1a2+b1b2+c1c2+d1d2 p 1 ⋅ p 2 = s 1 s 2 + v 1 ⋅ v 2 = a 1 a 2 + b 1 b 2 + c 1 c 2 + d 1 d 2

  • 四元数的外积： Outer(p1,p2) O u t e r ( p 1 , p 2 )

  • 四元数的偶积： Even(p1,p2) E v e n ( p 1 , p 2 )

  • 四元数的叉积： p1×p2 p 1 × p 2

  p1×p2=p1p2−p2p22 p 1 × p 2 = p 1 p 2 − p 2 p 2 2

  p1×p2=v1×v2 p 1 × p 2 = v 1 × v 2

  p1×p2=(c1d2−c2d1)i+(b1d2−b2d1)j+(b1c2−b2c1)k p 1 × p 2 = ( c 1 d 2 − c 2 d 1 ) i + ( b 1 d 2 − b 2 d 1 ) j + ( b 1 c 2 − b 2 c 1 ) k

  • 四元数的共轭 q∗ q ∗

  q∗=s−v=[s,−v] q ∗ = s − v = [ s , − v ]

  qq∗=q20+q21+q22+q23 q q ∗ = q 0 2 + q 1 2 + q 2 2 + q 3 2

  • 四元数的逆 q−1 q − 1

  qq−1=q−1q=1 q q − 1 = q − 1 q = 1

  q−1=q∗||q||2 q − 1 = q ∗ | | q | | 2

  • 单位四元数
    单位四元数的共轭和逆相等 q−1=q∗ q − 1 = q ∗

• 四元数乘法的性质：

  1、满足结合律
  2、不满足交换律
  3、乘积的模等于模的乘积
  4、乘积的逆等于各个四元数的逆以相反的顺序相乘

2.3.2 四元数求旋转矩阵

• 方法1：

  • 已知四元数
    

    q=q0+q1i+q2j+q3k=[s,v] q = q 0 + q 1 i + q 2 j + q 3 k = [ s , v ]

  • 利用Rodrigues公式可以由四元数求得旋转矩阵 R R :
    

    R=⎡⎣⎢⎢1−2q22−2q232q1q2+2q0q32q1q3−2q0q22q1q2−2q0q31−2q21−2q232q2q3+2q0q12q1q3+2q0q22q2q3−2q0q11−2q21−2q22⎤⎦⎥⎥ R = [ 1 − 2 q 2 2 − 2 q 3 2 2 q 1 q 2 − 2 q 0 q 3 2 q 1 q 3 + 2 q 0 q 2 2 q 1 q 2 + 2 q 0 q 3 1 − 2 q 1 2 − 2 q 3 2 2 q 2 q 3 − 2 q 0 q 1 2 q 1 q 3 − 2 q 0 q 2 2 q 2 q 3 + 2 q 0 q 1 1 − 2 q 1 2 − 2 q 2 2 ]

• 方法2：

• 亦可以将四元数转化为轴角 θ θ 与向量 (x,y,z) ( x , y , z ) ，然后再计算旋转矩阵（这样需要多计算一个arccos，不划算）
  

  q=(θxyz)T q = ( θ x y z ) T

• 利用Rodrigues公式可以由单位向量 ω=(x,y,z) ω = ( x , y , z ) 旋转 θ θ 角度后的四元数求得旋转矩阵 R R :

R=eωθ=I+ω×sinθ+ω2×(1−cosθ)=I+⎡⎣⎢0z−y−z0xy−x0⎤⎦⎥sinθ+⎡⎣⎢−y2−z2xyxzxy−x2−z2yzxzyz−x2−y2⎤⎦⎥(1−cosθ)=⎡⎣⎢1zsinθ−ysinθ−zsinθ1xsinθysinθ−xsinθ1⎤⎦⎥+⎡⎣⎢−y2−z2xyxzxy−x2−z2yzxzyz−x2−y2⎤⎦⎥(1−cosθ)=⎡⎣⎢1−(y2+z2)(1−cosθ)zsinθ+xy(1−cosθ)−ysinθ+xz(1−cosθ)−zsinθ+xy(1−cosθ)1−(x2+z2)(1−cosθ)xsinθ+yz(1−cosθ)ysinθ+xz(1−cosθ)−xsinθ+yz(1−cosθ)1−(x2+y2)(1−cosθ)⎤⎦⎥=⎡⎣⎢cosθ+x2(1−cosθ)zsinθ+xy(1−cosθ)−ysinθ+xz(1−cosθ)−zsinθ+xy(1−cosθ)cosθ+y2(1−cosθ)xsinθ+yz(1−cosθ)ysinθ+xz(1−cosθ)−xsinθ+yz(1−cosθ)cosθ+z2(1−cosθ)⎤⎦⎥ R = e ω θ = I + ω × s i n θ + ω × 2 ( 1 − c o s θ ) = I + [ 0 − z y z 0 − x − y x 0 ] s i n θ + [ − y 2 − z 2 x y x z x y − x 2 − z 2 y z x z y z − x 2 − y 2 ] ( 1 − c o s θ ) = [ 1 − z s i n θ y s i n θ z s i n θ 1 − x s i n θ − y s i n θ x s i n θ 1 ] + [ − y 2 − z 2 x y x z x y − x 2 − z 2 y z x z y z − x 2 − y 2 ] ( 1 − c o s θ ) = [ 1 − ( y 2 + z 2 ) ( 1 − c o s θ ) − z s i n θ + x y ( 1 − c o s θ ) y s i n θ + x z ( 1 − c o s θ ) z s i n θ + x y ( 1 − c o s θ ) 1 − ( x 2 + z 2 ) ( 1 − c o s θ ) − x s i n θ + y z ( 1 − c o s θ ) − y s i n θ + x z ( 1 − c o s θ ) x s i n θ + y z ( 1 − c o s θ ) 1 − ( x 2 + y 2 ) ( 1 − c o s θ ) ] = [ c o s θ + x 2 ( 1 − c o s θ ) − z s i n θ + x y ( 1 − c o s θ ) y s i n θ + x z ( 1 − c o s θ ) z s i n θ + x y ( 1 − c o s θ ) c o s θ + y 2 ( 1 − c o s θ ) − x s i n θ + y z ( 1 − c o s θ ) − y s i n θ + x z ( 1 − c o s θ ) x s i n θ + y z ( 1 − c o s θ ) c o s θ + z 2 ( 1 − c o s θ ) ]

R=⎡⎣⎢cosθ+x2(1−cosθ)zsinθ+xy(1−cosθ)−ysinθ+xz(1−cosθ)−zsinθ+xy(1−cosθ)cosθ+y2(1−cosθ)xsinθ+yz(1−cosθ)ysinθ+xz(1−cosθ)−xsinθ+yz(1−cosθ)cosθ+z2(1−cosθ)⎤⎦⎥ R = [ c o s θ + x 2 ( 1 − c o s θ ) − z s i n θ + x y ( 1 − c o s θ ) y s i n θ + x z ( 1 − c o s θ ) z s i n θ + x y ( 1 − c o s θ ) c o s θ + y 2 ( 1 − c o s θ ) − x s i n θ + y z ( 1 − c o s θ ) − y s i n θ + x z ( 1 − c o s θ ) x s i n θ + y z ( 1 − c o s θ ) c o s θ + z 2 ( 1 − c o s θ ) ]

2.3.3 旋转矩阵求四元数

• 情况1：

  q0=1+r11+r22+r33−−−−−−−−−−−−−−√2 q 0 = 1 + r 11 + r 22 + r 33 2

  q1=r32−r234q0 q 1 = r 32 − r 23 4 q 0

  q2=r13−r314q0 q 2 = r 13 − r 31 4 q 0

  q3=r21−r124q0 q 3 = r 21 − r 12 4 q 0

  其中要满足 q0≠0 q 0 ≠ 0 ， 1+r11+r22+r33>0 1 + r 11 + r 22 + r 33 > 0 ，即 1+tr(R)>0 1 + t r ( R ) > 0

• 情况2：
  如果 q0 q 0 趋近于0， tr(R) t r ( R ) 趋近于-1,则求解四元数的过程为：

  • 2.1 如果 max{ r11,r22,r33}=r11 m a x { r 11 , r 22 , r 33 } = r 11

    t=1+r11−r22−r33−−−−−−−−−−−−−−−√ t = 1 + r 1 1 − r 22 − r 33

    q0=r32−r23t q 0 = r 32 − r 23 t

    q1=t/4 q 1 = t / 4

    q2=r13+r31t q 2 = r 13 + r 31 t

    q3=r12+r21t q 3 = r 12 + r 21 t

  • 2.2 如果 max{ r11,r22,r33}=r22 m a x { r 11 , r 22 , r 33 } = r 22

    t=1−r11+r22−r33−−−−−−−−−−−−−−−√ t = 1 − r 1 1 + r 22 − r 33

    q0=r13−r31t q 0 = r 13 − r 31 t

    q1=r12+r21t q 1 = r 12 + r 21 t

    q2=t/4 q 2 = t / 4

    q3=r32+r23t q 3 = r 32 + r 23 t

  • 2.3 如果 max{ r11,r22,r33}=r33 m a x { r 11 , r 22 , r 33 } = r 33

    t=1−r11−r22+r33−−−−−−−−−−−−−−−√ t = 1 − r 1 1 − r 22 + r 33

    q0=r21−r12t q 0 = r 21 − r 12 t

    q1=r13+r31t q 1 = r 13 + r 31 t

    q2=r23−r32t q 2 = r 23 − r 32 t

    q3=t/4 q 3 = t / 4