3D中的方位与角位移

摘要：本文讲述了3D中描述方位与角位移的方法：矩阵、欧拉角和四元数，以及它们优缺点和适用场景。给理解Gimbal Lock提供了一种新视角。

1. 简介

“方位”是指物体的朝向，是一个状态；“旋转”可以把物体从一个方位变到另一个方位，是一个动作；“角位移”是旋转的量。
方位和角位移的关系类似于“位置”和“位移”的关系。
物体的位置是不能用绝对坐标来描述的，我们必须将其放置在一个坐标系内，通过描述物体相对于参考点的位移来描述物体的位置。例如，位于（100，0，20）的点，表示的是跟原点（0，0，0）的位移是（100，0，20）的位置。
类似的，方位也是通过相对已知方位的角位移来描述的。

描述角位移有3种最常用的方法：矩阵、欧拉角和四元数。下面逐一讲述。

2. 矩阵

矩阵有个非常好的几何解释：矩阵的列表示变换后的基向量位置。因此，如果你知道新坐标系的基向量，把它转换成3*3的矩阵，就得到了用矩阵表示的角位移。

2.1 矩阵形式的优点

• 可以立即进行向量的旋转：进行矩阵向量乘法$Ax$Ax，就能得到变换后向量$x$x的位置。后面会介绍，欧拉角没有乘法操作，必须转换成矩阵才能旋转向量。
• 矩阵的形式被图形API所使用
• 多个角位移连接很方便：如果知道A关于B的方位M1，B关于C的方位M2，那么A关于C的方位就是M1*M2。
• 矩阵的逆好求：逆矩阵表示“反”角位移。由于旋转矩阵是正交矩阵，矩阵的逆就是矩阵的转置$A^{-1}=A^T$A−1=AT，计算简单。

2.2 矩阵形式的缺点

• 矩阵占用了更多的内存：矩阵形式用9个元素表示角位移；后面会介绍，欧拉角只用3个元素，四元数用4个元素。
• 难于使用：矩阵形式对计算机是友好的，因为无需转换，直接做矩阵向量乘法即可；但是对人类并不直观，普通人没有办法直接从矩阵形式推断出旋转是如何发生的。
• 矩阵可能是病态的：不是所有矩阵都能表示角位移。矩阵使用9个数，其实只有3个数是必须的。也就是说，矩阵有6个自由度的冗余，也就需要满足6个约束，即，列向量是单位向量且互相垂直。不满足这6个约束的就是病态矩阵。例如，由于浮点精度的限制，大量的矩阵乘法操作可能导致病态矩阵，这种现象称作“矩阵蠕变”。

3. 欧拉角

欧拉（1707~1783）证明了角位移序列等价于单个角位移，即，角位移序列可以描述任意旋转，由此诞生了欧拉角。
欧拉角的基本思想是将角位移分解为绕三个互相垂直轴的三个旋转组成的序列。绕哪三个轴？按什么顺序？答案是，任意三个轴，任意顺序（同一个轴不能连续出现，比如x-x-y不行，x-y-x可以）都可以，但最有意义的是使用笛卡尔坐标系并按一定顺序组成的旋转序列。例如，roll-pitch-yaw。

图1 欧拉角示意[1]

• 它的基本思想是让物体开始于“标准”方位——就是物体坐标轴（roll-pitch-yaw，局部坐标系）和惯性坐标轴（x-y-z，世界坐标系）对齐，让物体做roll、pitch、yaw旋转，最后物体到达我们想要描述的方位。

• 如上图所示，开始时，roll与世界坐标系的x轴对齐，pitch与y轴对齐，yaw与z轴对齐。在做roll时，其实就是绕x轴旋转；但是在做pitch时，并不是绕y轴旋转，因为此时的pitch轴已经不与y轴对齐了；同理，做yaw时，是绕经历了roll和pitch变化后的yaw轴旋转。

