Rodrigues formula

本篇文章主要是解释 如何从数学上刻画 一个向量绕一个轴旋转 得到最终的向量的表达式，主要内容可参见Rodrigues formula.

从2D 谈起

不失一般性，我们考虑下图， 向量1，坐标为${\mathbf{p}}_1=(x,y)$ 绕$x$轴 （水平轴） 逆时针旋转$\theta$角得到向量2，坐标为${\mathbf{p}}_2=(x^{\prime },y^{\prime })$，此时向量2在该坐标系下的表示如下图所示

Observation 1

该公式的推导非常简单，假设向量1与$x$轴的夹角为$\alpha$, 利用三角函数求和公式阔以直接得到，故此不再赘述。
观察${\mathbf{p}}^{\prime }=(x^{\prime },y^{\prime })$的表达式，跟欧拉公式
$$e^{j\theta }=\cos (\theta )+j\sin (\theta ),j^2=-1$$
联系起来，将$x$轴看作$Re$实轴，$y$轴看作$Im$轴，此时向量1可以用一个复数$p_1=x+jy$表示，类似地， 向量2可以用一个复数$p_2=x^{\prime }+j{y}^{\prime }$表示。利用复数乘法， 向量2可以表示为
$$p_2=e^{j\theta }{p}_1$$
此时，我们得到了复数乘法的几何意义，即：一个复数与$e^{j\theta }$ 相乘，这个复数的相位发生了$\theta$的逆时针偏转。
（注：值得注意的是，规定旋转的正方向很重要，这里我们规定逆时针为正方向，否则会造成模糊）

Observation 2

为了方便3D的公式， 我们需要将向量${\mathbf{p}}_2$写成向量${\mathbf{p}}_1$的形式，观察上图中的公式
$$\begin{array}{r}\left[\begin{array}{c}{x}^{\prime }\\ y^{\prime }\end{array}\right]=\cos (\theta )\left[\begin{array}{c}x\\ y\end{array}\right]+\sin (\theta )\left[\begin{array}{c}-y\\ x\end{array}\right]=\cos (\theta )\mathbf{p}+\sin (\theta ){\mathbf{p}}_{\perp }\end{array}$$
其中${\mathbf{p}}_{\perp }=[-y,x{]}^T$ 与向量$\mathbf{p}$垂直，并且$\Vert {\mathbf{p}}_{\perp }{\Vert }_2=\Vert \mathbf{p}\Vert$。由此可见，旋转后的向量可以由旋转前的向量与其垂直的向量线性表示出来。

3D 向量旋转

Rodrigues formula

当我们考虑3维空间如下图所示， 选定旋转轴（ rotation axis)，其方向向量记为$\hat{\mathbf{n}}$, 并且其模值为1。

Special case

为了方便解释， 我们可以先考虑一个简单的场景: 向量$\overrightarrow{\mathbf{v}}$垂直于旋转轴, 记逆时针旋转$\theta$之后的向量为${\overrightarrow{\mathbf{v}}}^{\prime }$。

如何得到${\overrightarrow{\mathbf{v}}}^{\prime }$的表达式呢？
为了回答这个问题，我们首先注意到 向量 ${\overrightarrow{\mathbf{v}}}^{\prime }$ 跟向量 $\overrightarrow{\mathbf{v}}$位于同一平面，我们可以用之前得到2D下的公式 来刻画旋转后的向量${\overrightarrow{\mathbf{v}}}^{\prime }$， 但是怎么得到跟向量$\overrightarrow{\mathbf{v}}$ 正交的向量呢？
注意到旋转轴$\hat{\mathbf{n}}$垂直于$\overrightarrow{\mathbf{v}}$， 根据右手法则，向量$\hat{\mathbf{n}}$叉乘$\overrightarrow{\mathbf{v}}$得到
$$（\hat{\mathbf{n}}\times \overrightarrow{\mathbf{v}}）\perp \overrightarrow{\mathbf{v}},\text{and }\Vert （\hat{\mathbf{n}}\times \overrightarrow{\mathbf{v}}）\Vert =\Vert \hat{\mathbf{n}}\Vert \overrightarrow{\mathbf{v}}\Vert \sin (\pi /2)=\Vert \overrightarrow{\mathbf{v}}\Vert$$
因此，向量${\overrightarrow{\mathbf{v}}}^{\prime }$可以表示为
$${\overrightarrow{\mathbf{v}}}^{\prime }=\cos (\theta )\overrightarrow{\mathbf{v}}+\sin (\theta )（\hat{\mathbf{n}}\times \overrightarrow{\mathbf{v}}）$$
由此可见，3D空间中，旋转后的向量 依然可以表示为原向量于其垂直向量的线性组合，且其系数分别为$\cos (\theta )$和$\sin (\theta )$。

