罗德里格斯（Rodrigues）旋转公式及其推导

罗德里格斯（Rodrigues）旋转公式及其推导

三维空间旋转矩阵

计算机图形学中，三维空间下绕不同坐标轴的旋转矩阵如下（右手系逆时针）：
绕X轴旋转： $$R_x=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\theta & -\sin \theta \\ 0& sin\theta & \cos \theta \end{array}\right]\text{\hspace{1em}(1)}$$

绕Y轴旋转： $$R_y=\left[\begin{array}{ccc}\cos \theta & 0& \sin \theta \\ 0& 1& 0\\ -\sin \theta & 0& \cos \theta \end{array}\right]\text{\hspace{1em}(2)}$$

绕Z轴旋转： $$R_z=\left[\begin{array}{ccc}cos\theta & -\sin \theta & 0\\ sin\theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]\text{\hspace{1em}(3)}$$

可以看到，这三个旋转矩阵，只有在三维空间下物体围绕某一特定坐标轴旋转的特殊情况下才能使用。从几何角度来讲，三维空间中任意一个旋转（绕任意轴），都可以分解为绕X轴，Y轴，Z轴旋转的复合。即对于任意旋转轴$\vec{n}$,旋转角$\theta$：$$R(\vec{n},\theta )=R(\vec{x},\theta x)\ast R(\vec{y},\theta y)\ast R(\vec{z},\theta z).\text{\hspace{1em}(4)}$$
然而，这样分解与矩阵运算的计算量显然是十分大的。

罗德里格斯旋转方程（Rodrigues）

罗德里格斯旋转公式，用于表示空间中任一向量$\vec{v}$，沿任一旋转轴$\vec{k}$, 旋转任一角度$\theta$后，得到的结果:$${\vec{v}}_{rot}=\vec{v}\cos \theta +(1-\cos \theta )(\vec{k}\cdot \vec{v})\cdot \vec{k}+\sin \theta \ast \vec{k}\times \vec{v}\text{\hspace{1em}(5)}$$
这个式子还不是很直观，所以需要引入另外两个公式来再推导两步化简：

叉积矩阵

        ~~~~~~~        关于 $\vec{a}\times \vec{b}$ ，有：$$\left(\begin{array}{c}{x}_a\\ y_a\\ z_a\end{array}\right)\times \left(\begin{array}{c}{x}_b\\ y_b\\ z_b\end{array}\right)=\left(\begin{array}{c}{y}_a{z}_b-z_a{y}_b\\ z_a{x}_b-x_a{z}_b\\ x_a{y}_b-y_a{x}_b\end{array}\right)\text{\hspace{1em}(6)}$$
        ~~~~~~~        可以写成矩阵形式：$$\left(\begin{array}{c}{y}_a{z}_b-z_a{y}_b\\ z_a{x}_b-x_a{z}_b\\ x_a{y}_b-y_a{x}_b\end{array}\right)=\left(\begin{array}{ccc}0& -z_a& y_a\\ z_a& 0& -x_a\\ -y_a& x_a& 0\end{array}\right)\cdot \left(\begin{array}{c}{x}_b\\ y_b\\ z_b\end{array}\right)\text{\hspace{1em}(7)}$$
        ~~~~~~~        则可记 $\vec{a}$ 的 " 叉积矩阵 " 为：
$$R_{\vec{a}}=\left(\begin{array}{ccc}0& -z_a& y_a\\ z_a& 0& -x_a\\ -y_a& x_a& 0\end{array}\right)\text{\hspace{1em}(8)}$$
        ~~~~~~~        对于任意向量 $\vec{b}$ , 有 :
$$\vec{a}\times \vec{b}=R_{\vec{a}}\cdot \vec{b}\text{\hspace{1em}(9)}$$

拉格朗日公式（向量三重积展开）

        ~~~~~~~        对于三个向量$\vec{a}\vec{b}\vec{c}$ ，向量的三重积定义为：
$$\vec{a}\times (\vec{b}\times \vec{c})$$
        ~~~~~~~        值得注意的是，一般来说 :
$$\vec{a}\times (\vec{b}\times \vec{c})\ne (\vec{a}\times \vec{b})\times \vec{c}$$
        ~~~~~~~        以下恒等式，称作三重积展开或拉格朗日公式，对于任意向量 $\vec{a}、\vec{b}、\vec{c}$ 均成立 ：

$$\vec{a}\times (\vec{b}\times \vec{c})=(\vec{a}\cdot \vec{c})\vec{b}-(\vec{a}\cdot \vec{b})\vec{c}\text{\hspace{1em}(10)}$$

罗德里格斯旋转方程推导

        ~~~~~~~        如上图所示，描述了一个空间中的向量$\vec{v}$，沿旋转轴$\vec{k}$(单位向量), 逆时针旋转了$\theta$角度到${\vec{v}}_{rot}$的几何关系。
        ~~~~~~~        在$\vec{v}与\vec{k}$组成的平面上,$\vec{v}$可以分解为:与$\vec{k}$垂直的分量$\overrightarrow{v_{\perp }}$和与$\vec{k}$平行的分量$\overrightarrow{v_{\parallel }}$,有：
