罗德里格斯公式的推导

16

Rodrigues' Formula的推导

rodrigues_rotation

定义 \mathbf{k} 是旋转轴的单位矢量，图中，初始向量 \mathbf{v} 绕 \mathbf{k}$ 轴旋转了 \theta 角度得到 \mathbf{v}_{rot}

  • 对 \mathbf{v} 做正交分解

\mathbf{v}=\mathbf{v}_{\perp}+\mathbf{v}_{\|}

其中，\mathbf{v}_{\|} 平行于旋转轴，\mathbf{v}_{\perp} 垂直于旋转轴

  • 由点乘的几何意义得

\mathbf{v}_{\|} = (\mathbf{k}\cdot\mathbf{v})\mathbf{k}

\mathbf{v}\cdot\mathbf{k}得到标量，确定向量的大小，再乘以 \mathbf{k} 得到一个矢量

  • 垂直于 \mathbf{k} 的分量

\begin{aligned} \mathbf{v}{\perp} &= \mathbf{v} - \mathbf{v}{\|} \\ &= \mathbf{v}-(\mathbf{k}\cdot\mathbf{v})\mathbf{k} \\ &= -\mathbf{k}\times(\mathbf{k}\times\mathbf{v})\end{aligned}

对于给定的三个向量 \mathbf{a},\mathbf{b},\mathbf{c} ，有
\mathbf{a}\times(\mathbf{b}\times\mathbf{c})=(\mathbf{a}\cdot\mathbf{c})\mathbf{b}-(\mathbf{a}\cdot\mathbf{b})\mathbf{c}

  • \mathbf{v}_{\|}在旋转过程中保持不变

\mathbf{v}_{\|rot}=\mathbf{v}_{\|}

  • \mathbf{v}_{\perp}在旋转中，保持大小不变，方向改变

\begin{aligned} \left | \mathbf{v}{\perp rot} \right | &= \left | \mathbf{v}{\perp} \right | \\ \mathbf{v}{\perp rot} &= \cos \theta\ \mathbf{v}{\perp} + \sin \theta\ \mathbf{k} \times \mathbf{v}{\perp} \\ &= \cos \theta\ \mathbf{v}{\perp} + \sin \theta\ \mathbf{k} \times \mathbf{v} \end{aligned}

\begin{aligned} \mathbf{k} \times \mathbf{v}{\perp} &= \mathbf{k} \times (\mathbf{v} - \mathbf{v}{\|}) \\ &= \mathbf{k} \times \mathbf{v} - \mathbf{k} \times \mathbf{v}_{\|} \\ &= \mathbf{k} \times \mathbf{v} \end{aligned}

于是，

\begin{aligned} \mathbf{v}_{rot} &= \mathbf{v}_{\|} + \mathbf{v}_{\perp} \\ &= \mathbf{v}_{\|} + \cos \theta\ \mathbf{v}_{\perp} + \sin \theta\ \mathbf{k} \times \mathbf{v} \\ &= \mathbf{v}_{\|} + \cos \theta\ (\mathbf{v} - \mathbf{v}_{\|}) + \sin \theta\ \mathbf{k} \times \mathbf{v} \\ &= \cos \theta\ \mathbf{v} + (1-\cos\theta)\mathbf{v}_{\|} + \sin \theta \ \mathbf{k}\times\mathbf{v} \\ &= \cos \theta\ \mathbf{v} + (1-\cos\theta)(\mathbf{k}\cdot\mathbf{v})\mathbf{k} + \sin \theta \ \mathbf{k}\times\mathbf{v}\\ &=\mathbf{v}-\mathbf{v}+\cos \theta\ \mathbf{v}+(1-\cos \theta)(\mathbf{k} \cdot \mathbf{v}) \mathbf{k}+\sin \theta\ \mathbf{k} \times \mathbf{v} \\ &=\mathbf{v}-(1-\cos \theta) \mathbf{v}+(1-\cos \theta)(\mathbf{k} \cdot \mathbf{v}) \mathbf{k}+\sin \theta\ \mathbf{k} \times \mathbf{v} \\ &=\mathbf{v}-(1-\cos \theta)(\mathbf{k} \cdot \mathbf{k}) \mathbf{v}+(1-\cos \theta)(\mathbf{k} \cdot \mathbf{v}) \mathbf{k}+\sin \theta\ \mathbf{k} \times \mathbf{v} \\ &=\mathbf{v}+(1-\cos \theta)((\mathbf{k} \cdot \mathbf{v}) \mathbf{k}-(\mathbf{k} \cdot \mathbf{k}) \mathbf{v})+\sin \theta\ \mathbf{k} \times \mathbf{v} \\ &=\mathbf{v}+(1-\cos \theta)(\mathbf{k} \times(\mathbf{k} \times \mathbf{v}))+\sin \theta\ \mathbf{k} \times \mathbf{v} \end{aligned}

将 \mathbf{k} 和 \mathbf{v} 表示为列向量

\mathbf{k}=[k_x,k_y,k_z]^{\text{T}}, \mathbf{v}=[v_x,v_y,v_z]^{\text{T}}

则有

\mathbf{k}\times\mathbf{v}= \begin{bmatrix} k_yv_z-k_zv_y \\ k_zv_x-k_xv_z \\ k_xv_y-k_yv_x \end{bmatrix} =\begin{bmatrix} 0 & -k_z & k_y \\ k_z & 0 & -k_x \\ -k_y & k_x & 0 \end{bmatrix} \begin{bmatrix}v_x\\v_y\\v_z\end{bmatrix}

定义

\mathbf{k}^{\wedge}= \begin{bmatrix} 0 & -k_z & k_y \\ k_z & 0 & -k_x \\ -k_y & k_x & 0 \end{bmatrix}

于是

\begin{aligned} \mathbf{v}_{rot}&=\mathbf{v}+(1-\cos \theta)(\mathbf{k}^{\wedge})^2 \mathbf{v}+\sin \theta\ \mathbf{k}^{\wedge} \mathbf{v} \\ &= \left(\mathbf{I} + (1-\cos \theta)(\mathbf{k}^{\wedge})^2 +\sin \theta\ \mathbf{k}^{\wedge} \right)\mathbf{v} \\ &= \mathbf{R} \mathbf{v} \end{aligned}

有

\mathbf{R}=\mathbf{I} + (1-\cos \theta)(\mathbf{k}^{\wedge})^2 +\sin \theta\ \mathbf{k}^{\wedge}