任意向量的旋转

在上个小节，我们讨论了向量$\overrightarrow{\mathbf{v}}$ 垂直于旋转轴$\hat{\mathbf{n}}$的特殊情况, 若向量$\overrightarrow{\mathbf{v}}$于旋转轴不垂直呢？如下图所示，向量$\overrightarrow{\mathbf{v}}$可以做分解为
$$\overrightarrow{\mathbf{v}}={\overrightarrow{\mathbf{v}}}_{\parallel }+{\overrightarrow{\mathbf{v}}}_{\perp }$$

同理，旋转后的向量也可以由如下式子合成
$${\overrightarrow{\mathbf{v}}}^{\prime }={{\overrightarrow{\mathbf{v}}}^{\prime }}_{\parallel }+{{\overrightarrow{\mathbf{v}}}^{\prime }}_{\perp }$$
即，我们只需要找到对应的
$${{\overrightarrow{\mathbf{v}}}^{\prime }}_{\parallel }\mapsto {\overrightarrow{\mathbf{v}}}_{\parallel }\text{ and }{{\overrightarrow{\mathbf{v}}}^{\prime }}_{\perp }\mapsto {\overrightarrow{\mathbf{v}}}_{\perp }$$
的关系式，再拼起来就可以了。
乍一看好像多了步骤，但其实仔细看看就知道，${\overrightarrow{\mathbf{v}}}_{\parallel }$绕旋转轴不改变其方向和大小，而${\overrightarrow{\mathbf{v}}}_{\perp }$的公式我们已经从之前的special case中得到了。 也就是说
$$\begin{array}{rl}{\overrightarrow{\mathbf{v}}}^{\prime }=& {{\overrightarrow{\mathbf{v}}}^{\prime }}_{\parallel }+{{\overrightarrow{\mathbf{v}}}^{\prime }}_{\perp }\\ =& {\overrightarrow{\mathbf{v}}}_{\parallel }+\cos (\theta ){\overrightarrow{\mathbf{v}}}_{\perp }+\sin (\theta )(\hat{\mathbf{n}}\times {\overrightarrow{\mathbf{v}}}_{\perp })\\ \stackrel{(a)}{=}& {\overrightarrow{\mathbf{v}}}_{\parallel }+\cos (\theta )(\overrightarrow{\mathbf{v}}-{\overrightarrow{\mathbf{v}}}_{\parallel })+\sin (\theta )(\hat{\mathbf{n}}\times \overrightarrow{\mathbf{v}})\\ =& \cos (\theta )\overrightarrow{\mathbf{v}}+(1-\cos (\theta )){\overrightarrow{\mathbf{v}}}_{\parallel }+\sin (\theta )(\hat{\mathbf{n}}\times \overrightarrow{\mathbf{v}})\\ \stackrel{(b)}{=}& \cos (\theta )\overrightarrow{\mathbf{v}}+(1-\cos (\theta ))(\hat{\mathbf{n}}\cdot \overrightarrow{\mathbf{v}})\hat{\mathbf{n}}+\sin (\theta )(\hat{\mathbf{n}}\times \overrightarrow{\mathbf{v}})\end{array}$$

等号(a)中利用了向量分解
$${\overrightarrow{\mathbf{v}}}_{\perp }=\overrightarrow{\mathbf{v}}-{\overrightarrow{\mathbf{v}}}_{\parallel }$$
而等号(b)中利用向量
$${\overrightarrow{\mathbf{v}}}_{\parallel }=(\hat{\mathbf{n}}\cdot \overrightarrow{\mathbf{v}})\hat{\mathbf{n}}$$
(这里很简单，因为${\overrightarrow{\mathbf{v}}}_{\parallel }$方向与$\hat{\mathbf{n}}$相同，而其大小可以由两个向量的点积（即 投影）得到。由此，我们得到了Rodrigues 公式
$${\overrightarrow{\mathbf{v}}}^{\prime }=\cos (\theta )\overrightarrow{\mathbf{v}}+(1-\cos (\theta ))(\hat{\mathbf{n}}\cdot \overrightarrow{\mathbf{v}})\hat{\mathbf{n}}+\sin (\theta )(\hat{\mathbf{n}}\times \overrightarrow{\mathbf{v}})$$
记忆公式的时候，只要回想一遍推导过程就好了。

Reference:
[1] https://www.youtube.com/watch?v=q-ESzg03mQc&t=6s
[2]https://krasjet.github.io/quaternion/quaternion.pdf