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

转载：原文链接

1 罗德里格斯（Rodrigues）旋转公式简介

对于三维空间向量$v$的旋转问题，给定罗德里格斯旋转向量$q$（由旋转轴$n$和旋转角度$\theta$构成），那么，用罗德里格斯（Rodrigues）旋转公式就可以得出旋转后的向量$v^{\prime }$，如下：

$v^{\prime }=v+(1-cos\theta )\ast N^2\cdot v+sin\theta \ast N\cdot v$ （1）
或
$v^{\prime }=cos\theta \ast v+(1-cos\theta )\ast n\cdot n^T\cdot v+sin\theta \ast n^{\wedge }\cdot v$ （2）
式中：
$v=\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\end{array}\right]$， $v^{\prime }=\left[\begin{array}{c}{v}_x^{\prime }\\ v_y^{\prime }\\ v_z^{\prime }\end{array}\right]$， $n=\left[\begin{array}{c}{n}_x\\ n_y\\ n_z\end{array}\right]$， $n^{\wedge }=N=\left[\begin{array}{ccc}0& -n_z& n_y\\ n_z& 0& -n_x\\ -n_y& n_x& 0\end{array}\right]$ （3）

$n^{\wedge }$和$N$表示向量$n$的反对称矩阵形式。

罗德里格斯旋转公式可以用旋转矩阵表示，即将罗德里格斯旋转向量$q$转换成3*3的旋转矩阵$R$表示，如下：

$v^{\prime }=[I+(1-cos\theta )\ast N^2+sin\theta \ast N]\cdot v=R\cdot v$ （4）
或
$v^{\prime }=[cos\theta \ast I+(1-cos\theta )\ast n\cdot n^T+sin\theta \ast n^{\wedge }]\cdot v=R\cdot v$ （5）
因此，罗德里格斯向量将3D旋转表示成 $(n,\theta )$的形式，一般记作：
$q=\theta \ast n=\left[\begin{array}{c}{q}_x\\ q_y\\ q_z\end{array}\right]$ （6）
式中， $\theta =|q|=\sqrt{q_x^2+q_y^2+q_z^2}$。
罗德里格斯旋转被广泛应用于空间解析几何和计算机图形学领域，已成为刚体旋转运动的基本计算公式之一。

2 基础准备

2.1 旋转矩阵

$R=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]$ （7）

旋转矩阵是单位正交矩阵，用于基向量之间的刚性变换（旋转部分），因此满足如下特性：

1. $R\cdot R^T=R\cdot R^{-1}=I$；
2. $r_i\cdot r_j=0(i,j=1,2,3,i\ne j)$；
3. $r_i\cdot r_i=|r_i|=1(i=1,2,3)$；
4. $|R|=1$;
   

2.2 旋转向量

一般旋转向量是用一个单位向量$n$和旋转角度$\theta$来表示旋转过程，向量默认是列向量。
$n=\left[\begin{array}{c}{n}_x\\ n_y\\ n_z\end{array}\right]$，$|n|=\sqrt{n_x^2+n_y^2+n_z^2}=1$。
罗德里格斯旋转向量与一般旋转向量的异同点：

• 相同点：向量方向相同；
• 不同点：模长不同，一般旋转向量是单位向量，而罗德里格斯向量的模长是旋转角度。
  

2.3 向量叉积（Cross Product）

图1 向量叉积
向量 $a$与向量 $b$的叉积 $a\times b$，可以表示成向量 $a$的反对称矩阵 $A$与向量 $b$的点积，如下：

$\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]\times \left[\begin{array}{c}{b}_x\\ b_y\\ b_z\end{array}\right]=\left[\begin{array}{ccc}0& -a_z& a_y\\ a_z& 0& -a_x\\ -a_y& a_x& 0\end{array}\right]\cdot \left[\begin{array}{c}{b}_x\\ b_y\\ b_z\end{array}\right]$ （8）
上式可被简写成：
$a\times b=a^{\wedge }\cdot b=A\cdot b$ （9）

两向量叉积后所得向量的物理意义：其模长表示两向量构成平行四边形的面积。

另外补充：

$a\cdot a^T=\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]\cdot \left[\begin{array}{ccc}{a}_x& a_y& a_z\end{array}\right]=\left[\begin{array}{ccc}{a}_x^2& a_x{a}_y& a_x{a}_z\\ a_x{a}_y& a_y^2& a_y{a}_z\\ a_x{a}_z& a_y{a}_z& a_z^2\end{array}\right]$ （10）
$A\cdot A=\left[\begin{array}{ccc}0& -a_z& a_y\\ a_z& 0& -a_x\\ -a_y& a_x& 0\end{array}\right]\cdot \left[\begin{array}{ccc}0& -a_z& a_y\\ a_z& 0& -a_x\\ -a_y& a_x& 0\end{array}\right]=\left[\begin{array}{ccc}-a_y^2-a_z^2& a_x{a}_y& a_x{a}_z\\ a_x{a}_y& -a_x^2-a_z^2& a_y{a}_z\\ a_x{a}_z& a_y{a}_z& -a_x^2-a_y^2\end{array}\right]$ （11）
因此，对于单位向量，满足：
$a\cdot a^T=I+A\cdot A$ （12）

