四元数与欧拉角（RPY角）的相互转换

RPY角与Z-Y-X欧拉角

　　描述坐标系{B}相对于参考坐标系{A}的姿态有两种方式。第一种是绕固定（参考）坐标轴旋转：假设开始两个坐标系重合，先将{B}绕{A}的X轴旋转 $γ$

　　Roll:横滚

　　Pitch: 俯仰

Yaw: 偏航（航向）

　　由于是绕固定坐标系旋转，则旋转矩阵为（ $c α$

$R_{X Y Z} (γ, β, α) = R_{Z} (α) R_{Y} (β) R_{X} (γ) = [\begin{matrix} c α c β & c α s β s γ - s α c γ & c α s β c γ + s α s γ \\ s α c β & s α s β s γ + c α c γ & s α s β c γ - c α s γ \\ - s β & c β s γ & c β c γ \end{matrix}]$

　　另一种姿态描述方式是绕自身坐标轴旋转：假设开始两个坐标系重合，先将{B}绕自身的Z轴旋转 $α$

$R_{Z^{'} Y^{'} X^{'}} (α, β, γ) = R_{Z} (α) R_{Y} (β) R_{X} (γ) = [\begin{matrix} c α c β & c α s β s γ - s α c γ & c α s β c γ + s α s γ \\ s α c β & s α s β s γ + c α c γ & s α s β c γ - c α s γ \\ - s β & c β s γ & c β c γ \end{matrix}]$

　　可以发现这两种描述方式得到的旋转矩阵是一样的，即绕固定坐标轴X-Y-Z旋转 $(γ, β, α)$

Axis-Angle与四元数

　　绕坐标轴的多次旋转可以等效为绕某一转轴旋转一定的角度。假设等效旋转轴方向向量为 $\vec{K} = [k_{x}, k_{y}, k_{z}]^{T}$

$\begin{aligned} x & = k_{x} \cdot s i n \frac{θ}{2} \\ y & = k_{y} \cdot s i n \frac{θ}{2} \\ z & = k_{z} \cdot s i n \frac{θ}{2} \\ w & = c o s \frac{θ}{2} \end{aligned}$

　　且有 $x^{2} + y^{2} + z^{2} + w^{2} = 1$

　　即四元数存储了旋转轴和旋转角的信息，它能方便的描述刚体绕任意轴的旋转。

　　四元数转换为旋转矩阵：

$R = [\begin{matrix} 1 - 2 y^{2} - 2 z^{2} & 2 (x y - z w) & 2 (x z + y w) \\ 2 (x y + z w) & 1 - 2 x^{2} - 2 z^{2} & 2 (y z - x w) \\ 2 (x z - y w) & 2 (y z + x w) & 1 - 2 x^{2} - 2 y^{2} \end{matrix}]$

　　已知旋转矩阵为：

　　则对应的四元数为：

四元数与欧拉角的相互转换

　　定义两个四元数：

　　
　　其中表示矢量

；而

表示矢量

四元数加法：

　　跟复数、向量和矩阵一样，两个四元数之和需要将不同的元素加起来。

　　加法遵循实数和复数的所有交换律和结合律。

四元数乘法：

　　四元数的乘法的意义类似于矩阵的乘法，可以表示旋转的合成。当有多次旋转操作时，使用四元数可以获得更高的计算效率。

　　由于四元数乘法的非可换性，pq并不等于qp，qp乘积的向量部分是：

　　
　　Mathematica中有四元数相关的程序包Quaternions Package，需要先导入才能使用。下面计算了三个四元数的乘积：

　　计算结果为：Quaternion[-12, 4, 14, 2]

　　那么将Z-Y-X欧拉角（或RPY角：绕固定坐标系的X-Y-Z依次旋转 $α$

$q = [\begin{matrix} \cos \frac{γ}{2} \\ 0 \\ 0 \\ \sin \frac{γ}{2} \end{matrix}] [\begin{matrix} \cos \frac{β}{2} \\ 0 \\ \sin \frac{β}{2} \\ 0 \end{matrix}] [\begin{matrix} \cos \frac{α}{2} \\ \sin \frac{α}{2} \\ 0 \\ 0 \end{matrix}] = [\begin{matrix} \cos \frac{α}{2} \cos \frac{β}{2} \cos \frac{γ}{2} + \sin \frac{α}{2} \sin \frac{β}{2} \sin \frac{γ}{2} \\ \sin \frac{α}{2} \cos \frac{β}{2} \cos \frac{γ}{2} - \cos \frac{α}{2} \sin \frac{β}{2} \sin \frac{γ}{2} \\ \cos \frac{α}{2} \sin \frac{β}{2} \cos \frac{γ}{2} + \sin \frac{α}{2} \cos \frac{β}{2} \sin \frac{γ}{2} \\ \cos \frac{α}{2} \cos \frac{β}{2} \sin \frac{γ}{2} - \sin \frac{α}{2} \sin \frac{β}{2} \cos \frac{γ}{2} \end{matrix}]$

　　根据上面的公式可以求出逆解，即由四元数 $q = (q_{0}, q_{1}, q_{2}, q_{3})$

$[\begin{matrix} α \\ β \\ γ \end{matrix}] = [\begin{matrix} \arctan \frac{2 (q_{0} q_{1} + q_{2} q_{3})}{1 - 2 (q_{1}^{2} + q_{2}^{2})} \\ \arcsin (2 (q_{0} q_{2} - q_{1} q_{3})) \\ \arctan \frac{2 (q_{0} q_{3} + q_{1} q_{2})}{1 - 2 (q_{2}^{2} + q_{3}^{2})} \end{matrix}]$

　　由于arctan和arcsin的取值范围在 $\frac{- π}{2}$

$[\begin{matrix} α \\ β \\ γ \end{matrix}] = [\begin{matrix} a t a n 2 (2 (q_{0} q_{1} + q_{2} q_{3}), 1 - 2 (q_{1}^{2} + q_{2}^{2})) \\ \arcsin (2 (q_{0} q_{2} - q_{1} q_{3})) \\ a t a n 2 (2 (q_{0} q_{3} + q_{1} q_{2}), 1 - 2 (q_{2}^{2} + q_{3}^{2})) \end{matrix}]$

对于tan(θ) = y / x ：

　　θ = ATan(y / x)求出的θ取值范围是[-PI/2, PI/2]；

　　θ = ATan2(y, x)求出的θ取值范围是[-PI, PI]。

当 (x, y) 在第一象限, 0 < θ < PI/2

当 (x, y) 在第二象限 PI/2 < θ≤PI

当 (x, y) 在第三象限, –PI < θ < –PI/2

当 (x, y) 在第四象限, -PI/2 < θ < 0

　　将四元数转换为欧拉角可以参考下面的代码。需要注意欧拉角有12种旋转次序，而上面推导的公式是按照Z-Y-X顺序进行的，所以有时会在网上看到不同的转换公式（因为对应着不同的旋转次序），在使用时一定要注意旋转次序是什么。比如ADAMS软件里就默认Body 3-1-3次序，即Z-X-Z欧拉角，而VREP中则按照X-Y-Z欧拉角旋转。

　　上面的代码存在一个问题，即奇异性没有考虑。下面看一种特殊的情况（参考Maths – Conversion Quaternion to Euler）：假设一架飞机绕Y轴旋转了90°（俯仰角pitch=90），机头垂直向上，此时如何计算航向角和横滚角？

　　这时会发生自由度丢失的情况，即Yaw和Roll会变为一个自由度。此时再使用上面的公式根据四元数计算欧拉角会出现问题：

　　 $\arcsin (2 (q_{0} q_{2} - q_{1} q_{3}))$

　　 $β = π / 2$

$q = [\begin{matrix} w \\ x \\ y \\ z \end{matrix}] [\begin{matrix} 0.707 (\cos \frac{α}{2} \cos \frac{γ}{2} + \sin \frac{α}{2} \sin \frac{γ}{2}) \\ 0.707 (\sin \frac{α}{2} \cos \frac{γ}{2} - \cos \frac{α}{2} \sin \frac{γ}{2}) \\ 0.707 (\cos \frac{α}{2} \cos \frac{γ}{2} + \sin \frac{α}{2} \sin \frac{γ}{2}) \\ 0.707 (\cos \frac{α}{2} \sin \frac{γ}{2} - \sin \frac{α}{2} \cos \frac{γ}{2}) \end{matrix}] = [\begin{matrix} 0.707 \cos \frac{α - γ}{2} \\ 0.707 \sin \frac{α - γ}{2} \\ 0.707 \cos \frac{α - γ}{2} \\ 0.707 \sin \frac{α - γ}{2} \end{matrix}]$

　　则 $\frac{x}{w} = \frac{z}{y} = \tan \frac{α - γ}{2}$

$α - γ = 2 \cdot a t a n 2 (x, w)$

　　通常令 $α = 0$

　　当俯仰角为-90°，即机头竖直向下时的情况也与之类似，可以推导出奇异姿态时的计算公式。比较完整的四元数转欧拉角（Z-Y-X order）的代码如下：

　　在DirectXMath Library中有许多与刚体姿态变换相关的函数可以直接调用：

四元数乘法：XMQuaternionMultiply method –Computes the product of two quaternions.
旋转矩阵转四元数：XMQuaternionRotationMatrix method –Computes a rotation quaternion from a rotation matrix.
四元数转旋转矩阵：XMMatrixRotationQuaternion method — Builds a rotation matrix from a quaternion.
欧拉角转四元数：XMQuaternionRotationRollPitchYaw method –Computes a rotation quaternion based on the pitch, yaw, and roll (Euler angles).
四元数转Axis-Angle：XMQuaternionToAxisAngle method –Computes an axis and angle of rotation about that axis for a given quaternion.
欧拉角转旋转矩阵：XMMatrixRotationRollPitchYaw method –Builds a rotation matrix based on a given pitch, yaw, and roll (Euler angles).
Axis-Angle转旋转矩阵：XMMatrixRotationAxis method –Builds a matrix that rotates around an arbitrary axis.
构造绕X/Y/Z轴的旋转矩阵：XMMatrixRotationX method –Builds a matrix that rotates around the x-axis.(Angles are measured clockwise when looking along the rotation axis toward the origin)