rodrigues 旋转公式

Rodrigues 旋转公式用来计算空间中的物体绕着任意一个过原点的单位向量 \(\mathbf{n} = \begin{pmatrix} n_x \\ n_y \\n_z \end{pmatrix}\) 旋转的旋转矩阵. 其公式为:

\[\mathbf{R}(\mathbf{n}, \alpha) = \cos(\alpha) \mathbf{I} + (1 - \cos(\alpha)) \mathbf{n} \mathbf{n}^T + \sin(\alpha) \begin{pmatrix} 0 & -n_z & n_y \\ n_z & 0 & -n_x \\ -n_y & n_x & 0 \end{pmatrix}. \]

本文尝试推导该旋转矩阵.

向量分解

对于空间中的任意一个点 \(P = \begin{pmatrix} P_x \\ P_y \\ P_z \end{pmatrix}\) 都可以表示为一个向量 \(\mathbf{P}\). 该向量与 \(\mathbf{n}\) 的夹角的余弦值可以通过向量点积计算:

\[\cos{\theta} = \frac{\mathbf{n}^T \mathbf{P}}{|\mathbf{P}|}, \]

这里的向量点积通过列向量的矩阵乘表示.

向量 \(\mathbf{P}\) 可以分解为沿着向量 \(\mathbf{n}\) 的分量 \(\mathbf{P_{\!\parallel}} = |\mathbf{P}| \cos{(\theta)} \mathbf{n} = \mathbf{n}^T \mathbf{P} \mathbf{n}\), 以及垂直于向量 \(\mathbf{n}\) 的分量 \(\mathbf{P_{\perp}} = \mathbf{P} - \mathbf{P_{\!\parallel}}\). 因为 \(\mathbf{n}^T \mathbf{P}\) 是一个标量, 与向量的乘法可以交换顺序, 因此 \(\mathbf{P_{\!\parallel}} = \mathbf{n} \mathbf{n}^T \mathbf{P}\).

旋转

向量 \(\mathbf{P}\) 分解后, 其绕着 \(\mathbf{n}\) 旋转可以表示为把分解的两个向量 \(\mathbf{P_{\!\parallel}}\) 和 \(\mathbf{P_{\perp}}\) 分别绕 \(\mathbf{n}\) 旋转之后再合并起来.

向量 \(\mathbf{P_{\!\parallel}}\) 沿着 \(\mathbf{n}\), 因此旋转不会改变该向量.

向量 \(\mathbf{P_{\perp}}\) 绕着 \(\mathbf{n}\) 旋转得到 \(\mathbf{P^{rot}}\), 这两个向量的夹角就是 \(\alpha\). 把 \(\mathbf{P^{rot}}\) 分解为沿着 \(\mathbf{P_{\perp}}\) 的分量:

\[\mathbf{P_{\!\parallel}^{rot}} = |\mathbf{P^{rot}}| \cos{(\alpha)} \frac{\mathbf{P_{\perp}}}{|\mathbf{P_{\perp}}|} = \cos{(\alpha)} \mathbf{P_{\perp}}, \]

以及垂直于向量 \(\mathbf{P_{\perp}}\) 的分量 \(\mathbf{P^{rot}_{\perp}}\).

向量 \(\mathbf{n}\) 垂直于向量 \(\mathbf{P_{\perp}}\) 和 \(\mathbf{P^{rot}}\) 所在平面, 因此 \(\mathbf{P^{rot}_{\perp}} \perp \mathbf{n}\). 而 \(\mathbf{P^{rot}_{\perp}}\) 又垂直于 \(\mathbf{P_{\perp}}\), 因此 \(\mathbf{P^{rot}_{\perp}}\) 与 \(\mathbf{n} \times \mathbf{P_{\perp}}\) 方向相同.

\[\mathbf{P^{rot}_{\perp}} = |\mathbf{P^{rot}}| \sin{(\alpha)} \frac{\mathbf{n} \times \mathbf{P_{\perp}}}{|\mathbf{n} \times \mathbf{P_{\perp}}|} = \sin{(\alpha)} \mathbf{n} \times \mathbf{P_{\perp}} = \sin{(\alpha)} \mathbf{n} \times (\mathbf{P} - \mathbf{P_{\!\parallel}}) = \sin{(\alpha)} \mathbf{n} \times \mathbf{P}. \]

因此旋转之后

\[\mathbf{P^{rot}} = \mathbf{P^{rot}_{\!\parallel}} + \mathbf{P^{rot}_{\perp}} = \cos{(\alpha)} \mathbf{P_{\perp}} + \sin{(\alpha)} \mathbf{n} \times \mathbf{P}. \]

合并

把向量 \(\mathbf{P_{\!\parallel}}\) 和 \(\mathbf{P^{rot}}\) 合并得到最终结果:

\[\mathbf{P'} = \mathbf{P_{\!\parallel}} + \mathbf{P^{rot}} = \mathbf{P_{\!\parallel}} + \cos{(\alpha)} (\mathbf{P} - \mathbf{P_{\!\parallel}}) + \sin{(\alpha)} \mathbf{n} \times \mathbf{P} = \cos{(\alpha)} \mathbf{P} + (1 - \cos{(\alpha)}) \mathbf{n} \mathbf{n}^T \mathbf{P} + \sin(\alpha) \mathbf{n} \times \mathbf{P}. \]

之后需要把这个关系式变为矩阵向量乘的形式:

\[\mathbf{P'} = \left ( \cos{(\alpha)} \mathbf{I} + (1 - \cos{(\alpha)}) \mathbf{n} \mathbf{n}^T + \sin{(\alpha)}\begin{pmatrix} 0 & -n_z & n_y \\ n_z & 0 & -n_x \\ -n_y & n_x & 0 \end{pmatrix} \right ) \mathbf{P}. \]

就得到 Rodrigues 旋转公式.