3 罗德里格斯旋转公式推导

3.1 符号说明

图2 向量旋转示意图
对图2中的相关符号作如下说明：

注：旋转平面是与旋转轴相垂直的平面。

3.2 公式推导

对于三维空间中的一点$P$，构成向量$v=\overrightarrow{OP}$，分解到旋转平面和旋转轴，如下：

$v=v_{\perp }+v_{\parallel }$ （13）
同理，对于旋转后的向量 $v^{\prime }$：
$v^{\prime }=v_{\perp }^{\prime }+v_{\parallel }^{\prime }$ （14）
显然：
$v_{\parallel }^{\prime }=v_{\parallel }$ （15）
因此：
$v^{\prime }=v_{\perp }^{\prime }+v_{\parallel }$ （16）
定义向量 $w$，如下：
$w=n\times v$ （17）
根据几何意义， $|w|$为黄色阴影（平行四边形）的面积，这部分面积可等效成平行四边形底 $n$和高 $v_{\perp }$得到的面积，因此：
$w=n\times v=n\times v_{\perp }$ （18）
且：
$|w|=|v_{\perp }|=|v_{\perp }^{\prime }|$ （19）
进而得到：
$v_{\perp }=-n\times w=-n\times (n\times v)$ （20）
以及：
$v_{\parallel }=v_{\parallel }^{\prime }=v-v_{\perp }=v+n\times (n\times v)$ （21）

另外，将$v_{\perp }^{\prime }$用$v_{\perp }$和$w$表示，如下：

$v_{\perp }^{\prime }=|v_{\perp }^{\prime }|\ast cos\theta \ast \frac{v_{\perp }}{|v_{\perp }|}+|v_{\perp }^{{}^{\prime }}|\ast sin\theta \ast \frac{w}{|w|}=cos\theta \ast v_{\perp }+sin\theta \ast w$ （22）
将式（20）-（22）代入到式（16）中，最终可得：
$\begin{array}{rl}& v^{{}^{\prime }}=v_{\perp }^{{}^{\prime }}+v_{\parallel }\\ & =cos\theta \ast v_{\perp }+sin\theta \ast w+v-v_{\perp }\\ & =(1-cos\theta )\ast n\times (n\times v)+sin\theta \ast n\times v+v\\ & =(1-cos\theta )\ast N^2\cdot v+sin\theta \ast N\cdot v+v\\ & =[(1-cos\theta )\ast N^2+sin\theta \ast N+I]\cdot v\\ & =R\cdot v\end{array}$ （23）
因此：
$R=I+(1-cos\theta )\ast N^2+sin\theta \ast N$ （24）
将式（12）代入到式（24）中，可得：
$R=cos\theta \ast I+(1-cos\theta )\ast n\cdot n^T+sin\theta \ast n^{\wedge }$ （25）

4 拓展

4.1 公式理解和深入

罗德里格斯旋转公式，将3D旋转表示成绕空间中某一旋转轴$n$旋转角度$\theta$的形式，一般记作：

$q=\theta \ast n=\left[\begin{array}{c}{q}_x\\ q_y\\ q_z\end{array}\right]$ （26）
式中， $\theta =|q|=\sqrt{q_x^2+q_y^2+q_z^2}$。
这种表示形式非常简洁，但存在奇异问题，主要在于：

1. 旋转角度为$\theta$和$\theta +2k\pi$的结果是一样的；
2. $(n,\theta )$和$(-n,-\theta )$的结果是一样的。

对于非常小的旋转，旋转矩阵$R$和罗德里格斯向量$q$存在线性关系，推导如下：

$\begin{array}{rl}& R(q)=R(n,\theta )=I+(1-cos\theta )\ast N^2+sin\theta \ast N\\ & \approx I+sin\theta \ast N\\ & \approx I+\theta \ast N\\ & =I+q^{\wedge }\\ & =\left[\begin{array}{ccc}1& -q_z& q_y\\ q_z& 1& -q_x\\ -q_y& q_x& 1\end{array}\right]\end{array}$ （27）

4.2 极限方法推导罗德里格斯旋转公式

一次性绕旋转轴$n$旋转角度$\theta$，等价于绕旋转轴$n$旋转$k$次，每次旋转角度为$\theta /k$。
因此：

$R(n,\theta )={\lim }_{k\to \infty }(I+\frac{1}{k}\ast \theta \ast N{)}^k=e^{\theta \ast N}$ （28）
而：
$e^{\theta \ast N}=I+(\theta \ast N)+\frac{(\theta \ast N{)}^2}{2!}+\frac{(\theta \ast N{)}^3}{3!}+\cdots$ （29）
由于：
$N^{k+2}=-N^k,k>0$ （30）
最终式（29）可简化成如下：
$\begin{array}{rl}& e^{\theta \ast N}=I+(\theta -\frac{{\theta }^3}{3!}+\cdots )\ast N+(\frac{{\theta }^2}{2!}-\frac{{\theta }^4}{4!}+\cdots )\ast N^2\\ & =I+sin\theta \ast N+(1-cos\theta )\ast N^2\end{array}$ （31）