基础向量运算
在三维空间中，刚体运动的描述离不开基础的向量运算。
1 内积 (Inner Product)
对于两个实向量 a , b ∈ R 3 \mathbf{a}, \mathbf{b} \in \mathbb{R}^3 a , b ∈ R 3
a ⋅ b = a T b = ∣ a ∣ ∣ b ∣ cos θ a b \mathbf{a} \cdot \mathbf{b} = \mathbf{a}^T \mathbf{b} = |\mathbf{a}| |\mathbf{b}| \cos \theta_{\mathbf{a}\mathbf{b}} a ⋅ b = a T b = ∣ a ∣∣ b ∣ cos θ ab
内积主要用于描述两个向量之间的夹角关系。
2 外积 (Outer Product)
向量的外积结果是一个同时垂直于这两个向量的新向量。在坐标表示中，外积可以转化为矩阵与向量的乘法：
a × b = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] b ≜ a ∧ b \mathbf{a} \times \mathbf{b} =\begin{bmatrix}0 & -a_3 & a_2 \\a_3 & 0 & -a_1 \\-a_2 & a_1 & 0\end{bmatrix}\mathbf{b} \triangleq \mathbf{a}^{\wedge} \mathbf{b} a × b = 0 a 3 − a 2 − a 3 0 a 1 a 2 − a 1 0 b ≜ a ∧ b
其中 a ∧ \mathbf{a}^{\wedge} a ∧ ∧ \wedge ∧
3 旋转矩阵与特殊正交群S O ( 3 ) SO(3) S O ( 3 )
从线性代数而言，a = R a ′ \mathbf{a} = R \mathbf{a}' a = R a ′ a ′ \mathbf{a}' a ′ R R R a \mathbf{a} a
两个标准正交基的乘积还是标准正交基，因此旋转矩阵 R R R 1 1 1 R ⊤ R^\top R ⊤ R R R 1 1 1
n n n
S O ( n ) = { R ∈ R n × n ∣ R R ⊤ = I , det ( R ) = 1 } SO(n) = \{ R \in \mathbb{R}^{n \times n} \mid R R^\top = I,\ \det(R) = 1 \} S O ( n ) = { R ∈ R n × n ∣ R R ⊤ = I , det ( R ) = 1 }
S O ( n ) SO(n) S O ( n )
S O ( 3 ) SO(3) S O ( 3 )
3.1 旋转矩阵的数学推导
旋转矩阵 R R R { e 1 , e 2 , e 3 } \{e_1, e_2, e_3\} { e 1 , e 2 , e 3 } { e 1 ′ , e 2 ′ , e 3 ′ } \{e'_1, e'_2, e'_3\} { e 1 ′ , e 2 ′ , e 3 ′ } a a a a ′ a' a ′
a = ( e 1 , e 2 , e 3 ) [ a 1 a 2 a 3 ] = ( e 1 ′ , e 2 ′ , e 3 ′ ) [ a 1 ′ a 2 ′ a 3 ′ ] \mathbf{a} = (e_1, e_2, e_3) \begin{bmatrix} a_1 \\ a_2 \\ a_3 \end{bmatrix} = (e'_1, e'_2, e'_3) \begin{bmatrix} a'_1 \\ a'_2 \\ a'_3 \end{bmatrix} a = ( e 1 , e 2 , e 3 ) a 1 a 2 a 3 = ( e 1 ′ , e 2 ′ , e 3 ′ ) a 1 ′ a 2 ′ a 3 ′
通过两边左乘基向量的转置，可以导出坐标变换关系：
( a 1 a 2 a 3 ) = ( e 1 ⊤ e 1 ′ e 1 ⊤ e 2 ′ e 1 ⊤ e 3 ′ e 2 ⊤ e 1 ′ e 2 ⊤ e 2 ′ e 2 ⊤ e 3 ′ e 3 ⊤ e 1 ′ e 3 ⊤ e 2 ′ e 3 ⊤ e 3 ′ ) ( a 1 ′ a 2 ′ a 3 ′ ) ≜ R a ′ \begin{pmatrix} a_1 \\ a_2 \\ a_3 \end{pmatrix} = \begin{pmatrix} \mathbf{e}_1^\top \mathbf{e}_1' & \mathbf{e}_1^\top \mathbf{e}_2' & \mathbf{e}_1^\top \mathbf{e}_3' \\ \mathbf{e}_2^\top \mathbf{e}_1' & \mathbf{e}_2^\top \mathbf{e}_2' & \mathbf{e}_2^\top \mathbf{e}_3' \\ \mathbf{e}_3^\top \mathbf{e}_1' & \mathbf{e}_3^\top \mathbf{e}_2' & \mathbf{e}_3^\top \mathbf{e}_3' \end{pmatrix} \begin{pmatrix} a_1' \\ a_2' \\ a_3' \end{pmatrix} \triangleq R \, \mathbf{a}' a 1 a 2 a 3 = e 1 ⊤ e 1 ′ e 2 ⊤ e 1 ′ e 3 ⊤ e 1 ′ e 1 ⊤ e 2 ′ e 2 ⊤ e 2 ′ e 3 ⊤ e 2 ′ e 1 ⊤ e 3 ′ e 2 ⊤ e 3 ′ e 3 ⊤ e 3 ′ a 1 ′ a 2 ′ a 3 ′ ≜ R a ′
即a = R a ′ \mathbf{a} = R\mathbf{a}' a = R a ′
此时，旋转矩阵 R R R （方向余弦），因此 R R R 方向余弦矩阵 。
3.2 关键性质
正交性 ：旋转矩阵是单位正交阵，满足 R R T = I RR^T = I R R T = I R − 1 = R T R^{-1} = R^T R − 1 = R T 反向旋转 。行列式约束 ：旋转矩阵的行列式必为 1（det R = 1 \det R = 1 det R = 1 特殊正交群S O ( 3 ) SO(3) S O ( 3 ) ：三维空间旋转矩阵的集合构成了特殊正交群：
S O ( 3 ) = { R ∈ R 3 × 3 ∣ R R T = I , det ( R ) = 1 } SO(3) = \{R \in \mathbb{R}^{3 \times 3} | RR^T = I, \det(R) = 1\} S O ( 3 ) = { R ∈ R 3 × 3 ∣ R R T = I , det ( R ) = 1 }
4 欧式变换与特殊欧式群S E ( 3 ) SE(3) S E ( 3 )
3.1 欧式变换的定义
在三维空间中，刚体的运动通常由旋转 和平移 两部分组成，这种组合变换被称为欧式变换 。
对于空间中的一个向量 a a a R R R t t t a ′ a' a ′ a ′ = R a + t a' = Ra + t a ′ = R a + t t t t
例：
向量 a \mathbf{a} a R 1 R_1 R 1 t 1 t_1 t 1 b \mathbf{b} b R 2 R_2 R 2 t 2 t_2 t 2 b \mathbf{b} b c \mathbf{c} c b = R 1 a + t 1 , c = R 2 b + t 2 , \mathbf{b} = R_1 \mathbf{a} + t_1, \quad \mathbf{c} = R_2 \mathbf{b} + t_2, b = R 1 a + t 1 , c = R 2 b + t 2 , a a a c c c c = R 2 ( R 1 a + t 1 ) + t 2 . c = R_2 (R_1 a + t_1) + t_2. c = R 2 ( R 1 a + t 1 ) + t 2 .
3.2 齐次坐标与变换矩阵
为了将旋转和平移统一在同一个线性运算框架下，引入了齐次坐标 (Homogeneous Coordinates) 。
齐次化处理 ：在一个三维向量的末尾增加一个维度并取值为 1，将其变为四维向量。变换矩阵 T T T ：通过齐次坐标，可以将欧式变换写成一个 4 × 4 4 \times 4 4 × 4 [ a ′ 1 ] = [ R t 0 T 1 ] [ a 1 ] ≜ T [ a 1 ] \begin{bmatrix} a' \\ 1 \end{bmatrix} = \begin{bmatrix} R & t \\ 0^T & 1 \end{bmatrix} \begin{bmatrix} a \\ 1 \end{bmatrix} \triangleq T \begin{bmatrix} a \\ 1 \end{bmatrix} [ a ′ 1 ] = [ R 0 T t 1 ] [ a 1 ] ≜ T [ a 1 ] a a a T 1 T_1 T 1 b b b T 2 T_2 T 2 c c c c = T 2 T 1 a c = T_2 T_1 a c = T 2 T 1 a
3.3 特殊欧式群 SE(3)
变换矩阵 T T T 特殊欧式群 (Special Euclidean Group) ，定义为：
S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 } SE(3) = \left\{ T = \begin{bmatrix} R & t \\ 0^T & 1 \end{bmatrix} \in \mathbb{R}^{4 \times 4} \mid R \in SO(3), t \in \mathbb{R}^3 \right\} S E ( 3 ) = { T = [ R 0 T t 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ R 3 }
逆变换 ：T T T T − 1 = [ R T − R T t 0 T 1 ] T^{-1} = \begin{bmatrix} R^T & -R^T t \\ 0^T & 1 \end{bmatrix} T − 1 = [ R T 0 T − R T t 1 ] 行列式约束 ：变换矩阵 T T T
3.4 自由度与冗余性
欧式变换在描述空间运动时存在明显的参数冗余：
自由度 (DOF) ：三维空间的欧式变换实际上只有 6 个自由度 （3 个旋转自由度 + 3 个平移自由度）。参数量 ：变换矩阵 T T T 计算挑战 ：这种冗余性加上行列式为 1 的约束，使得在实际的优化求解（如视觉里程计或无人机姿态解算）中，直接对矩阵 T T T
简化表达方法
1 旋转向量与轴角表示 (Axis-Angle)
1.1 基本定义
三维空间中的任意旋转都可以描述为绕着某一个单位向量 n n n 旋转一个具体的角度 θ \theta θ 。为了实现更紧凑的描述，可以定义一个三维向量 ϕ \phi ϕ 旋转向量 （或轴角表示）：
ϕ = θ n \phi = \theta n ϕ = θ n θ \theta θ n n n
罗德里格斯公式 定义了从旋转向量到旋转矩阵 R R R R = cos θ I + ( 1 − cos θ ) n n T + sin θ n ∧ R = \cos\theta I + (1 - \cos\theta)nn^T + \sin\theta n^{\wedge} R = cos θ I + ( 1 − cos θ ) n n T + sin θ n ∧ I I I n ∧ n^{\wedge} n ∧ n n n
1.3 旋转矩阵到旋转向量的转换
若已知旋转矩阵 R R R θ \theta θ n n n
求解旋转角 θ \theta θ ：利用旋转矩阵的迹 (Trace) 与角度的关系：
tr ( R ) = 3 cos θ + 1 − cos θ = 1 + 2 cos θ ⟹ θ = arccos tr ( R ) − 1 2 \text{tr}(R) = 3\cos\theta + 1 - \cos\theta = 1 + 2\cos\theta \implies \theta = \arccos\frac{\text{tr}(R) - 1}{2} tr ( R ) = 3 cos θ + 1 − cos θ = 1 + 2 cos θ ⟹ θ = arccos 2 tr ( R ) − 1 求解单位旋转轴 n n n ：由于旋转轴在旋转过程中保持不动，因此 n n n R R R ( R − I ) n = 0 , ∥ n ∥ 2 = 1 (R - I)n = 0, \quad \|n\|_2 = 1 ( R − I ) n = 0 , ∥ n ∥ 2 = 1
或同右乘n n n R n = n Rn=n R n = n
1.4 优缺点与特性
紧凑性 ：旋转向量仅用 3 个量描述 3 自由度的旋转，消除了旋转矩阵（9 个量）的计算冗余。奇异性 ：轴角表示存在奇异性问题。例如，旋转 30 ∘ 30^\circ 3 0 ∘ 390 ∘ 390^\circ 39 0 ∘ 30 ∘ + 360 ∘ 30^\circ + 360^\circ 3 0 ∘ + 36 0 ∘ 直观性不足 ：尽管相比矩阵更紧凑，但旋转矩阵与轴角对于三维空间的旋转而言，在视觉直观性上均不如欧拉角。
轴角表示法通过旋转向量 ϕ \phi ϕ
2 欧拉角表示法 (Euler Angles)
2.1 基本定义
欧拉角 是一种简单且直观的旋转表示法，它直接将空间旋转分解为围绕三个坐标轴的连续旋转。通常表示为三个角度的集合，如 ( θ x , θ y , θ z ) (\theta_x, \theta_y, \theta_z) ( θ x , θ y , θ z )
2.2 基本轴旋转矩阵
当旋转仅围绕单一坐标轴发生时，其旋转矩阵具有以下闭式表达（保证旋转轴方向的坐标分量不变）：
绕 Z 轴旋转 θ \theta θ ：
R z ( θ ) = [ cos θ − sin θ 0 sin θ cos θ 0 0 0 1 ] R_z(\theta) = \begin{bmatrix} \cos\theta & -\sin\theta & 0 \\ \sin\theta & \cos\theta & 0 \\ 0 & 0 & 1 \end{bmatrix} R z ( θ ) = cos θ sin θ 0 − sin θ cos θ 0 0 0 1 绕 X 轴旋转 θ \theta θ ：
R x ( θ ) = [ 1 0 0 0 cos θ − sin θ 0 sin θ cos θ ] R_x(\theta) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\theta & -\sin\theta \\ 0 & \sin\theta & \cos\theta \end{bmatrix} R x ( θ ) = 1 0 0 0 cos θ sin θ 0 − sin θ cos θ 绕 Y 轴旋转 θ \theta θ ：
R y ( θ ) = [ cos θ 0 sin θ 0 1 0 − sin θ 0 cos θ ] R_y(\theta) = \begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \end{bmatrix} R y ( θ ) = cos θ 0 − sin θ 0 1 0 sin θ 0 cos θ
2.3 常见序列：Yaw-Pitch-Roll
在无人机及航空航天系统中，最常用的欧拉角定义是 Yaw-Pitch-Roll（偏航-俯仰-滚转） 。该序列通常对应于绕 Z、旋转后的 Y、旋转后的 X 轴进行旋转。
2.4 优缺点与局限性
直观性 ：欧拉角非常符合人类对空间运动的直觉描述，易于理解和设定。万向锁与奇异性 ：欧拉角存在著名的万向锁 (Gimbal Lock) 问题。例如，当俯仰角达到 ± 90 ∘ \pm 90^\circ ± 9 0 ∘
例：
飞机笔直向上爬升（俯仰 90°）。这时想让机头向左偏（偏航）——实际上只会导致飞机绕自身轴线滚转，而不是真正的水平转向。你无法单独实现偏航或单独实现滚转，因为这两个运动已经“锁”在了一起。但这不是真的机械卡死，而是数学上表示姿态的参数失效——本来需要三个角度唯一确定姿态，在奇异点处有无穷多种角度组合对应同一个物理姿态，导致计算和控制出问题。
欧拉角通过分步旋转的方式为刚体运动提供了最感性的描述，是工程应用中人机交互的首选。然而，万向锁导致的数学奇异性使其在需要处理全姿态连续运动（如特技飞行或机器人关节控制）的底层数值计算中存在天然缺陷。因此，在复杂的姿态解算任务中，研究者通常会转向更稳健的四元数表示。
3 四元数表示法 (Quaternions)
3.1 前序知识
在深入四元数之前，理解复数在二维平面中如何描述旋转 是理解其设计逻辑的关键源头。
向量的复数表示 ：复数 a + b i a+bi a + bi ( a , b ) (a,b) ( a , b ) 乘法即旋转 ：单位复数的乘法在几何上对应于旋转操作。
90度旋转示例 ：将向量 ( a , b ) (a,b) ( a , b ) i i i ( a + b i ) ⋅ i = a i + b i 2 = − b + a i (a+bi) \cdot i = ai + bi^2 = -b + ai ( a + bi ) ⋅ i = ai + b i 2 = − b + ai 通用旋转 (θ \theta θ ：若要将复平面上的向量旋转 θ \theta θ e i θ = cos θ + i sin θ e^{i\theta} = \cos\theta + i\sin\theta e i θ = cos θ + i sin θ 单位长度 的复数。
结论 ：在二维情况下，旋转可以通过单位复数 的乘法描述。
3.2 四元数的定义与代数性质
类似于复数在二维旋转中的应用，四元数 是描述三维空间旋转的直接推广。
代数定义 ：一个四元数 q q q s s s v = ( x , y , z ) T \mathbf{v} = (x, y, z)^T v = ( x , y , z ) T q = w + x i + y j + z k q = w + xi + yj + zk q = w + x i + y j + z k 向量表示法 ：为了简化表达，常记为 q = [ s , v ] T q = [s, \mathbf{v}]^T q = [ s , v ] T s = w ∈ R s = w \in \mathbb{R} s = w ∈ R v = ( x , y , z ) T ∈ R 3 \mathbf{v} = (x, y, z)^T \in \mathbb{R}^3 v = ( x , y , z ) T ∈ R 3 哈密顿算元规则 ：其虚部单位满足以下核心运算规则：
{ i j = k , j i = − k j k = i , k j = − i k i = j , i k = − j i 2 = − 1 , j 2 = − 1 , k 2 = − 1 \begin{cases} ij = k, ji = -k \\ jk = i, kj = -i \\ ki = j, ik = -j \\ i^2 = -1, j^2 = -1, k^2 = -1 \end{cases} ⎩ ⎨ ⎧ ij = k , j i = − k j k = i , k j = − i k i = j , ik = − j i 2 = − 1 , j 2 = − 1 , k 2 = − 1 特殊分类 ：
虚四元数（纯虚四元数） ：当实部 s = 0 s = 0 s = 0 实四元数 ：当虚部向量 v = 0 \mathbf{v} = 0 v = 0
优势 ：四元数描述方式既紧凑 （仅需 4 个参数），又彻底规避了欧拉角中的万向锁 问题。
3.3 四元数的基本运算
给定两个四元数 q a = [ s a , v a ] T q_a = [s_a, \mathbf{v}_a]^T q a = [ s a , v a ] T q b = [ s b , v b ] T q_b = [s_b, \mathbf{v}_b]^T q b = [ s b , v b ] T
加减法 ：q a ± q b = [ s a ± s b , v a ± v b ] T q_a \pm q_b = [s_a \pm s_b, \mathbf{v}_a \pm \mathbf{v}_b]^T q a ± q b = [ s a ± s b , v a ± v b ] T 乘法 ：遵循虚部单位运算规则，结果为：
q a q b = [ s a s b − v a T v b s a v b + s b v a + v a × v b ] q_a q_b = \begin{bmatrix} s_a s_b - \mathbf{v}_a^T \mathbf{v}_b \\ s_a \mathbf{v}_b + s_b \mathbf{v}_a + \mathbf{v}_a \times \mathbf{v}_b \end{bmatrix} q a q b = [ s a s b − v a T v b s a v b + s b v a + v a × v b ]
若是代数计算则q a q b = s a s b − v a T v b + s a v b + s b v a + v a × v b q_a q_b = s_a s_b - \mathbf{v}_a^T \mathbf{v}_b + s_a \mathbf{v}_b + s_b \mathbf{v}_a + \mathbf{v}_a \times \mathbf{v}_b q a q b = s a s b − v a T v b + s a v b + s b v a + v a × v b
模长与共轭 ：
模长 ∥ q ∥ = s 2 + ∥ v ∥ 2 2 \|q\| = \sqrt{s^2 + \|\mathbf{v}\|_2^2} ∥ q ∥ = s 2 + ∥ v ∥ 2 2 ∥ q a q b ∥ = ∥ q a ∥ ∥ q b ∥ \|q_a q_b\| = \|q_a\| \|q_b\| ∥ q a q b ∥ = ∥ q a ∥∥ q b ∥
共轭 q ∗ = [ s , − v ] T q^* = [s, -\mathbf{v}]^T q ∗ = [ s , − v ] T q q ∗ = q ∗ q = [ s 2 + v T v , 0 ] T = ∥ q ∥ 2 qq^* = q^*q = [s^2 + \mathbf{v}^T \mathbf{v}, \mathbf{0}]^T = \|q\|^2 q q ∗ = q ∗ q = [ s 2 + v T v , 0 ] T = ∥ q ∥ 2
逆运算 ：q − 1 = q ∗ / ∥ q ∥ 2 q^{-1} = q^* / \|q\|^2 q − 1 = q ∗ /∥ q ∥ 2 单位四元数 （模长为 1），其逆等于其共轭 q − 1 = q ∗ q^{-1} = q^* q − 1 = q ∗ ( q a q b ) − 1 = q b − 1 q a − 1 (q_a q_b)^{-1} = q_b^{-1} q_a^{-1} ( q a q b ) − 1 = q b − 1 q a − 1
3.4 旋转的代数操作描述
在三维空间中利用四元数进行旋转需要执行以下步骤：
点向量参数化 ：将待旋转的三维点 P ( x , y , z ) P(x, y, z) P ( x , y , z ) 虚四元数 （实部为 0）：
p = [ 0 , x , y , z ] T \mathbf{p} = [0, x, y, z]^T p = [ 0 , x , y , z ] T 单位四元数构造 ：若要描述绕单位向量 n \mathbf{n} n θ \theta θ q = [ cos θ 2 , n T sin θ 2 ] T q = [\cos\frac{\theta}{2}, \mathbf{n}^T\sin\frac{\theta}{2}]^T q = [ cos 2 θ , n T sin 2 θ ] T 夹心乘法运算 ：旋转后的点 p ′ \mathbf{p}' p ′ p ′ = q p q − 1 \mathbf{p}' = q\mathbf{p}q^{-1} p ′ = q p q − 1 p ′ \mathbf{p}' p ′
2、3的证明放在最后，先当已知结论
3.5 四元数乘法的矩阵化与转换推导
四元数乘法可以映射为 4 × 4 4 \times 4 4 × 4
矩阵化符号 ：定义算子 q ⊙ q^\odot q ⊙ q ⊕ q^\oplus q ⊕ q ⊙ = [ s − v T v s I + v ∧ ] , q ⊕ = [ s − v T v s I − v ∧ ] q^\odot = \begin{bmatrix} s & -\mathbf{v}^T \\ \mathbf{v} & s\mathbf{I} + \mathbf{v}^\wedge \end{bmatrix}, \quad q^\oplus = \begin{bmatrix} s & -\mathbf{v}^T \\ \mathbf{v} & s\mathbf{I} - \mathbf{v}^\wedge \end{bmatrix} q ⊙ = [ s v − v T s I + v ∧ ] , q ⊕ = [ s v − v T s I − v ∧ ] q 1 q 2 = q 1 ⊙ q 2 = q 2 ⊕ q 1 q_1 q_2 = q_1^\odot q_2 = q_2^\oplus q_1 q 1 q 2 = q 1 ⊙ q 2 = q 2 ⊕ q 1 推导旋转矩阵 ：旋转变换可写为 p ′ = q ⊙ q − 1 ⊕ p \mathbf{p}' = q^\odot q^{-1 \oplus} \mathbf{p} p ′ = q ⊙ q − 1 ⊕ p p \mathbf{p} p 从四元数到旋转矩阵 R R R 的转换公式：
q ⊙ ( q − 1 ) ⊕ = [ 1 0 ⊤ 0 R ] q^\odot (q^{-1})^\oplus = \begin{bmatrix}1 & \mathbf{0}^\top \\\mathbf{0} & R\end{bmatrix} q ⊙ ( q − 1 ) ⊕ = [ 1 0 0 ⊤ R ]
R = v v T + s 2 I + 2 s v ∧ + ( v ∧ ) 2 R = \mathbf{v}\mathbf{v}^T + s^2 \mathbf{I} + 2s\mathbf{v}^\wedge + (\mathbf{v}^\wedge)^2 R = v v T + s 2 I + 2 s v ∧ + ( v ∧ ) 2
3.6 旋转表示之间的转换关系
根据四元数到旋转矩阵的变换公式，给定单位四元数 q = [ s , v ] T q = [s, \mathbf{v}]^T q = [ s , v ] T R R R R = v v T + s 2 I + 2 s v ∧ + ( v ∧ ) 2 R = \mathbf{v}\mathbf{v}^T + s^2 I + 2s\mathbf{v}^{\wedge} + (\mathbf{v}^{\wedge})^2 R = v v T + s 2 I + 2 s v ∧ + ( v ∧ ) 2
为了建立四元数与旋转角的关系，对上述矩阵 R R R t r ( R ) = t r ( v v T + s 2 I + 2 s v ∧ + ( v ∧ ) 2 ) tr(R) = tr(\mathbf{v}\mathbf{v}^T + s^2 I + 2s\mathbf{v}^{\wedge} + (\mathbf{v}^{\wedge})^2) t r ( R ) = t r ( v v T + s 2 I + 2 s v ∧ + ( v ∧ ) 2 )
具体计算过程如下：
t r ( v v T ) tr(\mathbf{v}\mathbf{v}^T) t r ( v v T ) x 2 + y 2 + z 2 x^2 + y^2 + z^2 x 2 + y 2 + z 2 t r ( s 2 I ) tr(s^2 I) t r ( s 2 I ) 3 s 2 3s^2 3 s 2 t r ( 2 s v ∧ ) tr(2s\mathbf{v}^{\wedge}) t r ( 2 s v ∧ ) 0 0 0 t r ( ( v ∧ ) 2 ) tr((\mathbf{v}^{\wedge})^2) t r (( v ∧ ) 2 ) − 2 ( x 2 + y 2 + z 2 ) -2(x^2 + y^2 + z^2) − 2 ( x 2 + y 2 + z 2 )
将上述项合并：
t r ( R ) = ( x 2 + y 2 + z 2 ) + 3 s 2 + 0 − 2 ( x 2 + y 2 + z 2 ) tr(R) = (x^2 + y^2 + z^2) + 3s^2 + 0 - 2(x^2 + y^2 + z^2) t r ( R ) = ( x 2 + y 2 + z 2 ) + 3 s 2 + 0 − 2 ( x 2 + y 2 + z 2 ) t r ( R ) = 3 s 2 − ( x 2 + y 2 + z 2 ) tr(R) = 3s^2 - (x^2 + y^2 + z^2) t r ( R ) = 3 s 2 − ( x 2 + y 2 + z 2 )
由于 q q q 单位四元数 ，满足 s 2 + x 2 + y 2 + z 2 = 1 s^2 + x^2 + y^2 + z^2 = 1 s 2 + x 2 + y 2 + z 2 = 1 x 2 + y 2 + z 2 = 1 − s 2 x^2 + y^2 + z^2 = 1 - s^2 x 2 + y 2 + z 2 = 1 − s 2 t r ( R ) = 3 s 2 − ( 1 − s 2 ) = 4 s 2 − 1 tr(R) = 3s^2 - (1 - s^2) = \mathbf{4s^2 - 1} t r ( R ) = 3 s 2 − ( 1 − s 2 ) = 4 s 2 − 1
求解旋转角 θ \theta θ
已知旋转矩阵的迹与旋转角 θ \theta θ t r ( R ) = 1 + 2 cos θ ⟹ θ = arccos t r ( R ) − 1 2 tr(R) = 1 + 2\cos\theta \implies \theta = \arccos \frac{tr(R) - 1}{2} t r ( R ) = 1 + 2 cos θ ⟹ θ = arccos 2 t r ( R ) − 1
将我们推导出的 t r ( R ) = 4 s 2 − 1 tr(R) = 4s^2 - 1 t r ( R ) = 4 s 2 − 1 θ = arccos ( 4 s 2 − 1 ) − 1 2 = arccos ( 2 s 2 − 1 ) \theta = \arccos \frac{(4s^2 - 1) - 1}{2} = \arccos(2s^2 - 1) θ = arccos 2 ( 4 s 2 − 1 ) − 1 = arccos ( 2 s 2 − 1 )
利用三角恒等式 cos θ = 2 cos 2 θ 2 − 1 \cos\theta = 2\cos^2\frac{\theta}{2} - 1 cos θ = 2 cos 2 2 θ − 1 s = cos θ 2 s = \cos\frac{\theta}{2} s = cos 2 θ θ = 2 arccos s \mathbf{\theta = 2\arccos s} θ = 2 arccos s
求解旋转轴 n n n
已知四元数的虚部 v \mathbf{v} v n \mathbf{n} n
已知四元数虚部定义为 v = n sin θ 2 \mathbf{v} = \mathbf{n} \sin\frac{\theta}{2} v = n sin 2 θ
因此，单位旋转轴为：
n T = v T / sin θ 2 \mathbf{n}^T = \mathbf{v}^T / \sin\frac{\theta}{2} n T = v T / sin 2 θ
总结：四元数转旋转向量公式
若已知单位四元数 q = [ s , v ] T q = [s, \mathbf{v}]^T q = [ s , v ] T ϕ = θ n \phi = \theta \mathbf{n} ϕ = θ n { θ = 2 arccos s n = v / sin θ 2 \begin{cases} \theta = 2\arccos s \\ \mathbf{n} = \mathbf{v} / \sin\frac{\theta}{2} \end{cases} { θ = 2 arccos s n = v / sin 2 θ
四元数通过构建四维代数空间，将二维复数的单位圆旋转 逻辑升维。这种表述不仅在数学上通过夹心乘法保证了旋转的连续性和稳健性，其计算效率也优于冗余的 9 参数旋转矩阵（通过矩阵化算子推导可见其线性本质）。
补充部分（单位四元数构造和夹心乘法运算证明）
约定：
四元数表示为 q = [ s , v ] ⊤ q = [s,\ \mathbf{v}]^\top q = [ s , v ] ⊤ s ∈ R s\in\mathbb{R} s ∈ R v ∈ R 3 \mathbf{v}\in\mathbb{R}^3 v ∈ R 3 s 2 + ∥ v ∥ 2 = 1 s^2 + \|\mathbf{v}\|^2 = 1 s 2 + ∥ v ∥ 2 = 1
待旋转的三维点用纯虚四元数 p = [ 0 , r ] ⊤ \mathbf{p} = [0,\ \mathbf{r}]^\top p = [ 0 , r ] ⊤ r ∈ R 3 \mathbf{r}\in\mathbb{R}^3 r ∈ R 3
目标：找到 q q q q p q − 1 q\mathbf{p}q^{-1} q p q − 1 n \mathbf{n} n θ \theta θ
1 从夹心乘法的期望结果反推
1.1 计算 q p q − 1 q\mathbf{p}q^{-1} q p q − 1
已知 q − 1 = [ s , − v ] ⊤ q^{-1}=[s,\ -\mathbf{v}]^\top q − 1 = [ s , − v ] ⊤ q p q\mathbf{p} q p q p = [ s , v ] [ 0 , r ] = [ − v ⋅ r , s r + v × r ] q\mathbf{p} = [s,\ \mathbf{v}]\,[0,\ \mathbf{r}] = \big[ -\mathbf{v}\cdot\mathbf{r},\ s\mathbf{r} + \mathbf{v}\times\mathbf{r} \big] q p = [ s , v ] [ 0 , r ] = [ − v ⋅ r , s r + v × r ]
记 a = − v ⋅ r a = -\mathbf{v}\cdot\mathbf{r} a = − v ⋅ r w = s r + v × r \mathbf{w} = s\mathbf{r} + \mathbf{v}\times\mathbf{r} w = s r + v × r q p = [ a , w ] q\mathbf{p}=[a,\ \mathbf{w}] q p = [ a , w ]
再右乘 q − 1 q^{-1} q − 1 ( q p ) q − 1 = [ a , w ] [ s , − v ] = [ a s − w ⋅ ( − v ) , a ( − v ) + s w + w × ( − v ) ] (q\mathbf{p})q^{-1} = [a,\ \mathbf{w}]\,[s,\ -\mathbf{v}] = \big[ a s - \mathbf{w}\cdot(-\mathbf{v}),\ a(-\mathbf{v}) + s\mathbf{w} + \mathbf{w}\times(-\mathbf{v}) \big] ( q p ) q − 1 = [ a , w ] [ s , − v ] = [ a s − w ⋅ ( − v ) , a ( − v ) + s w + w × ( − v ) ] a s + w ⋅ v as + \mathbf{w}\cdot\mathbf{v} a s + w ⋅ v − a v + s w − w × v -a\mathbf{v} + s\mathbf{w} - \mathbf{w}\times\mathbf{v} − a v + s w − w × v
计算 w ⋅ v \mathbf{w}\cdot\mathbf{v} w ⋅ v w ⋅ v = ( s r + v × r ) ⋅ v = s ( r ⋅ v ) + ( v × r ) ⋅ v = s ( v ⋅ r ) + 0 \mathbf{w}\cdot\mathbf{v} = (s\mathbf{r} + \mathbf{v}\times\mathbf{r})\cdot\mathbf{v} = s(\mathbf{r}\cdot\mathbf{v}) + (\mathbf{v}\times\mathbf{r})\cdot\mathbf{v} = s(\mathbf{v}\cdot\mathbf{r}) + 0 w ⋅ v = ( s r + v × r ) ⋅ v = s ( r ⋅ v ) + ( v × r ) ⋅ v = s ( v ⋅ r ) + 0
因为 ( v × r ) ⊥ v (\mathbf{v}\times\mathbf{r})\perp\mathbf{v} ( v × r ) ⊥ v a = − v ⋅ r a = -\mathbf{v}\cdot\mathbf{r} a = − v ⋅ r a s = − s ( v ⋅ r ) as = -s(\mathbf{v}\cdot\mathbf{r}) a s = − s ( v ⋅ r )
a s + w ⋅ v = − s ( v ⋅ r ) + s ( v ⋅ r ) = 0 as + \mathbf{w}\cdot\mathbf{v} = -s(\mathbf{v}\cdot\mathbf{r}) + s(\mathbf{v}\cdot\mathbf{r}) = 0 a s + w ⋅ v = − s ( v ⋅ r ) + s ( v ⋅ r ) = 0
故结果仍为纯虚四元数。
虚部：
r ′ = − a v + s w − w × v \mathbf{r}' = -a\mathbf{v} + s\mathbf{w} - \mathbf{w}\times\mathbf{v} r ′ = − a v + s w − w × v
代入 a , w a,\mathbf{w} a , w
− a v = − ( − v ⋅ r ) v = ( v ⋅ r ) v -a\mathbf{v} = -(-\mathbf{v}\cdot\mathbf{r})\,\mathbf{v} = (\mathbf{v}\cdot\mathbf{r})\,\mathbf{v} − a v = − ( − v ⋅ r ) v = ( v ⋅ r ) v s w = s ( s r + v × r ) = s 2 r + s ( v × r ) s\mathbf{w} = s(s\mathbf{r} + \mathbf{v}\times\mathbf{r}) = s^2\mathbf{r} + s(\mathbf{v}\times\mathbf{r}) s w = s ( s r + v × r ) = s 2 r + s ( v × r ) 计算 w × v = ( s r + v × r ) × v = s ( r × v ) + ( v × r ) × v \mathbf{w}\times\mathbf{v} = (s\mathbf{r} + \mathbf{v}\times\mathbf{r})\times\mathbf{v} = s(\mathbf{r}\times\mathbf{v}) + (\mathbf{v}\times\mathbf{r})\times\mathbf{v} w × v = ( s r + v × r ) × v = s ( r × v ) + ( v × r ) × v ( v × r ) × v = ( v ⋅ v ) r − ( r ⋅ v ) v = ∥ v ∥ 2 r − ( v ⋅ r ) v (\mathbf{v}\times\mathbf{r})\times\mathbf{v} = (\mathbf{v}\cdot\mathbf{v})\mathbf{r} - (\mathbf{r}\cdot\mathbf{v})\mathbf{v} = \|\mathbf{v}\|^2\mathbf{r} - (\mathbf{v}\cdot\mathbf{r})\mathbf{v} ( v × r ) × v = ( v ⋅ v ) r − ( r ⋅ v ) v = ∥ v ∥ 2 r − ( v ⋅ r ) v r × v = − ( v × r ) \mathbf{r}\times\mathbf{v} = -(\mathbf{v}\times\mathbf{r}) r × v = − ( v × r ) w × v = s ( − ( v × r ) ) + ∥ v ∥ 2 r − ( v ⋅ r ) v = − s ( v × r ) + ∥ v ∥ 2 r − ( v ⋅ r ) v \mathbf{w}\times\mathbf{v} = s\big(-(\mathbf{v}\times\mathbf{r})\big) + \|\mathbf{v}\|^2\mathbf{r} - (\mathbf{v}\cdot\mathbf{r})\mathbf{v}= -s(\mathbf{v}\times\mathbf{r}) + \|\mathbf{v}\|^2\mathbf{r} - (\mathbf{v}\cdot\mathbf{r})\mathbf{v} w × v = s ( − ( v × r ) ) + ∥ v ∥ 2 r − ( v ⋅ r ) v = − s ( v × r ) + ∥ v ∥ 2 r − ( v ⋅ r ) v − w × v = s ( v × r ) − ∥ v ∥ 2 r + ( v ⋅ r ) v -\mathbf{w}\times\mathbf{v} = s(\mathbf{v}\times\mathbf{r}) - \|\mathbf{v}\|^2\mathbf{r} + (\mathbf{v}\cdot\mathbf{r})\mathbf{v} − w × v = s ( v × r ) − ∥ v ∥ 2 r + ( v ⋅ r ) v
将各项相加：
r ′ = ( v ⋅ r ) v + s 2 r + s ( v × r ) + s ( v × r ) − ∥ v ∥ 2 r + ( v ⋅ r ) v = ( s 2 − ∥ v ∥ 2 ) r + 2 s ( v × r ) + 2 ( v ⋅ r ) v . \begin{aligned}\mathbf{r}' &= (\mathbf{v}\cdot\mathbf{r})\mathbf{v} \\&\quad + s^2\mathbf{r} + s(\mathbf{v}\times\mathbf{r}) \\&\quad + s(\mathbf{v}\times\mathbf{r}) - \|\mathbf{v}\|^2\mathbf{r} + (\mathbf{v}\cdot\mathbf{r})\mathbf{v} \\&= (s^2 - \|\mathbf{v}\|^2)\mathbf{r} + 2s(\mathbf{v}\times\mathbf{r}) + 2(\mathbf{v}\cdot\mathbf{r})\mathbf{v}.\end{aligned} r ′ = ( v ⋅ r ) v + s 2 r + s ( v × r ) + s ( v × r ) − ∥ v ∥ 2 r + ( v ⋅ r ) v = ( s 2 − ∥ v ∥ 2 ) r + 2 s ( v × r ) + 2 ( v ⋅ r ) v .
1.2 与罗德里格斯公式对比
我们希望 r ′ \mathbf{r}' r ′ n \mathbf{n} n θ \theta θ r rot = cos θ r + sin θ ( n × r ) + ( 1 − cos θ ) ( n ⋅ r ) n . \mathbf{r}_{\text{rot}} = \cos\theta\,\mathbf{r} + \sin\theta\,(\mathbf{n}\times\mathbf{r}) + (1-\cos\theta)(\mathbf{n}\cdot\mathbf{r})\mathbf{n}. r rot = cos θ r + sin θ ( n × r ) + ( 1 − cos θ ) ( n ⋅ r ) n . v = n sin θ 2 \mathbf{v} = \mathbf{n}\sin\frac{\theta}{2} v = n sin 2 θ v \mathbf{v} v n \mathbf{n} n s = cos θ 2 s = \cos\frac{\theta}{2} s = cos 2 θ
∥ v ∥ = ∣ sin θ 2 ∣ \|\mathbf{v}\| = |\sin\frac{\theta}{2}| ∥ v ∥ = ∣ sin 2 θ ∣ ∥ v ∥ 2 = sin 2 θ 2 \|\mathbf{v}\|^2 = \sin^2\frac{\theta}{2} ∥ v ∥ 2 = sin 2 2 θ s 2 = cos 2 θ 2 s^2 = \cos^2\frac{\theta}{2} s 2 = cos 2 2 θ s 2 − ∥ v ∥ 2 = cos 2 θ 2 − sin 2 θ 2 = cos θ s^2 - \|\mathbf{v}\|^2 = \cos^2\frac{\theta}{2} - \sin^2\frac{\theta}{2} = \cos\theta s 2 − ∥ v ∥ 2 = cos 2 2 θ − sin 2 2 θ = cos θ 2 s = 2 cos θ 2 2s = 2\cos\frac{\theta}{2} 2 s = 2 cos 2 θ 2 s ( v × r ) = 2 cos θ 2 ( ( sin θ 2 n ) × r ) = ( 2 sin θ 2 cos θ 2 ) ( n × r ) = sin θ ( n × r ) 2s(\mathbf{v}\times\mathbf{r}) = 2\cos\frac{\theta}{2}\big( (\sin\frac{\theta}{2}\mathbf{n})\times\mathbf{r} \big) = (2\sin\frac{\theta}{2}\cos\frac{\theta}{2})\,(\mathbf{n}\times\mathbf{r}) = \sin\theta\,(\mathbf{n}\times\mathbf{r}) 2 s ( v × r ) = 2 cos 2 θ ( ( sin 2 θ n ) × r ) = ( 2 sin 2 θ cos 2 θ ) ( n × r ) = sin θ ( n × r ) 2 ( v ⋅ r ) v = 2 ( ( sin θ 2 n ) ⋅ r ) ( sin θ 2 n ) = 2 sin 2 θ 2 ( n ⋅ r ) n = ( 1 − cos θ ) ( n ⋅ r ) n 2(\mathbf{v}\cdot\mathbf{r})\mathbf{v} = 2\big((\sin\frac{\theta}{2}\mathbf{n})\cdot\mathbf{r}\big)(\sin\frac{\theta}{2}\mathbf{n}) = 2\sin^2\frac{\theta}{2}(\mathbf{n}\cdot\mathbf{r})\mathbf{n} = (1-\cos\theta)(\mathbf{n}\cdot\mathbf{r})\mathbf{n} 2 ( v ⋅ r ) v = 2 ( ( sin 2 θ n ) ⋅ r ) ( sin 2 θ n ) = 2 sin 2 2 θ ( n ⋅ r ) n = ( 1 − cos θ ) ( n ⋅ r ) n
代入后正好得到罗德里格斯公式。因此，若令
s = cos θ 2 , v = n sin θ 2 s = \cos\frac{\theta}{2},\quad \mathbf{v} = \mathbf{n}\sin\frac{\theta}{2} s = cos 2 θ , v = n sin 2 θ
则 q = [ cos θ 2 , n sin θ 2 ] ⊤ q = [\cos\frac{\theta}{2},\ \mathbf{n}\sin\frac{\theta}{2}]^\top q = [ cos 2 θ , n sin 2 θ ] ⊤ s 2 + ∥ v ∥ 2 = cos 2 θ 2 + sin 2 θ 2 = 1 s^2+\|\mathbf{v}\|^2 = \cos^2\frac{\theta}{2}+\sin^2\frac{\theta}{2}=1 s 2 + ∥ v ∥ 2 = cos 2 2 θ + sin 2 2 θ = 1
1.3 反解过程（不预设形式）
直接从系数比较出发：设 v \mathbf{v} v n \mathbf{n} n v = t n \mathbf{v} = t\mathbf{n} v = t n t ∈ R t\in\mathbb{R} t ∈ R ∥ v ∥ = ∣ t ∣ \|\mathbf{v}\| = |t| ∥ v ∥ = ∣ t ∣ v ⋅ r = t ( n ⋅ r ) \mathbf{v}\cdot\mathbf{r} = t(\mathbf{n}\cdot\mathbf{r}) v ⋅ r = t ( n ⋅ r ) v × r = t ( n × r ) \mathbf{v}\times\mathbf{r} = t(\mathbf{n}\times\mathbf{r}) v × r = t ( n × r ) r ′ \mathbf{r}' r ′
r ′ = ( s 2 − t 2 ) r + 2 s t ( n × r ) + 2 t 2 ( n ⋅ r ) n . \mathbf{r}' = (s^2 - t^2)\mathbf{r} + 2s t(\mathbf{n}\times\mathbf{r}) + 2t^2(\mathbf{n}\cdot\mathbf{r})\mathbf{n}. r ′ = ( s 2 − t 2 ) r + 2 s t ( n × r ) + 2 t 2 ( n ⋅ r ) n . 令其等于 cos θ r + sin θ ( n × r ) + ( 1 − cos θ ) ( n ⋅ r ) n \cos\theta\,\mathbf{r} + \sin\theta\,(\mathbf{n}\times\mathbf{r}) + (1-\cos\theta)(\mathbf{n}\cdot\mathbf{r})\mathbf{n} cos θ r + sin θ ( n × r ) + ( 1 − cos θ ) ( n ⋅ r ) n
{ s 2 − t 2 = cos θ , 2 s t = sin θ , 2 t 2 = 1 − cos θ . \begin{cases}
s^2 - t^2 = \cos\theta, \\
2s t = \sin\theta, \\
2t^2 = 1-\cos\theta.
\end{cases} ⎩ ⎨ ⎧ s 2 − t 2 = cos θ , 2 s t = sin θ , 2 t 2 = 1 − cos θ . 由第三式得 t 2 = 1 − cos θ 2 = sin 2 θ 2 t^2 = \frac{1-\cos\theta}{2} = \sin^2\frac{\theta}{2} t 2 = 2 1 − c o s θ = sin 2 2 θ t = sin θ 2 t = \sin\frac{\theta}{2} t = sin 2 θ 2 s sin θ 2 = sin θ = 2 sin θ 2 cos θ 2 2s\sin\frac{\theta}{2} = \sin\theta = 2\sin\frac{\theta}{2}\cos\frac{\theta}{2} 2 s sin 2 θ = sin θ = 2 sin 2 θ cos 2 θ s = cos θ 2 s = \cos\frac{\theta}{2} s = cos 2 θ
s = cos θ 2 , v = n sin θ 2 . s = \cos\frac{\theta}{2},\quad \mathbf{v} = \mathbf{n}\sin\frac{\theta}{2}. s = cos 2 θ , v = n sin 2 θ . 至此，单位四元数的半角构造被必然推导出来。