• 如果使用惯性坐标轴的旋转序列来表示角位移，就是Fixed Angle。例如，使用x-y-z的顺序，Fixed Angle (10, 20, 30）就表示，先绕x轴旋转10度；再绕y轴旋转20度（注意，y轴没有发生变化）；再绕z轴旋转30度。 后面会证明，欧拉角表示的角位移等价于相反旋转顺序表示的角位移。

3.1 三种角度理解Gimbal Lock

众所周知，欧拉角最严重的问题是Gimbal Lock（万向锁），它是一个底层问题，至今没有简单的解决方案。下面分别从计算上和几何上来理解Gimbal Lock。

引理1：欧拉角表示的角位移等价于相反旋转顺序Fixed Angle表示的角位移。例如，x-y-z顺序的Fixed Angle等价于z-y-x顺序的欧拉角。
证：
1）设x-y-z顺序的Fixed Angle为$(\theta_x,\theta_y,\theta_z)$(θx​,θy​,θz​)，则该角位移的旋转矩阵为$R_z(\theta_z)\cdot R_y(\theta_y)\cdot R_x(\theta_x).$Rz​(θz​)⋅Ry​(θy​)⋅Rx​(θx​). 其中，$R_x, R_y, R_z$Rx​,Ry​,Rz​分别表示绕惯性坐标系的x轴，y轴，z轴的旋转矩阵。
2）另一方面，设z-y-x顺序的欧拉角为$(\theta_z,\theta_y,\theta_x)$(θz​,θy​,θx​)。
a) 首先绕物体坐标系的z轴旋转$\theta_z$θz​，此时物体坐标系的z轴和惯性坐标系重合，因此旋转矩阵为$R_z(\theta_z)$Rz​(θz​)；
b) 然后绕物体坐标系的y轴旋转$\theta_y$θy​，此时物体坐标系的y轴不再和惯性坐标系的y轴重合，也就是说此时的旋转矩阵不再是$R_y(\theta_y)$Ry​(θy​)。换一种角度考虑：a)使得物体坐标系的y轴绕惯性坐标系的z轴旋转了$R_z(\theta_z)$Rz​(θz​)，我们首先做个逆操作，把物体坐标系的y轴变回到初始位置，即让它与世界坐标系的y轴对齐；然后再绕此时的y轴旋转$\theta_y$θy​；紧接着，再把物体坐标系的y轴变回来。即，$R_z(\theta_z)\cdot R_y(\theta_y)\cdot R'_z(\theta_z)$Rz​(θz​)⋅Ry​(θy​)⋅Rz′​(θz​)（注：旋转矩阵的逆等于转置）。
因此，经历了a) b)的旋转矩阵为：$R_z(\theta_z)\cdot R_y(\theta_y)\cdot R'_z(\theta_z)\cdot R_z(\theta_z)=R_z(\theta_z)\cdot R_y(\theta_y)$Rz​(θz​)⋅Ry​(θy​)⋅Rz′​(θz​)⋅Rz​(θz​)=Rz​(θz​)⋅Ry​(θy​)
c) 最后绕物体坐标系的x轴旋转$\theta_x$θx​，按照上面的思路可得，最终的旋转矩阵为，$R_z(\theta_z)\cdot R_y(\theta_y)\cdot R_x(\theta_x).$Rz​(θz​)⋅Ry​(θy​)⋅Rx​(θx​).
证毕。

根据引理1，欧拉角和Fixed Angle是等价的。之所以证明这个结论，是因为从Fixed Angle的角度来理解Gimbal Lock会更直观，因为不需要去想坐标轴的变化。

3.1.1 计算的角度理解Gimbal Lock

设x-y-z顺序的Fixed Angle为$(\theta_x,\theta_y,\theta_z)$(θx​,θy​,θz​)，则该角位移的旋转矩阵为$R_z(\theta_z)\cdot R_y(\theta_y)\cdot R_x(\theta_x).$Rz​(θz​)⋅Ry​(θy​)⋅Rx​(θx​). 其中，$R_x, R_y, R_z$Rx​,Ry​,Rz​分别表示绕惯性坐标系的x轴，y轴，z轴的旋转矩阵。即，
$\left[ \begin{matrix} 1 & 0 & 0 \\ 0 & cos\theta_x & -sin\theta_x \\ 0 & sin\theta_x & cos\theta_x \end{matrix} \right] \cdot \left[ \begin{matrix} cos\theta_y & 0 & sin\theta_y \\ 0 & 1 & 0 \\ -sin\theta_y & 0 & cos\theta_y \end{matrix} \right]\cdot \left[ \begin{matrix} cos\theta_z & -sin\theta_z & 0 \\ sin\theta_z & cos\theta_z & 0 \\ 0 & 0 & 1 \end{matrix} \right] \tag{Matrix of Fixed Angle}$⎣⎡​100​0cosθx​sinθx​​0−sinθx​cosθx​​⎦⎤​⋅⎣⎡​cosθy​0−sinθy​​010​sinθy​0cosθy​​⎦⎤​⋅⎣⎡​cosθz​sinθz​0​−sinθz​cosθz​0​001​⎦⎤​(Matrix of Fixed Angle)