$$\vec{v}=\overrightarrow{v_{\parallel }}+\overrightarrow{v_{\perp }}{\vec{v}}_{rot}={\vec{v}}_{rot\parallel }+{\vec{v}}_{rot\perp }\text{\hspace{1em}(11)}$$
        ~~~~~~~        其中,易得：
$$\overrightarrow{v_{\parallel }}=(\vec{v}\cdot \vec{k})\ast \vec{k}\text{\hspace{1em}(12)}$$
        ~~~~~~~        则，由（11）式：
$$\overrightarrow{v_{\perp }}=\vec{v}-\overrightarrow{v_{\parallel }}=\vec{v}-(\vec{v}\cdot \vec{k})\ast \vec{k}\text{\hspace{1em}(13)}$$
        ~~~~~~~        由 （10）式拉格朗日公式：
$$\vec{v}-(\vec{v}\cdot \vec{k})\ast \vec{k}=(\vec{k}\cdot \vec{k})\ast \vec{v}-(\vec{k}\cdot \vec{v})\ast \vec{k}=\vec{k}\times (\vec{v}\times \overrightarrow{k)}\text{\hspace{1em}(14)}$$
        ~~~~~~~        则：
$$\overrightarrow{v_{\perp }}=\vec{k}\times (\vec{v}\times \overrightarrow{k)}=-\vec{k}\times (\vec{k}\times \overrightarrow{v)}\text{\hspace{1em}(15)}$$
        ~~~~~~~        根据几何关系，平行于旋转轴的分量在旋转时不会改变其幅度和方向，因此有：
$${\vec{v}}_{rot\parallel }={\vec{v}}_{\parallel }\text{\hspace{1em}(16)}$$
        ~~~~~~~        解旋转后的垂直分量，由图中的几何关系可得 ${\vec{v}}_{rot\perp }$可以分解为$\vec{k}\times \vec{v}$和 ${\vec{v}}_{\perp }$方向上两个分量相加，即

$$\begin{array}{r}{\vec{v}}_{rot\perp }={\vec{v}}_{rot\perp }\cdot \frac{\vec{k}\times \vec{v}}{|\vec{k}\times \vec{v}|}+{\vec{v}}_{rot\perp }\cdot \frac{{\vec{v}}_{\perp }}{|{\vec{v}}_{\perp |}}=\sin \theta \ast (\vec{k}\times \vec{v})+\cos \theta \ast {\vec{v}}_{\perp }\end{array}\text{\hspace{1em}(17)}$$
        ~~~~~~~        将(12)(16)(17)式代入，有：
$$\begin{array}{rlr}{\vec{v}}_{rot}& ={\vec{v}}_{\parallel }+\cos \theta \ast (\vec{v}-{\vec{v}}_{\parallel })+\sin \theta \ast (\vec{k}\times \vec{v})& \\ & =\cos \theta \vec{v}+(1-\cos \theta ){\vec{v}}_{\parallel }+\sin \theta (\vec{k}\times \vec{v})& \\ & =\vec{v}\cos \theta +(1-\cos \theta )(\vec{k}\cdot \vec{v})\cdot \vec{k}+\sin \theta \ast \vec{k}\times \vec{v}& =式5\end{array}$$
        ~~~~~~~        此式还可继续化简，变成矩阵形式：

$$\begin{array}{rl}{\vec{v}}_{rot}& =\vec{v}\cos \theta +(1-\cos \theta )(\vec{k}\cdot \vec{v})\cdot \vec{k}+\sin \theta \ast \vec{k}\times \vec{v}\\ & =\vec{v}-\vec{v}+\vec{v}\cos \theta +(1-\cos \theta )(\vec{k}\cdot \vec{v})\cdot \vec{k}+\sin \theta \ast \vec{k}\times \vec{v}\\ & =\vec{v}-(1-\cos \theta )\vec{v}+(1-\cos \theta )(\vec{k}\cdot \vec{v})\cdot \vec{k}+\sin \theta \ast \vec{k}\times \vec{v}\\ & =\vec{v}+(1-\cos \theta )((\vec{k}\cdot \vec{v})\vec{k}-(\vec{k}\cdot \vec{k})\vec{v})+\sin \theta \ast \vec{k}\times \vec{v}\\ & =\vec{v}+\sin \theta \vec{k}\times \vec{v}+(1-\cos \theta )\vec{k}\times (\vec{k}\times \vec{v})\end{array}$$
        ~~~~~~~        设$\vec{k}$的叉积矩阵为$R_{\vec{k}}$，有：
$$\begin{array}{rl}{\vec{v}}_{rot}& =\vec{v}+\sin \theta R_{\vec{k}}\ast \vec{v}+(1-\cos \theta )R_{\vec{k}}\ast R_{\vec{k}}\ast \vec{v}\\ & =(I+\sin \theta R_{\vec{k}}+(1-\cos \theta )R_{\vec{k}}^2)\ast \vec{v}\\ & =M\vec{v}\end{array}$$
        ~~~~~~~        其中：
$$M=I+\sin \theta R_{\vec{k}}+(1-\cos \theta )R_{\vec{k}}^2$$
        ~~~~~~~        为三维空间中任意向量绕轴$\vec{k}$逆时针旋转$\theta$角度的旋转矩阵。