注：本文为 "旋转变换" 相关合辑。
图片清晰度受引文原图所限。
略作重排，如有内容异常，请看原文。
四元数、欧拉角和方向余弦的定义及关系
wust_fly_high 于 2016-08-03 19:26:35 发布
本文系统地介绍了描述两个坐标系间关系的三种常见方法：欧拉角法、方向余弦矩阵法和四元数法。以四旋翼无人机为例，详细阐述了各方法的定义、计算过程及其相互之间的转换关系。
1. 坐标系的定义
描述两个坐标系之间关系时，需首先明确坐标系的定义。以四旋翼无人机为例，定义两个坐标系：导航坐标系（参考坐标系） n n n 和载体坐标系（机体坐标系） b b b。导航坐标系 n n n 采用东北天右手直角坐标系，载体坐标系 b b b 亦为右手直角坐标系，其定义为：四轴向右为 X X X 正方向，向前为 Y Y Y 轴正方向，向上为 Z Z Z 轴正方向。
2. 欧拉角的定义与计算
欧拉角是一种描述方位的方法，其基本思想是将两个坐标系的变换分解为绕三个不同坐标轴的三次连续转动。本文采用 Z-Y-X 旋转顺序来描述载体坐标系 b b b 与导航坐标系 n n n 的关系。具体而言，绕 Z Z Z 轴旋转偏航角（YAW） ψ \psi ψ，绕 Y Y Y 轴旋转横滚角（ROLL） ϕ \phi ϕ，绕 X X X 轴旋转俯仰角（PITCH） θ \theta θ。
2.1 二维坐标系中的旋转关系
在二维 XY 坐标系中，假设载体坐标系 b b b 和导航坐标系 n n n 初始重合，绕 X X X 轴逆时针旋转 θ \theta θ 角。向量 O A OA OA 在导航坐标系中的坐标为 ( Y , Z ) (Y, Z) (Y,Z)，在载体坐标系中的坐标为 ( Y ′ , Z ′ ) (Y', Z') (Y′,Z′)。通过几何关系可得：
{ Y = Y ′ cos θ − Z ′ sin θ Z = Y ′ sin θ + Z ′ cos θ \begin{cases} Y = Y' \cos \theta - Z' \sin \theta \\ Z = Y' \sin \theta + Z' \cos \theta \end{cases} {Y=Y′cosθ−Z′sinθZ=Y′sinθ+Z′cosθ
写成矩阵形式为：
Y Z \] = \[ cos θ − sin θ sin θ cos θ \] \[ Y ′ Z ′ \] \\begin{bmatrix} Y \\\\ Z \\end{bmatrix}= \\begin{bmatrix} \\cos \\theta \& -\\sin \\theta \\\\ \\sin \\theta \& \\cos \\theta \\end{bmatrix} \\begin{bmatrix} Y' \\\\ Z' \\end{bmatrix} \[YZ\]=\[cosθsinθ−sinθcosθ\]\[Y′Z′
2.2 推广至三维坐标系
推广至三维坐标系，绕 X X X 轴旋转的坐标关系为：
X Y Z \] = \[ 1 0 0 0 cos θ − sin θ 0 sin θ cos θ \] \[ X ′ Y ′ Z ′ \] \\begin{bmatrix} X \\\\ Y \\\\ Z \\end{bmatrix}= \\begin{bmatrix} 1 \& 0 \& 0 \\\\ 0 \& \\cos \\theta \& -\\sin \\theta \\\\ 0 \& \\sin \\theta \& \\cos \\theta \\end{bmatrix} \\begin{bmatrix} X' \\\\ Y' \\\\ Z' \\end{bmatrix} XYZ = 1000cosθsinθ0−sinθcosθ X′Y′Z′
绕 Y Y Y 轴旋转的坐标关系为：
\[ X Y Z \] = \[ cos ϕ 0 sin ϕ 0 1 0 − sin ϕ 0 cos ϕ \] \[ X ′ Y ′ Z ′ \] \\begin{bmatrix} X \\\\ Y \\\\ Z \\end{bmatrix}= \\begin{bmatrix} \\cos \\phi \& 0 \& \\sin \\phi \\\\ 0 \& 1 \& 0 \\\\ -\\sin \\phi \& 0 \& \\cos \\phi \\end{bmatrix} \\begin{bmatrix} X' \\\\ Y' \\\\ Z' \\end{bmatrix} XYZ = cosϕ0−sinϕ010sinϕ0cosϕ X′Y′Z′
绕 Z Z Z 轴旋转的坐标关系为：
\[ X Y Z \] = \[ cos ψ − sin ψ 0 sin ψ cos ψ 0 0 0 1 \] \[ X ′ Y ′ Z ′ \] \\begin{bmatrix} X \\\\ Y \\\\ Z \\end{bmatrix}= \\begin{bmatrix} \\cos \\psi \& -\\sin \\psi \& 0 \\\\ \\sin \\psi \& \\cos \\psi \& 0 \\\\ 0 \& 0 \& 1 \\end{bmatrix} \\begin{bmatrix} X' \\\\ Y' \\\\ Z' \\end{bmatrix} XYZ = cosψsinψ0−sinψcosψ0001 X′Y′Z′
#### 2.3 欧拉角的综合旋转矩阵
根据 Z-Y-X 旋转顺序，导航坐标系下的向量 \[ X b Y b Z b \] \\begin{bmatrix} X\^b \\\\ Y\^b \\\\ Z\^b \\end{bmatrix} XbYbZb 与载体坐标系下的向量 \[ X n Y n Z n \] \\begin{bmatrix} X\^n \\\\ Y\^n \\\\ Z\^n \\end{bmatrix} XnYnZn 之间的关系为：
\[ X b Y b Z b \] = \[ 1 0 0 0 cos θ − sin θ 0 sin θ cos θ \] \[ cos ϕ 0 sin ϕ 0 1 0 − sin ϕ 0 cos ϕ \] \[ cos ψ − sin ψ 0 sin ψ cos ψ 0 0 0 1 \] \[ X n Y n Z n \] \\begin{bmatrix} X\^b \\\\ Y\^b \\\\ Z\^b \\end{bmatrix}= \\begin{bmatrix} 1 \& 0 \& 0 \\\\ 0 \& \\cos \\theta \& -\\sin \\theta \\\\ 0 \& \\sin \\theta \& \\cos \\theta \\end{bmatrix} \\begin{bmatrix} \\cos \\phi \& 0 \& \\sin \\phi \\\\ 0 \& 1 \& 0 \\\\ -\\sin \\phi \& 0 \& \\cos \\phi \\end{bmatrix} \\begin{bmatrix} \\cos \\psi \& -\\sin \\psi \& 0 \\\\ \\sin \\psi \& \\cos \\psi \& 0 \\\\ 0 \& 0 \& 1 \\end{bmatrix} \\begin{bmatrix} X\^n \\\\ Y\^n \\\\ Z\^n \\end{bmatrix} XbYbZb = 1000cosθsinθ0−sinθcosθ cosϕ0−sinϕ010sinϕ0cosϕ cosψsinψ0−sinψcosψ0001 XnYnZn
即：
\[ X b Y b Z b \] = R n b \[ X n Y n Z n \] \\begin{bmatrix} X\^b \\\\ Y\^b \\\\ Z\^b \\end{bmatrix}= R_n\^b \\begin{bmatrix} X\^n \\\\ Y\^n \\\\ Z\^n \\end{bmatrix} XbYbZb =Rnb XnYnZn
其中， R n b R_n\^b Rnb 为坐标系 n n n 到坐标系 b b b 的变换矩阵。通常定义的欧拉角形式的方向余弦矩阵为 R b n R_b\^n Rbn，且 R b n = ( R n b ) T R_b\^n = (R_n\^b)\^T Rbn=(Rnb)T。
R b n = \[ cos ϕ cos ψ sin θ sin ϕ cos ψ − cos θ sin ψ cos θ sin ϕ cos ψ + sin θ sin ψ cos ϕ sin ψ sin θ sin ϕ sin ψ + cos θ cos ψ cos θ sin ϕ sin ψ − sin θ cos ψ − sin ϕ sin θ cos ϕ cos θ cos ϕ \] R_b\^n = \\begin{bmatrix} \\cos \\phi \\cos \\psi \& \\sin \\theta \\sin \\phi \\cos \\psi - \\cos \\theta \\sin \\psi \& \\cos \\theta \\sin \\phi \\cos \\psi + \\sin \\theta \\sin \\psi \\\\ \\cos \\phi \\sin \\psi \& \\sin \\theta \\sin \\phi \\sin \\psi + \\cos \\theta \\cos \\psi \& \\cos \\theta \\sin \\phi \\sin \\psi - \\sin \\theta \\cos \\psi \\\\ -\\sin \\phi \& \\sin \\theta \\cos \\phi \& \\cos \\theta \\cos \\phi \\end{bmatrix} Rbn= cosϕcosψcosϕsinψ−sinϕsinθsinϕcosψ−cosθsinψsinθsinϕsinψ+cosθcosψsinθcosϕcosθsinϕcosψ+sinθsinψcosθsinϕsinψ−sinθcosψcosθcosϕ
### 3. 四元数的定义与计算
四元数由四个元素构成，表示为 Q ( q 0 , q 1 , q 2 , q 3 ) = q 0 + q 1 i ⃗ + q 2 j ⃗ + q 3 k ⃗ Q(q_0, q_1, q_2, q_3) = q_0 + q_1 \\vec{i} + q_2 \\vec{j} + q_3 \\vec{k} Q(q0,q1,q2,q3)=q0+q1i +q2j +q3k ，其中 q 0 , q 1 , q 2 , q 3 q_0, q_1, q_2, q_3 q0,q1,q2,q3 是实数， i ⃗ , j ⃗ , k ⃗ \\vec{i}, \\vec{j}, \\vec{k} i ,j ,k 是互相正交的单位向量，同时也是虚单位 − 1 \\sqrt{-1} −1 。四元数的乘法规则为：
{ i ⃗ ⊗ i ⃗ = − 1 j ⃗ ⊗ j ⃗ = − 1 k ⃗ ⊗ k ⃗ = − 1 i ⃗ ⊗ j ⃗ = k ⃗ j ⃗ ⊗ k ⃗ = i ⃗ k ⃗ ⊗ i ⃗ = j ⃗ j ⃗ ⊗ i ⃗ = − k ⃗ k ⃗ ⊗ j ⃗ = − i ⃗ i ⃗ ⊗ k ⃗ = − j ⃗ \\begin{cases} \\vec{i} \\otimes \\vec{i} = -1 \\\\ \\vec{j} \\otimes \\vec{j} = -1 \\\\ \\vec{k} \\otimes \\vec{k} = -1 \\\\ \\vec{i} \\otimes \\vec{j} = \\vec{k} \\\\ \\vec{j} \\otimes \\vec{k} = \\vec{i} \\\\ \\vec{k} \\otimes \\vec{i} = \\vec{j} \\\\ \\vec{j} \\otimes \\vec{i} = -\\vec{k} \\\\ \\vec{k} \\otimes \\vec{j} = -\\vec{i} \\\\ \\vec{i} \\otimes \\vec{k} = -\\vec{j} \\end{cases} ⎩ ⎨ ⎧i ⊗i =−1j ⊗j =−1k ⊗k =−1i ⊗j =k j ⊗k =i k ⊗i =j j ⊗i =−k k ⊗j =−i i ⊗k =−j
其中， ⊗ \\otimes ⊗ 表示四元数乘法。具体展开为：
Q ( q 0 , q 1 , q 2 , q 3 ) ⊗ P ( p 0 , p 1 , p 2 , p 3 ) = ( q 0 + q 1 i ⃗ + q 2 j ⃗ + q 3 k ⃗ ) ⊗ ( p 0 + p 1 i ⃗ + p 2 j ⃗ + p 3 k ⃗ ) = ( q 0 p 0 − q 1 p 1 − q 2 p 2 − q 3 p 3 ) + ( q 0 p 1 + q 1 p 0 + q 2 p 3 − q 3 p 2 ) i ⃗ + ( q 0 p 2 + q 2 p 0 + q 3 p 1 − q 1 p 3 ) j ⃗ + ( q 0 p 3 + q 3 p 0 + q 1 p 2 − q 2 p 1 ) k ⃗ \\begin{aligned} Q(q_0, q_1, q_2, q_3) \\otimes P(p_0, p_1, p_2, p_3) \&= (q_0 + q_1 \\vec{i} + q_2 \\vec{j} + q_3 \\vec{k}) \\otimes (p_0 + p_1 \\vec{i} + p_2 \\vec{j} + p_3 \\vec{k}) \\\\ \&= (q_0 p_0 - q_1 p_1 - q_2 p_2 - q_3 p_3) + (q_0 p_1 + q_1 p_0 + q_2 p_3 - q_3 p_2) \\vec{i} \\\\\&\\quad+ (q_0 p_2 + q_2 p_0 + q_3 p_1 - q_1 p_3) \\vec{j} + (q_0 p_3 + q_3 p_0 + q_1 p_2 - q_2 p_1) \\vec{k}\\end{aligned} Q(q0,q1,q2,q3)⊗P(p0,p1,p2,p3)=(q0+q1i +q2j +q3k )⊗(p0+p1i +p2j +p3k )=(q0p0−q1p1−q2p2−q3p3)+(q0p1+q1p0+q2p3−q3p2)i +(q0p2+q2p0+q3p1−q1p3)j +(q0p3+q3p0+q1p2−q2p1)k
写成矩阵形式为：
\[ r 0 r 1 r 2 r 3 \] = \[ q 0 − q 1 − q 2 − q 3 q 1 q 0 − q 3 q 2 q 2 q 3 q 0 − q 1 q 3 − q 2 q 1 q 0 \] \[ p 0 p 1 p 2 p 3 \] = M ( Q ) P \\begin{bmatrix} r_0 \\\\ r_1 \\\\ r_2 \\\\ r_3 \\end{bmatrix}= \\begin{bmatrix} q_0 \& -q_1 \& -q_2 \& -q_3 \\\\ q_1 \& q_0 \& -q_3 \& q_2 \\\\ q_2 \& q_3 \& q_0 \& -q_1 \\\\ q_3 \& -q_2 \& q_1 \& q_0 \\end{bmatrix} \\begin{bmatrix} p_0 \\\\ p_1 \\\\ p_2 \\\\ p_3 \\end{bmatrix}= M(Q)P r0r1r2r3 = q0q1q2q3−q1q0q3−q2−q2−q3q0q1−q3q2−q1q0 p0p1p2p3 =M(Q)P
或者：
\[ r 0 r 1 r 2 r 3 \] = \[ p 0 − p 1 − p 2 − p 3 p 1 p 0 − p 3 p 2 p 2 p 3 p 0 − p 1 p 3 − p 2 p 1 p 0 \] \[ q 0 q 1 q 2 q 3 \] = M ′ ( P ) Q \\begin{bmatrix} r_0 \\\\ r_1 \\\\ r_2 \\\\ r_3 \\end{bmatrix}= \\begin{bmatrix} p_0 \& -p_1 \& -p_2 \& -p_3 \\\\ p_1 \& p_0 \& -p_3 \& p_2 \\\\ p_2 \& p_3 \& p_0 \& -p_1 \\\\ p_3 \& -p_2 \& p_1 \& p_0 \\end{bmatrix} \\begin{bmatrix} q_0 \\\\ q_1 \\\\ q_2 \\\\ q_3 \\end{bmatrix}= M'(P)Q r0r1r2r3 = p0p1p2p3−p1p0p3−p2−p2−p3p0p1−p3p2−p1p0 q0q1q2q3 =M′(P)Q
其中， M ( Q ) M(Q) M(Q) 的构成形式为：第一列是四元数 Q Q Q 本身，第一行是 Q Q Q 的共轭四元数 Q ∗ Q\^\* Q∗ 的转置，剩下的部分是一个 3 行 3 列的矩阵 V Q V_Q VQ，称为 M ( Q ) M(Q) M(Q) 的核，是由四元数构成的反对称矩阵：
V Q = \[ q 0 − q 3 q 2 q 3 q 0 − q 1 − q 2 q 1 q 0 \] V_Q = \\begin{bmatrix} q_0 \& -q_3 \& q_2 \\\\ q_3 \& q_0 \& -q_1 \\\\ -q_2 \& q_1 \& q_0 \\end{bmatrix} VQ= q0q3−q2−q3q0q1q2−q1q0
同理可得 M ′ ( P ) M'(P) M′(P) 的核 V P V_P VP：
V P = \[ p 0 − p 3 p 2 p 3 p 0 − p 1 − p 2 p 1 p 0 \] V_P = \\begin{bmatrix} p_0 \& -p_3 \& p_2 \\\\ p_3 \& p_0 \& -p_1 \\\\ -p_2 \& p_1 \& p_0 \\end{bmatrix} VP= p0p3−p2−p3p0p1p2−p1p0
#### 3.1 四元数表示的旋转
定义四元数 q n b q_n\^b qnb 用于表示导航坐标系 n n n 到载体坐标系 b b b 的旋转变换，则有：
r n = q b n ⊗ r b ⊗ q b n ∗ = q b n ⊗ ( r b ⊗ q b n ∗ ) = q b n ⊗ \[ M ( q b n ∗ ) r b \] = M ( q b n ) M ( q b n ∗ ) r b r\^n = q_b\^n \\otimes r\^b \\otimes q_b\^{n\*} = q_b\^n \\otimes (r\^b \\otimes q_b\^{n\*}) = q_b\^n \\otimes \[M(q_b\^{n\*}) r\^b\] = M(q_b\^n) M(q_b\^{n\*}) r\^b rn=qbn⊗rb⊗qbn∗=qbn⊗(rb⊗qbn∗)=qbn⊗\[M(qbn∗)rb\]=M(qbn)M(qbn∗)rb
M ( q b n ) M ( q b n ∗ ) = \[ q 0 − q 1 − q 2 − q 3 q 1 q 0 − q 3 q 2 q 2 q 3 q 0 − q 1 q 3 − q 2 q 1 q 0 \] \[ q 0 q 1 q 2 q 3 − q 1 q 0 − q 3 q 2 − q 2 q 3 q 0 − q 1 − q 3 − q 2 q 1 q 0 \] M(q_b\^n) M(q_b\^{n\*}) = \\begin{bmatrix} q_0 \& -q_1 \& -q_2 \& -q_3 \\\\ q_1 \& q_0 \& -q_3 \& q_2 \\\\ q_2 \& q_3 \& q_0 \& -q_1 \\\\ q_3 \& -q_2 \& q_1 \& q_0 \\end{bmatrix} \\begin{bmatrix} q_0 \& q_1 \& q_2 \& q_3 \\\\ -q_1 \& q_0 \& -q_3 \& q_2 \\\\ -q_2 \& q_3 \& q_0 \& -q_1 \\\\ -q_3 \& -q_2 \& q_1 \& q_0 \\end{bmatrix} M(qbn)M(qbn∗)= q0q1q2q3−q1q0q3−q2−q2−q3q0q1−q3q2−q1q0 q0−q1−q2−q3q1q0q3−q2q2−q3q0q1q3q2−q1q0
即：
\[ 0 r x n r y n r z n \] = \[ q 0 2 + q 1 2 + q 2 2 + q 3 2 0 0 0 0 q 0 2 + q 1 2 − q 2 2 − q 3 2 2 ( q 1 q 2 − q 0 q 3 ) 2 ( q 0 q 2 + q 1 q 3 ) 0 2 ( q 0 q 3 + q 1 q 2 ) q 0 2 − q 1 2 + q 2 2 − q 3 2 2 ( q 2 q 3 − q 0 q 1 ) 0 2 ( q 1 q 3 − q 0 q 2 ) 2 ( q 0 q 1 + q 2 q 3 ) q 0 2 − q 1 2 − q 2 2 + q 3 2 \] \[ 0 r x b r y b r z b \] \\begin{bmatrix} 0 \\\\ r_x\^n \\\\ r_y\^n \\\\ r_z\^n \\end{bmatrix}= \\begin{bmatrix} q_0\^2 + q_1\^2 + q_2\^2 + q_3\^2 \& 0 \& 0 \& 0 \\\\ 0 \& q_0\^2 + q_1\^2 - q_2\^2 - q_3\^2 \& 2(q_1 q_2 - q_0 q_3) \& 2(q_0 q_2 + q_1 q_3) \\\\ 0 \& 2(q_0 q_3 + q_1 q_2) \& q_0\^2 - q_1\^2 + q_2\^2 - q_3\^2 \& 2(q_2 q_3 - q_0 q_1) \\\\ 0 \& 2(q_1 q_3 - q_0 q_2) \& 2(q_0 q_1 + q_2 q_3) \& q_0\^2 - q_1\^2 - q_2\^2 + q_3\^2 \\end{bmatrix} \\begin{bmatrix} 0 \\\\ r_x\^b \\\\ r_y\^b \\\\ r_z\^b \\end{bmatrix} 0rxnrynrzn = q02+q12+q22+q320000q02+q12−q22−q322(q0q3+q1q2)2(q1q3−q0q2)02(q1q2−q0q3)q02−q12+q22−q322(q0q1+q2q3)02(q0q2+q1q3)2(q2q3−q0q1)q02−q12−q22+q32 0rxbrybrzb
简化为：
\[ r x n r y n r z n \] = \[ q 0 2 + q 1 2 − q 2 2 − q 3 2 2 ( q 1 q 2 − q 0 q 3 ) 2 ( q 0 q 2 + q 1 q 3 ) 2 ( q 0 q 3 + q 1 q 2 ) q 0 2 − q 1 2 + q 2 2 − q 3 2 2 ( q 2 q 3 − q 0 q 1 ) 2 ( q 1 q 3 − q 0 q 2 ) 2 ( q 0 q 1 + q 2 q 3 ) q 0 2 − q 1 2 − q 2 2 + q 3 2 \] \[ r x b r y b r z b \] \\begin{bmatrix} r_x\^n \\\\ r_y\^n \\\\ r_z\^n \\end{bmatrix}= \\begin{bmatrix} q_0\^2 + q_1\^2 - q_2\^2 - q_3\^2 \& 2(q_1 q_2 - q_0 q_3) \& 2(q_0 q_2 + q_1 q_3) \\\\ 2(q_0 q_3 + q_1 q_2) \& q_0\^2 - q_1\^2 + q_2\^2 - q_3\^2 \& 2(q_2 q_3 - q_0 q_1) \\\\ 2(q_1 q_3 - q_0 q_2) \& 2(q_0 q_1 + q_2 q_3) \& q_0\^2 - q_1\^2 - q_2\^2 + q_3\^2 \\end{bmatrix} \\begin{bmatrix} r_x\^b \\\\ r_y\^b \\\\ r_z\^b \\end{bmatrix} rxnrynrzn = q02+q12−q22−q322(q0q3+q1q2)2(q1q3−q0q2)2(q1q2−q0q3)q02−q12+q22−q322(q0q1+q2q3)2(q0q2+q1q3)2(q2q3−q0q1)q02−q12−q22+q32 rxbrybrzb
其中， C b n C_b\^n Cbn 为通常定义的四元数旋转矩阵，表示载体坐标系 b b b 到导航坐标系 n n n 的旋转矩阵。
### 4. 四元数、欧拉角和方向余弦矩阵之间的关系
已知方向余弦矩阵和四元数旋转矩阵后，可通过方向余弦矩阵的第一列和第三行元素计算姿态欧拉角，具体公式如下：
\[ θ ϕ ψ \] = \[ arctan 2 ( 2 q 2 q 3 + 2 q 0 q 1 , q 3 2 − q 2 2 − q 1 2 + q 0 2 ) − arcsin ( 2 q 1 q 3 − 2 q 0 q 2 ) arctan 2 ( 2 q 1 q 2 + 2 q 0 q 3 , q 1 2 + q 0 2 − q 3 2 − q 2 2 ) \] \\begin{bmatrix}\\theta \\\\\\phi \\\\\\psi\\end{bmatrix}=\\begin{bmatrix} \\arctan 2(2 q_2 q_3 + 2 q_0 q_1, q_3\^2 - q_2\^2 - q_1\^2 + q_0\^2) \\\\ -\\arcsin(2 q_1 q_3 - 2 q_0 q_2) \\\\ \\arctan 2(2 q_1 q_2 + 2 q_0 q_3, q_1\^2 + q_0\^2 - q_3\^2 - q_2\^2) \\end{bmatrix} θϕψ = arctan2(2q2q3+2q0q1,q32−q22−q12+q02)−arcsin(2q1q3−2q0q2)arctan2(2q1q2+2q0q3,q12+q02−q32−q22)
以上内容对四元数、欧拉角和方向余弦矩阵的定义及其相互关系进行了系统阐述。
*** ** * ** ***
## 欧拉角与旋转矩阵的转换关系
陋室逢雨 原创 已于 2023-10-11 22:22:07 修改
欧拉角因存在奇异性，通常不用于优化与插值过程，但在描述旋转过程时具备直观性，例如无人机姿态控制领域常采用欧拉角表征姿态。明确欧拉角与旋转矩阵的转换关系是一项具有挑战性的工作，本文对此展开总结，以期助力相关概念的理解。
需明确每次旋转所绕的轴为固定轴还是旋转后的动轴，若无特殊说明，下文讨论均以绕旋转后的动轴旋转为前提。旋转顺序的选取不具有唯一性，采用 Z Y X ZYX ZYX（先绕 Z Z Z 轴、再绕 Y Y Y 轴、最后绕 X X X 轴）仅为使用习惯所致。绕单一坐标轴旋转某一角度亦可得到对应的旋转矩阵。
* 基础旋转矩阵：绕某一单一坐标轴进行旋转所对应的矩阵。
* 组合旋转矩阵：多次旋转操作组合后所对应的矩阵。
### 1、欧拉角的定义
定义一组欧拉角，需明确以下 5 条核心要素^\[1\]^：
* 三个旋转角的组合方式；
* 旋转角度的参考坐标系统（旋转基于固定坐标系还是自身坐标系）；
* 旋转角度所采用的坐标系旋向（左手系或右手系）；
* 三个旋转角的记法规则；
* 旋转类型（主动旋转或被动旋转）。
#### 1.1 欧拉角的旋转顺序（共12种）
* 经典欧拉角（Proper/classic Euler angle）：
z z z- x x x- z z z、 x x x- y y y- x x x、 y y y- z z z- y y y、 z z z- y y y- z z z、 x x x- z z z- x x x、 y y y- x x x- y y y。
* 泰特-布莱恩角（Tait-Bryan angle，亦称作 Cardan angles、nautical angles、heading-elevation-bank、yaw-pitch-roll）：
x x x- y y y- z z z、 y y y- z z z- x x x、 z z z- x x x- y y y、 x x x- z z z- y y y、 z z z- y y y- x x x、 y y y- x x x- z z z。
经典欧拉角的特征为三个旋转角并非完全独立，例如依次执行"绕 x x x 轴旋转 9 0 ∘ 90\^\\circ 90∘、绕 y y y 轴旋转 9 0 ∘ 90\^\\circ 90∘、绕 x x x 轴旋转 − 9 0 ∘ -90\^\\circ −90∘"的旋转组合，其效果等价于仅绕 z z z 轴旋转 − 9 0 ∘ -90\^\\circ −90∘。这表明仅在2个平面内完成旋转操作（其中一个平面需执行两次旋转），即可实现任意三维旋转。
#### 1.2 内在旋转（intrinsic rotations）与外在旋转（extrinsic rotations）
内在旋转指每次旋转围绕的轴为上一次旋转后坐标系的坐标轴；外在旋转指每次旋转围绕的轴为固定坐标系的坐标轴。内在旋转与外在旋转的转换关系为：互换第一次与第三次旋转的顺序，二者最终得到的旋转矩阵相同。例如， Z Z Z- Y Y Y- X X X 顺序的内在旋转与 X X X- Y Y Y- Z Z Z 顺序的外在旋转所对应的旋转矩阵一致。
#### 1.3 旋转角度的坐标系旋向（左手系/右手系）
以右手大拇指指向旋转轴，其余四指握拳的指向即为旋转角度的正方向：
* 右手系下，正角度对应逆时针旋转；
* 左手系下，正角度对应顺时针旋转。
#### 1.4 主动旋转与被动旋转
主动旋转指将向量绕旋转轴逆时针旋转；被动旋转指对坐标轴进行逆时针旋转，其效果等价于主动旋转的逆操作。
### 2、不同轴的欧拉角转换为旋转矩阵
定义逆时针旋转为角度正方向（与右手系旋转方向一致），给出绕各坐标轴旋转的基础旋转矩阵：
R x ( θ ) = \[ 1 0 0 0 cos θ − sin θ 0 sin θ cos θ \] , R y ( ϕ ) = \[ cos ϕ 0 sin ϕ 0 1 0 − sin ϕ 0 cos ϕ \] , R z ( ψ ) = \[ cos ψ − sin ψ 0 sin ψ cos ψ 0 0 0 1 \] R_x(\\theta)=\\begin{bmatrix} 1 \& 0 \& 0 \\\\ 0 \& \\cos\\theta \& -\\sin\\theta \\\\ 0 \& \\sin\\theta \& \\cos\\theta \\end{bmatrix}, \\quad R_y(\\phi)=\\begin{bmatrix} \\cos\\phi \& 0 \& \\sin\\phi \\\\ 0 \& 1 \& 0 \\\\ -\\sin\\phi \& 0 \& \\cos\\phi \\end{bmatrix}, \\quad R_z(\\psi)=\\begin{bmatrix} \\cos\\psi \& -\\sin\\psi \& 0 \\\\ \\sin\\psi \& \\cos\\psi \& 0 \\\\ 0 \& 0 \& 1 \\end{bmatrix} Rx(θ)= 1000cosθsinθ0−sinθcosθ ,Ry(ϕ)= cosϕ0−sinϕ010sinϕ0cosϕ ,Rz(ψ)= cosψsinψ0−sinψcosψ0001
设存在坐标系 b b b，其内部有一点 p p p（点与向量在空间中为同一几何对象，仅在指定坐标系时讨论其坐标，此处点 p p p 的坐标亦记为 p p p）。若坐标系 b b b 绕 X X X 轴逆时针旋转 α \\alpha α 角度，求旋转后坐标系下点 p p p 的坐标（记为 p ′ p' p′）。通过数学推导^\[2\]^可得：
R x ( α ) ⋅ p ′ = p (1) R_x(\\alpha) \\cdot p' = p \\tag{1} Rx(α)⋅p′=p(1)
对式 (1) 变形，得到：
p ′ = R x T ( α ) ⋅ p (2) p' = R_x\^T(\\alpha) \\cdot p \\tag{2} p′=RxT(α)⋅p(2)
若从世界坐标系 w w w 依次按 Z Y X ZYX ZYX 顺序旋转得到坐标系 b b b，结合式 (2) 可推导坐标系 b b b 相对于坐标系 w w w 的旋转矩阵 R b w R_{bw} Rbw：
R b w = R x T ( θ ) ⋅ R y T ( ϕ ) ⋅ R z T ( ψ ) = \[ 1 0 0 0 cos θ − sin θ 0 sin θ cos θ \] T ⋅ \[ cos ϕ 0 sin ϕ 0 1 0 − sin ϕ 0 cos ϕ \] T ⋅ \[ cos ψ − sin ψ 0 sin ψ cos ψ 0 0 0 1 \] T = \[ 1 0 0 0 cos θ sin θ 0 − sin θ cos θ \] ⋅ \[ cos ϕ 0 − sin ϕ 0 1 0 sin ϕ 0 cos ϕ \] ⋅ \[ cos ψ sin ψ 0 − sin ψ cos ψ 0 0 0 1 \] \\begin{align\*} R_{bw} \&= R_x\^T(\\theta) \\cdot R_y\^T(\\phi) \\cdot R_z\^T(\\psi) \\\\ \&= \\begin{bmatrix} 1 \& 0 \& 0 \\\\ 0 \& \\cos\\theta \& -\\sin\\theta \\\\ 0 \& \\sin\\theta \& \\cos\\theta \\end{bmatrix}\^T \\cdot \\begin{bmatrix} \\cos\\phi \& 0 \& \\sin\\phi \\\\ 0 \& 1 \& 0 \\\\ -\\sin\\phi \& 0 \& \\cos\\phi \\end{bmatrix}\^T \\cdot \\begin{bmatrix} \\cos\\psi \& -\\sin\\psi \& 0 \\\\ \\sin\\psi \& \\cos\\psi \& 0 \\\\ 0 \& 0 \& 1 \\end{bmatrix}\^T \\\\ \&= \\begin{bmatrix} 1 \& 0 \& 0 \\\\ 0 \& \\cos\\theta \& \\sin\\theta \\\\ 0 \& -\\sin\\theta \& \\cos\\theta \\end{bmatrix} \\cdot \\begin{bmatrix} \\cos\\phi \& 0 \& -\\sin\\phi \\\\ 0 \& 1 \& 0 \\\\ \\sin\\phi \& 0 \& \\cos\\phi \\end{bmatrix} \\cdot \\begin{bmatrix} \\cos\\psi \& \\sin\\psi \& 0 \\\\ -\\sin\\psi \& \\cos\\psi \& 0 \\\\ 0 \& 0 \& 1 \\end{bmatrix} \\end{align\*} Rbw=RxT(θ)⋅RyT(ϕ)⋅RzT(ψ)= 1000cosθsinθ0−sinθcosθ T⋅ cosϕ0−sinϕ010sinϕ0cosϕ T⋅ cosψsinψ0−sinψcosψ0001 T= 1000cosθ−sinθ0sinθcosθ ⋅ cosϕ0sinϕ010−sinϕ0cosϕ ⋅ cosψ−sinψ0sinψcosψ0001
Z Y X ZYX ZYX 顺序旋转的物理意义为：坐标系 b b b 在世界坐标系 w w w 下的欧拉角，表征从 w w w 系按该欧拉角顺序旋转可得到 b b b 系。此时需求解的是世界坐标系 w w w 相对于坐标系 b b b 的旋转矩阵 R w b R_{wb} Rwb，因此对 R b w R_{bw} Rbw 取转置：
R w b = R b w T = R z ( ψ ) ⋅ R y ( ϕ ) ⋅ R x ( θ ) = \[ cos ψ − sin ψ 0 sin ψ cos ψ 0 0 0 1 \] ⋅ \[ cos ϕ 0 sin ϕ 0 1 0 − sin ϕ 0 cos ϕ \] ⋅ \[ 1 0 0 0 cos θ − sin θ 0 sin θ cos θ \] \\begin{align\*} R_{wb} \&= R_{bw}\^T \\\\ \&= R_z(\\psi) \\cdot R_y(\\phi) \\cdot R_x(\\theta) \\\\ \&= \\begin{bmatrix} \\cos\\psi \& -\\sin\\psi \& 0 \\\\ \\sin\\psi \& \\cos\\psi \& 0 \\\\ 0 \& 0 \& 1 \\end{bmatrix} \\cdot \\begin{bmatrix} \\cos\\phi \& 0 \& \\sin\\phi \\\\ 0 \& 1 \& 0 \\\\ -\\sin\\phi \& 0 \& \\cos\\phi \\end{bmatrix} \\cdot \\begin{bmatrix} 1 \& 0 \& 0 \\\\ 0 \& \\cos\\theta \& -\\sin\\theta \\\\ 0 \& \\sin\\theta \& \\cos\\theta \\end{bmatrix} \\end{align\*} Rwb=RbwT=Rz(ψ)⋅Ry(ϕ)⋅Rx(θ)= cosψsinψ0−sinψcosψ0001 ⋅ cosϕ0−sinϕ010sinϕ0cosϕ ⋅ 1000cosθsinθ0−sinθcosθ
需注意，此处旋转矩阵的左乘顺序与旋转顺序相反，这也是VINS代码中采用的写法。同理，从旋转矩阵 R w b R_{wb} Rwb 转换为 Z Y X ZYX ZYX 顺序的欧拉角时，该欧拉角表征 b b b 系在 w w w 系下的姿态，即从 w w w 系（ R w b R_{wb} Rwb 的目标坐标系）变换至 b b b 系（ R w b R_{wb} Rwb 的源坐标系）所采用的欧拉角。
从旋转矩阵反求欧拉角时，需采用与欧拉角转旋转矩阵相同的公式体系，以避免推导逻辑混乱。
### 3、旋转的本质
设坐标系 b 1 b_1 b1 内有一点 p p p（坐标记为 p p p），依次执行"绕 X X X 轴旋转 α \\alpha α、绕 Y Y Y 轴旋转 β \\beta β、绕 Z Z Z 轴旋转 γ \\gamma γ"的操作后得到坐标系 b 2 b_2 b2，推导旋转矩阵 R b 2 b 1 R_{b_2b_1} Rb2b1 的过程如下：
1）坐标系 b 1 b_1 b1 绕 X X X 轴旋转 α \\alpha α，得到新坐标系 b 1 ′ b_1' b1′，点 p p p 在 b 1 ′ b_1' b1′ 下的坐标 p ′ p' p′ 满足：
p ′ = R x T ( α ) ⋅ p p' = R_x\^T(\\alpha) \\cdot p p′=RxT(α)⋅p
亦可写作 R x ( α ) ⋅ p ′ = p R_x(\\alpha) \\cdot p' = p Rx(α)⋅p′=p，但后续旋转操作基于当前旋转结果叠加，因此采用 p ′ p' p′ 更便于后续推导。
2）坐标系 b 1 ′ b_1' b1′ 绕 Y Y Y 轴旋转 β \\beta β，得到新坐标系 b 1 ′ ′ b_1'' b1′′，点 p p p 在 b 1 ′ ′ b_1'' b1′′ 下的坐标 p ′ ′ p'' p′′ 满足：
p ′ ′ = R y T ( β ) ⋅ p ′ p'' = R_y\^T(\\beta) \\cdot p' p′′=RyT(β)⋅p′
3）坐标系 b 1 ′ ′ b_1'' b1′′ 绕 Z Z Z 轴旋转 γ \\gamma γ，得到新坐标系 b 1 ′ ′ ′ b_1''' b1′′′（即 b 2 b_2 b2），点 p p p 在 b 1 ′ ′ ′ b_1''' b1′′′ 下的坐标 p ′ ′ ′ p''' p′′′ 满足：
p ′ ′ ′ = R z T ( γ ) ⋅ p ′ ′ p''' = R_z\^T(\\gamma) \\cdot p'' p′′′=RzT(γ)⋅p′′
将上述步骤合并，可得：
p ′ ′ ′ = R z T ( γ ) ⋅ R y T ( β ) ⋅ R x T ( α ) ⋅ p p''' = R_z\^T(\\gamma) \\cdot R_y\^T(\\beta) \\cdot R_x\^T(\\alpha) \\cdot p p′′′=RzT(γ)⋅RyT(β)⋅RxT(α)⋅p
因此，坐标系 b 2 b_2 b2 相对于 b 1 b_1 b1 的旋转矩阵为：
R b 2 b 1 = R z T ( γ ) ⋅ R y T ( β ) ⋅ R x T ( α ) R_{b_2b_1} = R_z\^T(\\gamma) \\cdot R_y\^T(\\beta) \\cdot R_x\^T(\\alpha) Rb2b1=RzT(γ)⋅RyT(β)⋅RxT(α)
上述推导方法适用于任意旋转组合的求解，因旋转具有叠加性（后续旋转矩阵左乘于当前结果）。例如，依次执行"绕 X X X 轴旋转 α \\alpha α、绕 X X X 轴旋转 α ′ \\alpha' α′、绕 Y Y Y 轴旋转 β \\beta β、绕 X X X 轴旋转 α ′ ′ \\alpha'' α′′"的操作，其组合旋转矩阵为：
R = R x T ( α ′ ′ ) ⋅ R y T ( β ) ⋅ R x T ( α ′ ) ⋅ R x T ( α ) = R x T ( α ′ ′ ) ⋅ R y T ( β ) ⋅ R x T ( α ′ + α ) R = R_x\^T(\\alpha'') \\cdot R_y\^T(\\beta) \\cdot R_x\^T(\\alpha') \\cdot R_x\^T(\\alpha) = R_x\^T(\\alpha'') \\cdot R_y\^T(\\beta) \\cdot R_x\^T(\\alpha'+\\alpha) R=RxT(α′′)⋅RyT(β)⋅RxT(α′)⋅RxT(α)=RxT(α′′)⋅RyT(β)⋅RxT(α′+α)
其中，绕同一 X X X 轴的旋转角 α \\alpha α 与 α ′ \\alpha' α′ 可合并，原因是二者旋转轴相同。
### 4、内在旋转（ Z Z Z- Y Y Y- X X X 顺序）对应的旋转矩阵
前文按 X X X- Y Y Y- Z Z Z 旋转顺序推导得到：
R b 2 b 1 = R z T ( γ ) ⋅ R y T ( β ) ⋅ R x T ( α ) R_{b_2b_1} = R_z\^T(\\gamma) \\cdot R_y\^T(\\beta) \\cdot R_x\^T(\\alpha) Rb2b1=RzT(γ)⋅RyT(β)⋅RxT(α)
将 b 2 b_2 b2 替换为 b b b、 b 1 b_1 b1 替换为 w w w，可得：
R b w = R z T ( γ ) ⋅ R y T ( β ) ⋅ R x T ( α ) R_{bw} = R_z\^T(\\gamma) \\cdot R_y\^T(\\beta) \\cdot R_x\^T(\\alpha) Rbw=RzT(γ)⋅RyT(β)⋅RxT(α)
对 R b w R_{bw} Rbw 取转置，得到：
R w b = R x ( α ) ⋅ R y ( β ) ⋅ R z ( γ ) R_{wb} = R_x(\\alpha) \\cdot R_y(\\beta) \\cdot R_z(\\gamma) Rwb=Rx(α)⋅Ry(β)⋅Rz(γ)
若旋转顺序改为 Z Z Z- Y Y Y- X X X（从 w w w 系到 b b b 系的内在旋转），则对应的旋转矩阵为：
R w b = R z ( γ ) ⋅ R y ( β ) ⋅ R x ( α ) R_{wb} = R_z(\\gamma) \\cdot R_y(\\beta) \\cdot R_x(\\alpha) Rwb=Rz(γ)⋅Ry(β)⋅Rx(α)
该结果与前文推导一致，需注意此为 R w b R_{wb} Rwb 而非 R b w R_{bw} Rbw，相关验证见第6节。
### 5、内在旋转（ Z Z Z- Y Y Y- X X X）与外在旋转（ X X X- Y Y Y- Z Z Z）对应旋转矩阵相同的原因
外在旋转的旋转轴始终为固定坐标系 w w w 的坐标轴。旋转前，坐标系 w w w 与初始坐标系 b 1 b_1 b1 重合；旋转后，二者间存在旋转矩阵形式的外参。
1）绕 w w w 系 X X X 轴旋转，得到坐标系 b 1 ′ b_1' b1′，此时外参为：
R b 1 ′ w = R w ′ w = R x T ( α ) R_{b_1'w} = R_{w'w} = R_x\^T(\\alpha) Rb1′w=Rw′w=RxT(α)
2）接着绕 w w w 系 Y Y Y 轴旋转，得到坐标系 b 1 ′ ′ b_1'' b1′′，此时 R w ′ w = R y T ( β ) R_{w'w}=R_y\^T(\\beta) Rw′w=RyT(β)，结合外参转换可得：
R b 1 ′ ′ w = ( R b 1 ′ w ⋅ R w ′ w ⋅ R b 1 ′ w T ) ⋅ R b 1 ′ w = R x T ( α ) ⋅ R y T ( β ) ⋅ R x ( α ) ⋅ R x T ( α ) = R x T ( α ) ⋅ R y T ( β ) \\begin{align\*} R_{b_1''w} \&= \\left(R_{b_1'w} \\cdot R_{w'w} \\cdot R_{b_1'w}\^T\\right) \\cdot R_{b_1'w} \\\\ \&= R_x\^T(\\alpha) \\cdot R_y\^T(\\beta) \\cdot R_x(\\alpha) \\cdot R_x\^T(\\alpha) \\\\ \&= R_x\^T(\\alpha) \\cdot R_y\^T(\\beta) \\end{align\*} Rb1′′w=(Rb1′w⋅Rw′w⋅Rb1′wT)⋅Rb1′w=RxT(α)⋅RyT(β)⋅Rx(α)⋅RxT(α)=RxT(α)⋅RyT(β)
3）继续绕 w w w 系 Z Z Z 轴旋转，得到坐标系 b 1 ′ ′ ′ b_1''' b1′′′，此时：
R b 1 ′ ′ ′ w = R x T ( α ) ⋅ R y T ( β ) ⋅ R z T ( γ ) R_{b_1'''w} = R_x\^T(\\alpha) \\cdot R_y\^T(\\beta) \\cdot R_z\^T(\\gamma) Rb1′′′w=RxT(α)⋅RyT(β)⋅RzT(γ)
对 R b 1 ′ ′ ′ w R_{b_1'''w} Rb1′′′w 取转置，得：
R w b 1 ′ ′ ′ = R z ( γ ) ⋅ R y ( β ) ⋅ R x ( α ) R_{wb_1'''} = R_z(\\gamma) \\cdot R_y(\\beta) \\cdot R_x(\\alpha) Rwb1′′′=Rz(γ)⋅Ry(β)⋅Rx(α)
综上，绕固定轴的 X X X- Y Y Y- Z Z Z 顺序外在旋转，与绕旋转后动轴的 Z Z Z- Y Y Y- X X X 顺序内在旋转，二者对应的旋转矩阵等价。
### 6、验证：内在旋转（ Z Z Z- Y Y Y- X X X）得到的是 R w b R_{wb} Rwb 而非 R b w R_{bw} Rbw
```cpp
#include