当$\theta_y=\frac{\pi}{2}$θy​=2π​时，上式可化简为：

$\left[ \begin{matrix} 0 & 0 & 1 \\ sin(\theta_x+\theta_z) & cos(\theta_x+\theta_z) & 0 \\ -cos(\theta_x+\theta_z) & sin(\theta_x+\theta_z) & 0 \end{matrix} \right]\tag{Matrix of Gimbal Lock}$⎣⎡​0sin(θx​+θz​)−cos(θx​+θz​)​0cos(θx​+θz​)sin(θx​+θz​)​100​⎦⎤​(Matrix of Gimbal Lock)

可以看出，$\theta_x$θx​变化$\Delta \theta$Δθ和$\theta_z$θz​变化$\Delta \theta$Δθ的作用是一样的，即，$\theta_x$θx​和$\theta_z$θz​两个自由度，变成了一个自由度。这种现象就叫做Gimbal Lock。

3.1.2 几何角度理解Gimbal Lock: Fixed Angle

用Fixed Angle表示角位移，旋转轴始终是不变的，一直是惯性坐标系。设所用的Fixed Angle为x-y-z顺序。飞机的初始位置如下图1所示（为方便阅读，图1 拷贝如下），即，头尾方向与x轴对齐。

图1（复制）

此时绕x轴的旋转就是roll。绕y轴-90度后，如图2所示，飞机的头尾和z轴对齐（惯性坐标系的xyz轴没有发生变化）,此时绕z轴旋转，仍然相当于做roll。也就是说，当第二个旋转轴y的旋转角度是90度时，第一个旋转轴x和第三个旋转中z做了同样的事情。我们真正想要的yaw效果做不了了。

图2 y轴旋转90度后

3.1.3 几何角度理解Gimbal Lock: Euler Angle

用Euler Angle表示角位移使用的是物体坐标系，是不断变化的，因此理解起来不是很直观。目前还没有想到更好的方法来从这个角度理解Gimbal Lock。

3.1.4 总结

1. 为什么会出现Gimbal Lock？

• Fixed Angle/Euler Angle的这种表示角位移的方式：有顺序的绕三个坐标轴旋转，就决定了，当第二个轴的旋转角度为90度时，第一个轴和第三个轴的作用一致，从而丢失掉一个自由度，导致某些姿势无法达到。这里“不能到达”的意思是，不能经过“第二个旋转轴为90度”这条路径达到。实际上，你可通过另一个欧拉角（第二个角度不等于90度）来达到那些所谓“不能达到”的姿势。
• 再换个角度理解一下，本来欧拉角有3个自由度，限制某个角度为固定值，还剩下2个自由度。然而不幸的是，如果限制第二个角度为90度，那么只剩下1个自由度。这就是Gimbal Lock的实质。

2. Gimbal Lock会导致什么问题？
   比如，追踪问题。
   假设有个相机使用欧拉角roll-pitch-yaw 追踪一个物体。当pitch不断变化，直到等于90度的那一刻，相机失去了一个自由度，也就是有些方位无法达到。此时，如果物体朝那些无法达到的方位移动，相机就跟踪不到物体了。参考文献[5]有这个例子更详细的描述。

3.2 欧拉角的优点

• 欧拉角对人类来说很容易使用：欧拉角中的数都是角度，符合人们思考方位的方式。
• 最简洁的表达方式：在3D中，表达方位不能少于3个数。如果要考虑内存问题，欧拉角是最合适的描述方位的方法。
• 任意三个数都是合法的：如前所述，矩阵必须满足6个条件才能表示方位；后面会讲到，四元数也需要满足一定条件才能表示方位。

