罗德里格斯公式附图推导，理解

罗德里格斯公式，将3D旋转表达成了$(n^{\wedge },\theta )$的形式，一般记作$$\omega =\theta \ast n={\left({\omega }_x,{\omega }_y,{\omega }_z\right)}^T$$
两种表达：
$$R=[I+(1-\cos (\theta ))\ast N^2+\sin (\theta )\ast N]$$

$$R=\cos (\theta )\ast I+(1-\cos (\theta ))\ast n\ast n^T+\sin (\theta )\ast n$$

一，基础准备

1. 旋转矩阵

$$R=\left[\begin{array}{lll}{r}_{xx}& r_{xy}& r_{xz}\\ r_{yx}& r_{yy}& r_{yz}\\ r_{zx}& r_{zy}& r_{zz}\end{array}\right]$$
R矩阵是标准正交矩阵，用于基向量之间的刚性变换。满足

• $R^{T}R=R^{-1}R=E$
• $|R|=1$

2. 旋转向量

用一个单位向量n和旋转角度$\theta$来表示旋转的过程。默认向量是列向量

$$n=\left[\begin{array}{l}{n}_1\\ n_2\\ n_3\end{array}\right]$$

• $n_1^2+n_2^2+n_3^2=1$

3. 向量叉积

图片来源:https://www.mathsisfun.com/algebra/vectors-cross-product.html
两个向量的叉积可以写成一个反对称矩阵与一个向量的点积
$$a=\left[\begin{array}{l}{a}_1\\ a_2\\ a_3\end{array}\right]$$

$$a^{\wedge }=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]=A$$

$$a\times b=\left[\begin{array}{c}{a}_2{b}_3-a_3{b}_2\\ -a_1{b}_3+a_3{b}_1\\ a_1{b}_2-a_2{b}_1\end{array}\right]=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]\ast \left[\begin{array}{l}{b}_1\\ b_2\\ b_3\end{array}\right]=A\ast a$$
所以两个向量叉积可以写成一个反对称矩阵与向量的点积 乘积
$$a\times b=a^{\wedge }\ast b$$

二，公式推导

1.符号说明与图例

