本文详细介绍了坐标变换中的旋转向量、旋转矩阵和四元数之间的相互转换，包括旋转向量到旋转矩阵、旋转矩阵到旋转向量、四元数到旋转矩阵以及旋转矩阵到四元数的公式推导。同时，还探讨了欧拉角与四元数的转换，并强调了与欧拉角相关的转换需注意变换顺序。此外，文章提供了相关证明和参考资源，是学习坐标变换理论的好材料。
坐标变换学习笔记—理论篇
坐标变换符号说明
符号含义说明：
- 旋转矩阵： R = [ R 11 R 12 R 13 R 21 R 22 R 23 R 31 R 32 R 33 ] \mathbf{R} = \left[\begin{matrix} R_{11} & R_{12} & R_{13} \\ R_{21} & R_{22} & R_{23} \\ R_{31} & R_{32} & R_{33} \end{matrix}\right] R=⎣⎡R11R21R31R12R22R32R13R23R33⎦⎤。
- 旋转向量：包含一个旋转角 θ \theta θ和一个单位向量旋转轴 n \mathbf{n} n。
- 四元数： [ s , v ] T [s, \mathbf{v}]^T [s,v]T，与 [ q 0 , q 1 , q 2 , q 3 ] T [q_0, q_1, q_2, q_3]^T [q0,q1,q2,q3]T等价，其中 s = q 0 , v = [ q 1 , q 2 , q 3 ] s = q_0,~ \mathbf{v} = [q_1, q_2, q_3] s=q0, v=[q1,q2,q3]，这里默认指单位四元数，即 s 2 + v 2 = s 2 + v v T = q 0 2 + q 1 2 + q 2 2 + q 3 2 = 1 s^2+\mathbf{v}^2 = s^2 + \mathbf{vv}^T = q_0^2 + q_1^2 + q_2^2 + q_3^2 = 1 s2+v2=s2+vvT=q02+q12+q22+q32=1。
- 欧拉角： { r o l l : 横 滚 角 ， 表 示 x 轴 的 方 位 角 θ x ∈ [ − π , π ) ; p i t c h : 俯 仰 角 ， 表 示 y 轴 的 方 位 角 θ y ∈ [ − π 2 , π 2 ] ; y a w : 航 偏 角 ， 表 示 z 轴 的 方 位 角 θ z ∈ [ − π , π ) ; \left\{\begin{aligned} roll&: 横滚角，表示x轴的方位角\theta_x \in [-\pi, \pi); \\ pitch&: 俯仰角，表示y轴的方位角\theta_y \in [-\frac{\pi}{2},\frac{\pi}{2}]; \\ yaw&: 航偏角，表示z轴的方位角\theta_z \in [-\pi, \pi); \end{aligned}\right. ⎩⎪⎪⎨⎪⎪⎧rollpitchyaw:横滚角，表示x轴的方位角θx∈[−π,π);:俯仰角，表示y轴的方位角θy∈[−2π,2π];:航偏角，表示z轴的方位角θz∈[−π,π);
旋转向量 <==> 旋转矩阵
-
旋转向量 → \to → 旋转矩阵：
R = c o s ( θ ) I + ( 1 − c o s ( θ ) ) n n T + s i n ( θ ) n ∧ (1.1.1) \mathbf{R} = cos(\theta) \mathbf{I} + (1-cos(\theta))\mathbf{n}\mathbf{n}^T + sin(\theta) \mathbf{n}^{\wedge} \tag{1.1.1} R=cos(θ)I+(1−cos(θ))nnT+sin(θ)n∧(1.1.1)
也可以写成如下形式:
R = I + ( 1 − c o s ( θ ) ) ( n ∧ ) 2 + s i n ( θ ) n ∧ (1.1.2) \mathbf{R} = \mathbf{I} + (1-cos(\theta))(\mathbf{n^\wedge})^2 + sin(\theta) \mathbf{n}^{\wedge} \tag{1.1.2} R=I+(1−cos(θ))(n∧)2+sin(θ)n∧(1.1.2) -
旋转矩阵 → \to → 旋转向量：
θ = a r c c o s ( t r ( R ) − 1 2 ) (1.2) \theta = arccos(\frac{tr(\mathbf{R})-1}{2}) \tag{1.2} θ=arccos(2tr(R)−1)(1.2)
关于旋转轴，旋转轴上的向量旋转后不发生改变，因此有： R n = n \mathbf{R}\mathbf{n} = \mathbf{n} Rn=n ，转轴 n \mathbf{n} n是旋转矩阵 R \mathbf{R} R特征值 1 1 1对应的特征向量，求解此方程，再归一化，即可得到旋转轴 n \mathbf{n} n。
四元数 <==> 旋转矩阵
-
四元数 → \to → 旋转矩阵：
R = v v T + s 2 I + 2 s v ∧ + ( v ∧ ) 2 (2.1.1) \mathbf{R} = \mathbf{vv}^T + s^2\mathbf{I} + 2s\mathbf{v}^{\wedge} + (\mathbf{v}^{\wedge})^2 \tag{2.1.1} R=vvT+s2I+2sv∧+(v∧)2(2.1.1)
将 s = q 0 , v = [ q 1 , q 2 , q 3 ] T s=q_0, \mathbf{v} = [q_1, q_2, q_3]^T s=q0,v=[q1,q2,q3]T 代入式（2.1.1）可得：
R = [ q 0 2 + q 1 2 − q 2 2 − q 3 2 2 ( q 1 q 2 − q 0 q 3 ) 2 ( q 1 q 3 + q 0 q 2 ) 2 ( q 1 q 2 + q 0 q 3 ) q 0 2 + q 2 2 − q 1 2 − q 3 2 2 ( q 2 q 3 − q 0 q 1 ) 2 ( q 1 q 3 − q 0 q 2 ) 2 ( q 2 q 3 + q 0 q 1 ) q 0 2 + q 3 2 − q 1 2 − q 2 2 ] (2.1.2) \mathbf{R} = \left[\begin{matrix} q_0^2+q_1^2-q_2^2-q_3^2 & 2(q_1q_2-q_0q_3) & 2(q_1q_3+q_0q_2) \\ 2(q_1q_2+q_0q_3) & q_0^2+q_2^2-q_1^2-q_3^2 & 2(q_2q_3 - q_0 q_1) \\ 2(q_1q_3-q_0q_2) & 2(q_2q_3 + q_0 q_1) & q_0^2+q_3^2-q_1^2-q_2^2 \end{matrix}\right] \tag{2.1.2} R=⎣⎡q02+q12−q22−q322(q1q2+q0q3)2(q1q3−q0q2)2(q1q2−q0q3)q02+q22−q12−q322(q2q3+q0q1)2(q1q3+q0q2)2(q2q3−q0q1)q02+q32−q12−q22⎦⎤(2.1.2)
对于单位四元数有： q 0 2 + q 1 2 + q 2 2 + q 3 2 = 1 q_0^2 + q_1^2 + q_2^2 + q_3^2 = 1 q02+q12+q22+q32=1，因此式（2.2.2）也可写成如下形式：
R = [ 1 − 2 ( q 2 2 + q 3 2 ) 2 ( q 1 q 2 − q 0 q 3 ) 2 ( q 1 q 3 + q 0 q 2 ) 2 ( q 1 q 2 + q 0 q 3 ) 1 − 2 ( q 1 2 + q 3 2 ) 2 ( q 2 q 3 − q 0 q 1 ) 2 ( q 1 q 3 − q 0 q 2 ) 2 ( q 2 q 3 + q 0 q 1 ) 1 − 2 ( q 1 2 + q 2 2 ) ] (2.1.3) \mathbf{R} = \left[\begin{matrix} 1-2(q_2^2+q_3^2) & 2(q_1q_2-q_0q_3) & 2(q_1q_3+q_0q_2) \\ 2(q_1q_2+q_0q_3) & 1-2(q_1^2+q_3^2) & 2(q_2q_3 - q_0 q_1) \\ 2(q_1q_3-q_0q_2) & 2(q_2q_3 + q_0 q_1) & 1-2(q_1^2+q_2^2) \end{matrix}\right] \tag{2.1.3} R=⎣⎡1−2(q22+q32)2(q1q2+q0q3)2(q1q3−q0q2)2(q1q2−q0q3)1−2(q12+q32)2(q2q3+q0q1)2(q1q3+q0q2)2(q2q3−q0q1)1−2(q12+q22)⎦⎤(2.1.3) -
旋转矩阵 → \to → 四元数：
（1）当 t r ( R ) > 0 tr{(\mathbf{R})} > 0 tr(R)>0 时：
q = { q 0 = t r ( R ) + 1 2 q 1 = R 32 − R 23 4 q 0 q 2 = R 13 −
最低0.47元/天 解锁文章
扫一扫
1604
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