3.3 欧拉角的缺点

• 给定方位的表达方式不唯一：对于给定的方位，存在多个欧拉角可以描述它。
  不唯一的问题有两个原因：1）加上360度并不会改变方位；
  2）三个角度不互相独立。例如，pitch 135度等价于yaw 180度，pitch 45度，再roll 180度。为了保证任意方位都有独一无二的表示，必须限制角度范围，例如，roll和yaw限制在[-180, 180]，pitch限制在[-90, 90]。
  不幸的是，这仍然不能彻底解决唯一性问题，原因在于前文论述的Gimbal Lock：当第二个角度为90度时，第一个和第二个角度等价，即，变化第一个角度跟变化第二个角度得到的方位是一样的。

• 两个角度间插值非常困难：例如，方位A的yaw是720度；方位B的yaw是0度，那么简单的插值会绕720度。而实际上720读的yaw等价于0度，也就是说AB的插值为0度。解决这个问题的方法仍然是将欧拉角限制在合理范围。

4. 四元数

一个四元数包含一个标量分量和一个3D向量分量。通常标量分量记为$w$w，向量分量记为 $\vec{v}$v，或者分开的$x,y,z$x,y,z：
$[w,\vec{v}],\quad or \quad [w, (x,y,z)]$[w,v],or[w,(x,y,z)]

4.1 四元数和“轴–角对”

3D中的任意角位移都能表示为绕单一轴的单一旋转，即“轴–角对”。设$\vec{n}$n为旋转轴，$\theta$θ为绕旋转轴的旋转量。那么轴–角对$(\vec{n},\theta)$(n,θ)就定义了一个角位移：绕$\vec{n}$n指定的轴旋转$\theta$θ角。而表示该角位移的四元数为：
$q=[cos(\theta/2),\quad sin(\theta/2)\cdot\vec{n}] =[cos(\theta/2),\quad (sin(\theta/2)\cdot n_x, sin(\theta/2)\cdot n_y,sin(\theta/2)\cdot n_z,)]$q=[cos(θ/2),sin(θ/2)⋅n]=[cos(θ/2),(sin(θ/2)⋅nx​,sin(θ/2)⋅ny​,sin(θ/2)⋅nz​,)]

4.2 四元数的运算

1. 负四元数
   $-q=-[w, \quad (x, y, z)] = [-w, \quad(-x, -y, -z)]$−q=−[w,(x,y,z)]=[−w,(−x,−y,−z)]
   $q$q和$-q$−q代表的角位移是相同的。几何解释：绕反方向的轴旋转负的角度与绕正方向的轴旋转正的角度等价。

2. 单位四元数
   $[1,\quad \vec{0}]$[1,0]表示单位四元数。

3. 四元数的模
   $||q||=\sqrt{w^2+x^2+y^2+z^2}$∣∣q∣∣=w2+x2+y2+z2​

4. 四元数的共轭和逆
   四元数的共轭$q^*=[w, \quad -\vec{v}]=[w,\quad (-x, -y, -z)]$q∗=[w,−v]=[w,(−x,−y,−z)]
   四元数的逆$q^{-1}=\frac{q^*}{||q||}$q−1=∣∣q∣∣q∗​

5. 四元数的叉乘
   四元数的叉乘
   $q_1\times q_2=[w_1\quad \vec{v_1}]\times [w_2 \quad \vec{v_2}]=[w_1w_2-\vec{v_1}\cdot \vec{v_2}, \quad w_1\vec{v_1}+w_2\vec{v_2}+\vec{v_1}\times \vec{v_2}]$q1​×q2​=[w1​v1​​]×[w2​v2​​]=[w1​w2​−v1​​⋅v2​​,w1​v1​​+w2​v2​​+v1​​×v2​​]