名称	符号
旋转向量	$n$
旋转向量的反对称矩阵形式	$n^{\wedge }orN$
旋转角度	$\theta$
旋转点	P
OP向量	$v$
v旋转$\theta$后的向量	$v^{{}^{\prime }}$
v在旋转平面的投影	$v_{\perp }$
v在旋转轴的投影	$v_{\parallel }$
v旋转$\theta$后旋转平面的分量	$v_{\perp }^{{}^{\prime }}$
v旋转$\theta$后在n的分量	$v_{\parallel }^{{}^{\prime }}$
w向量，垂直与n向量与v向量的平面, w与n向量与v向量都正交
v方向的单位向量$\frac{v}{\left	v\right

2. 公式推导

空间中的一个点P，构成向量$v=\overrightarrow{OP}$，则分解后有

$$v=v_{\perp }+v_{\parallel }$$
同理旋转后的向量$$v^{\prime }=v_{\perp }^{{}^{\prime }}+v_{\parallel }^{{}^{\prime }}$$
显然$$v_{\parallel }=v_{\parallel }^{{}^{\prime }}$$
所以 $$v^{{}^{\prime }}=v_{\perp }^{{}^{\prime }}+v_{\parallel }$$
定义一个向量w
$$w=n\times v$$
根据几何意义，w的模长就是黄色阴影的的面积。即
$$|w|=|n\times v|$$
这部分面积也可以看成平行四边形面积公式底乘以高，因为n向量的模长是1，高就是$|v_{\perp }|$
所以$$|w|=|n\times v|=|v_{\perp }|$$
而由于$n\times w$与$v_{\perp }$方向相反
至此我们可以用n与v向量表示出v的两个投影的分量，即
$$v_{\perp }=-n\times w=-n\times (n\times v)$$

$$v_{\parallel }=v-v_{\perp }=v+n\times (n\times v)$$
我们现在讲v’表示出来
$\begin{array}{rl}{v}_{\perp }^{\prime }& =\left|v_{\perp }\right|\ast \left[\cos (\theta )\ast \frac{v_{\perp }}{\left|v_{\perp }\right|}+\sin (\theta )\ast \frac{w}{|w|}\right]\\ & =\cos (\theta )\ast v_{\perp }+\sin (\theta )\ast \left|v_{\perp }\right|\ast \frac{w}{|w|}\\ & =\cos (\theta )\ast v_{\perp }+\sin (\theta )\ast w\end{array}$
即
$$\begin{array}{r}{v}_{\perp }^{\prime }=\cos (\theta )\ast v_{\perp }+\sin (\theta )\ast w\end{array}$$
将上式子带入
$$\begin{array}{rl}{v}^{\prime }& =v_{//}+v_{\perp }^{\prime }\\ & =\left(v-v_{\perp }\right)+\cos (\theta )\ast v_{\perp }+\sin (\theta )\ast w\\ & =v+(\cos (\theta )-1)\ast v_{\perp }+\sin (\theta )\ast w\\ & =v+(1-\cos (\theta ))\ast n\times (n\times v)+\sin (\theta )\ast n\times v\\ & =v+(1-\cos (\theta ))\ast n^{\wedge }\ast (n^{\wedge }\ast v)+\sin (\theta )\ast n^{\wedge }\ast v\\ & =v+(1-\cos (\theta ))\ast N\ast N\ast v+\sin (\theta )\ast N\ast v\\ & =v+(1-\cos (\theta ))\ast N^2\ast v+\sin (\theta )\ast N\ast v\\ & =\left[I+(1-\cos (\theta ))\ast N^2+\sin (\theta )\ast N\right]\ast v\end{array}$$

至此我们就可以得到：
$$v^{{}^{\prime }}=[I+(1-\cos (\theta ))\ast N^2+\sin (\theta )\ast N]\ast v$$
所以$R=[I+(1-\cos (\theta ))\ast N^2+\sin (\theta )\ast N]$
有些地方习惯用n来表示，那么继续推导
$$n\ast n^T=\left[\begin{array}{ccc}{n}_1^2& n_1{n}_2& n_1{n}_3\\ n_1{n}_2& n_2^2& n_2{n}_3\\ n_1{n}_3& n_2{n}_3& n_3^2\end{array}\right]$$

$$n^{\wedge }=N=\left[\begin{array}{ccc}0& -n_3& n_2\\ n_3& 0& -n_1\\ -n_2& n_1& 0\end{array}\right]$$

$$N\ast N=\left[\begin{array}{ccc}-n_2^2-n_3^2& n_1{n}_2& n_1{n}_3\\ n_1{n}_2& -n_1^2-n_3^2& n_2{n}_3\\ n_1{n}_3& n_2{n}_3& -n_1^2-n_2^2\end{array}\right]$$

$$n\ast n^T=I+N\ast N$$

$$N\ast N=n\ast n^T-I$$
带入$R=[I+(1-\cos (\theta ))\ast N^2+\sin (\theta )\ast N]$可以得到

$$R=\cos (\theta )\ast I+(1-\cos (\theta ))\ast n\ast n^T+\sin (\theta )\ast n$$

三，公式理解和深入

罗德里格斯公式，将3D旋转表达成了$(n^{\wedge },\theta )$的形式，一般记作$$\omega =\theta \ast n={\left({\omega }_x,{\omega }_y,{\omega }_z\right)}^T$$

这是一种非常简洁和简单的表示形式，但是这不是一个奇异的变换。主要原因有两点

• 旋转$\theta$和旋转$\theta +2\pi$表达了相同的旋转
• $(n^{\wedge },\theta )$与$(-n^{\wedge },-\theta )$也表达了相同的旋转
  但是这样的好处在于，对于非常小的旋转，罗德里格斯公式可以表示成
  $$\begin{gathered} R(\omega )=R(n^{\wedge },\theta )\approx I+sin(\theta )\ast N \\ \approx I+\theta \ast N \\ =\left[\begin{array}{ccc}1& -\theta \ast n_3& \theta \ast n_2\\ \theta \ast n_3& 1& -\theta \ast n_1\\ -\theta \ast n_2& \theta \ast n_1& 1\end{array}\right] \\ =\left[\begin{array}{ccc}1& -{\omega }_z& {\omega }_y\\ {\omega }_z& 1& {\omega }_x\\ -{\omega }_y& {\omega }_x& 1\end{array}\right] \end{gathered}$$
  这个式子给出了，非常漂亮的的$\omega$与R之间的线性的关系。
  我们也可以写成，$Rv\approx v+w\times v$,这样我们只需要简单的求导，就可以得到v
  $$\frac{\partial \mathbf{R}\mathbf{v}}{\partial \mathbf{\omega }}=-v=\left[\begin{array}{ccc}0& z& -y\\ -z& 0& x\\ y& -x& 0\end{array}\right]$$

四，极限的方式简洁推导罗德里格斯公式

叫做exponential twist(Murray, Li, and Sastry 1994), 旋转$\theta$角度，等价于旋转k次$\theta /k$角度。
$$R(n^{\wedge },\theta )=\underset{k\to \infty }{\lim }{\left(\mathbf{I}+\frac{1}{k}(\theta \ast Ｎ)\right)}^k=e^{\theta \ast N}$$

$$e^{\theta \ast N}=I+(\theta \ast N)+\frac{(\theta \ast N{)}^2}{2}+\frac{(\theta \ast N{)}^3}{3!}+...$$
因为$$N^{k+2}=-N^k,k>0$$
所以
$$\begin{gathered} e^{\theta \ast N}=I+(\theta -\frac{{\theta }^3}{3!}+...)\ast N+(\frac{{\theta }^2}{2}-\frac{{\theta }^3}{4!}+...)\ast N^2 \\ =I+sin\theta \ast N+(1-cos\theta )\ast N^2 \end{gathered}$$