罗德里格斯公式及其推导

罗德里格斯公式及其推导

---

---

罗格里格斯公式，将3D旋转表达成了$(n^{\wedge },\theta )$的形式，一般记作
$$\omega =\theta \ast n=({\omega }_x,{\omega }_y,{\omega }_z{)}^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^{\wedge }$$

一、基础准备

1. 旋转矩阵

$$R=\left[\begin{array}{ccc}{r}_x{}_x& r_x{}_y& r_x{}_z\\ r_y{}_x& r_y{}_y& r_y{}_z\\ r_z{}_x& r_z{}_y& r_z{}_z\end{array}\right]$$
R矩阵是标准正交矩阵，用于基向量之间的刚性变换，且满足：

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

2. 旋转向量

旋转过程用一个单位向量n和旋转角度θ来表示。设旋转向量为：
$$n=\left[\begin{array}{c}{n}_1\\ n_2\\ n_3\end{array}\right]$$
其中：
$$n_1^2+n_2^2+n_3^2=1$$

3. 向量叉积及反对称矩阵

$$a\times b=a\ast b\ast sin(\theta )$$
叉积为矢量，长度为两个向量相夹的面积，方向与两个向量垂直。如下图：

我们设置向量
$$a=\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\end{array}\right]$$
a的反对称矩阵为：
$$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}{c}{b}_1\\ b_2\\ b_3\end{array}\right]=a^{\wedge }\ast b$$

二、公式推导

1. 符号说明与图例

名称	符号
旋转向量	$n$
反对称矩阵	$n^{\wedge }orN$
旋转角度	$\theta$
旋转点	$P$
$OP$向量	$v$
$v$旋转$\theta$后的向量	$v^{{}^{\prime }}$
$v$在旋转平面的投影	$v_{\perp }$
$v$在旋转轴的投影	$v_{\Vert }$
$v$旋转$\theta$后旋转平面的分量	$v_{\perp }^{{}^{\prime }}$
$v$旋转$\theta$后旋转轴$n$的分量	$v_{\Vert }^{{}^{\prime }}$
$w$向量，垂直于$n$与$v$所在的平面，且与 $n$、$v$均正交	$w$

2. 推导

假定空间中一个点$P$，构成向量 $v=\overrightarrow{OP}$&，对其进行分解：
$$v=v_{\perp }+v_{\Vert }$$
同理旋转后的向量：
$$v^{{}^{\prime }}=v_{\perp }^{{}^{\prime }}+v_{\Vert }^{{}^{\prime }}$$
显然：
$$v_{\Vert }=v_{\Vert }^{{}^{\prime }}$$
所以有：
$$v^{{}^{\prime }}=v_{\perp }^{{}^{\prime }}+v_{\Vert }$$
又向量$w$满足：
$$w=n\times v$$
那么：
$$|w|=|n\times v|$$
由于$|w|$代表黄色平行四边形阴影面积，而面积计算也可以使用 $|n|\ast |v_{\perp }|$，而$n$为单位向量，所以有
$$|w|=|n\times v|=|v_{\perp }|$$
由于$n\times w$与$v_{\perp }$相反，我们可以用$n$和$v$表示出$v_{\perp }$和$v_{\Vert }$，即：
$$v_{\perp }=-n\times w=-n\times (n\times v)$$
$$v_{\Vert }=v-v_{\perp }=v+n\times (n\times v)$$
由于$v_{\perp }$与向量$w$合成即为$v_{\perp }^{{}^{\prime }}$，所以有：
$$v_{\perp }^{{}^{\prime }}=v_{\perp }+w=cos(\theta )\ast v_{\perp }+sin(\theta )\ast w$$
将上式带入有：
$$\begin{array}{rl}{v}^{{}^{\prime }}& =v_{\Vert }+v_{\perp }^{{}^{\prime }}\\ & =(v-v_{\perp })+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^{\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\\ & =[I+(1-cos(\theta ))\ast N^2+sin(\theta )\ast N]\ast v\end{array}$$
所以有：
$$R=[I+(1-cos(\theta ))\ast N^2+sin(\theta )\ast 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_{1}2n_3& n_3^2\end{array}\right]$$
$$n^{\wedge }=\left[\begin{array}{ccc}0& -n_3& n_2\\ n_3& 0& -n_1\\ -n_2& n_1& 0\end{array}\right]=N$$
$$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_{1}2n_3& -n_1^2-n_2^2\end{array}\right]$$
$$n\ast n^T=I+N\ast N$$
将上式带入表达式一，则有：
$$R=cos(\theta )\ast I+(1-cos(\theta ))\ast n\ast n^T+sin(\theta )\ast n^{\wedge }$$

三、公式应用及深入理解

罗德里格斯旋转公式用于旋转三维空间的向量.它需要知道旋转轴和旋转角度。
罗德里格斯旋转公式，将3D旋转表示成绕空间中某一旋转轴$n$旋转角度$\theta$的形式，一般记作：
$$q=\theta \ast n=\left[\begin{array}{c}{q}_x\\ q_y\\ q_z\end{array}\right]$$
式中，$\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}$$

四、极限方式推导

一次性绕旋转轴$n$旋转角度$\theta$，等价于绕旋转轴$n$旋转$k$次，每次旋转角度为$\theta /k$。
因此：
$$R(n,\theta )=\underset{k\to +\infty }{\lim }(I+\frac{1}{k}\ast \theta \ast N{)}^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!}+\cdots$$
由于：
$$N^{k+2}=-N^k,k>0$$
所以上式可简化为：
$$\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}$$