6. 四元数旋转向量
   3D中的点或向量$(x,y,z)$(x,y,z)在四元数空间的表示为$p=[0, \quad (x,y,z)]$p=[0,(x,y,z)]，用四元数代表的角位移旋转$p$p的公式为：
   $p'=q^{-1}pq$p′=q−1pq
   其中，$q$q是单位四元数。
   对于多个四元数连续变换的情况，例如先进行a旋转，再进行b旋转，其公式为：
   $p'=b^{-1}(a^{-1}pa)b=(ab)^{-1}p(ab)$p′=b−1(a−1pa)b=(ab)−1p(ab)

7. 四元数的“差”
   四元数的“差”是指一个方位到另一个方位的角位移。给定角位移a,b，从a旋转到b的角位移为d，则，$ad=b$ad=b。两边同乘以$a^{-1}$a−1可得
   $d = a^{-1}b$d=a−1b

8. 四元数的点乘
   四元数点乘
   $q_1\cdot q_2=[w_1, \quad \vec{v_1}] \cdot [w_2, \quad \vec{v_2}]=w_1w_2+\vec{v_1}\cdot \vec{v_2}$q1​⋅q2​=[w1​,v1​​]⋅[w2​,v2​​]=w1​w2​+v1​​⋅v2​​
   四元数的点乘越大，$q_1$q1​和$q_2$q2​代表的角位移越相似。

9. 四元数的对数、指数和标量乘
   记与轴–角对$(\vec{n},\quad 2*\alpha)$(n,2∗α)对应的四元数$q=[cos\alpha \quad \vec{n}sin\alpha]$q=[cosαnsinα]
   则$q$q的对数
   $logq=log([cos\alpha \quad \vec{n}sin\alpha])=[0 \quad \alpha \vec{n}]$logq=log([cosαnsinα])=[0αn]
   四元数的指数则以严格相反的方式定义。首先，设四元数$p=[0, \quad \alpha \vec{n}]$p=[0,αn]，$\vec{n}$n为单位向量，则$q$q的指数
   $exp p=exp([0\quad \alpha \vec{n}])=[cos\alpha \quad \vec{n}sin\alpha]$expp=exp([0αn])=[cosαnsinα]
   与标量形式类似，四元数的指数运算和对数运算互逆：
   $exp(logq)=q$exp(logq)=q
   四元数与标量相差就是每个分量都乘以这个标量：
   $kq=k[w,\quad \vec{v}]=[kw,\quad k\vec{v}]$kq=k[w,v]=[kw,kv]

10. 四元数求幂
    当t从0变到1时，$q^t$qt从[1,0]变到$q.$q. 例如，四元数$q$q表示一个角位移，现在想要得到代表$1/3$1/3这个角位移的四元数，可以这样计算：$q^{1/3}.$q1/3. 同时，指数超出[0, 1]的含义和预期一样，$q^2$q2代表的角位移是$q$q的2倍。
    四元数的幂
    $q^t=exp(t\cdot logq)$qt=exp(t⋅logq)

11. 四元数插值“slerp”
    slerp的基本思想是沿着4D球面（四元数是4维的）上链接两个四元数的弧插值。考虑2D中圆的弧插值很容易得到下面的公式：
    $slerp(q_0,q_1,t)=\frac{sin(1-t)w}{sinw}\cdot q_0+\frac{sin(tw)}{sinw}\cdot q_1$slerp(q0​,q1​,t)=sinwsin(1−t)w​⋅q0​+sinwsin(tw)​⋅q1​
    上面的公式需要注意两个问题：

• 四元数$q$q和$-q$−q代表相同的方位，但它们作为slerp的参数时可能导致不一样的结果，解决方法是选择$q_0$q0​和$q_1$q1​的符号，使得点乘$q_0\cdot q_1$q0​⋅q1​的结果是非负。
• 如果$q_0$q0​和$q_1$q1​非常接近，$sinw$sinw会非常小，这是除法可能出现问题。解决方法是，当$sinw$sinw很小时，使用线性插值。

4.3 四元数的优点

• 平滑插值：只有四元数能提供方位角的平滑插值
• 快速连接和角位移求逆：四元数叉乘能将角位移序列转换为单个角位移，用矩阵做同样的操作明显会慢一些。四元数共轭提供了一种有效计算反角位移的方法，通过旋转矩阵转置也能达到同样的目的，但不如四元数来得容易。
• 能和矩阵形式快速转换：四元数和矩阵间的转换比欧拉角与矩阵间的转换稍微快一点。
• 仅用四个数：相比于矩阵的9个数，可以节省很多空间。

4.4 四元数的缺点

• 比欧拉角稍微大一些：欧拉角使用3个数，因此四元数多占33%的存储空间
• 四元数可能不合法：坏的输入数据或浮点数舍入误差积累都可能使四元数不合法，需要通过四元数标准化，确保四元数为单位大小。
• 难于使用：在所有三种形式中，四元数是最难于直接使用的。

5. 各方法比较

5.1 三种表示方法比较

图3 矩阵、欧拉角、四元数的比较[2]

5.2 使用建议

• 欧拉角最容易使用。当需要为虚拟世界中的物体指定方位时，欧拉角能大大简化人机交互。
• 如果需要在坐标系之间转换向量，那么就选择矩阵形式。或者，用欧拉角作为方位的“主拷贝”，但同时维护一个旋转矩阵，当欧拉角发生改变时矩阵也要同时进行更新。
• 当需要大量保存方位数据（如动画）时，就使用欧拉角或四元数。欧拉角比四元数少占用25%的内存，但它在转换到矩阵时要稍微慢一些。例如，计算机动画领域最流行的运动数据库CMU Motion Cpature Data [3]，就使用欧拉角存储每一帧动画的角位移。
• 平滑的插值只能用四元数完成。如果使用矩阵或欧拉角，需要先将其转换到四元数，插值完毕后再转换回原格式。

6. 总结

本文介绍了方位角的三种表示方法及其优缺点和适用领域，同时详细介绍了Gimbal Lock。

7. 参考文献

• [1] Visualizing Quaternions with Unity
• [2] 3D数学基础：图形与游戏开发 Fletcher Dunn & Ian Parberry （美）著 清华大学出版社
• [3] CMU graphics lab motion capture database.
• [4] ZJU 计算机动画
• [5] 有关万向节死锁（Gimbal Lock）的问题 防止网页丢失，转录于此。

通常来说，一个物体的取向用欧拉角来表示是简单有效的。但是在某种特殊的情况下，欧拉角将失效，形成所谓的“万向节死锁”。
一个简单直观的例子是炮塔模型。假设地面上的一个炮塔有两个旋转轴：Y垂直于地面，使炮塔可以平行地面360度旋转（正北设为0度）；X平行于地面，使炮口可以绕着它上下90度旋转（平行地面使设为0度）。现在，天空中的任意一点就可以使用两个坐标的度数来表示了！
这时，一架敌机从正东面飞来，我们转动炮塔对准它，目前的坐标是（10,90）。因为飞机飞行方向不变，所以Y固定为90，而X由于飞机距离的接近而增大。当飞机恰好飞到炮塔顶端时，即X的角度也达到90度时，飞机忽然向南飞行！我们必须立即改变炮塔朝向，否则即将都丢失目标。但是我们发现，无论是转动X轴还是Y轴，我们都无法让炮塔转向南方了，即炮塔在此时丢失了一个自由度！
为什么不把Y转动到180度的位置继续跟踪呢？注意此时炮塔的状态: 炮口已经对准炮台的正上方, 平行于Y轴，无论Y轴怎样转动，炮口都改变不了朝向的, 即炮台的物体坐标系不会变化了。能转动的只有X轴，但是这样一来，炮口又回到东面了。
欧拉角的万向节死锁就是这样：我们依次绕物体坐标系的X轴、Y轴、Z轴旋转，当Y轴旋转了90度之后，Z就会指向原来的X轴。这样一来，我们事实上只绕了X轴和Y轴两个轴旋转，第三根轴的自由度就丢失了！(值得指出的是，我们在描述的时候使用的是世界坐标系的x, y, z轴，但是旋转的时候却是使用绕物体的)
万向锁的一个有趣实验是当先把飞机Yaw 45度，再Pitch 90度，与先Pitch 90度，再Roll 45度的结果完全相同。 这个现象也叫欧拉角的别名现象（同一个惯性系下同一个值有不同的表示方法），这也是很糟糕的。