利用四元数求解旋转矩阵

绕空间某一旋转轴$\vec{\omega}$旋转$\theta$角度的表示形式

如下图所示：

$O$表示坐标原点，$\vec{OR}$表示原始向量，$\vec{\omega}$表示旋转轴，为单位矢量，平面$v$表示垂直于$\vec{\omega}$并且包含向量${R}$的平面，$\vec{OR}$围绕$\vec{\omega}$旋转$\theta$角度之后得到旋转之后的向量$\vec{OR'}$，沿着$\vec{\omega}$的逆方向看去能够得到如右图所示的平面图，能够得到如下关系：

$$\vec{OR'}=\vec{OP}+\vec{PR}cos{\theta}+\vec{PQ}sin{\theta}$$

$$\vec{OP}=(\vec{OR}\cdot\vec{\omega})\vec{\omega}$$

$$\vec{PQ}=(\vec{\omega}\times\vec{OR})$$

$$\vec{PR}=\vec{OR}-(\vec{OR}\cdot\vec{\omega})\vec{\omega}$$

$$\vec{OR'}=(\vec{OR}\cdot\vec{\omega})\vec{\omega}+cos{}\theta(\vec{OR}-\vec{OR})\vec{\omega}+sin{\theta}(\vec{\omega}\times\vec{OR})$$

$$=cos{\theta}\vec{OR}+sin{\theta}\vec{\omega}\times\vec{OR}+(1-cos{\theta})(\vec{OR}\cdot\vec{\omega})\vec{\omega}\ \ \ \ (1)$$

已知空间中一向量$\vec{OR}$，沿着$\vec{\omega}$旋转$\theta$角度后得到$\vec{OR'}$；$(1)$式描述的即为$\vec{OR'}$与$\vec{OR}$、$\vec{\omega}$、以及$\theta$之间的关系，也即著名的$Rodrigues$公式。

四元数简介

​ 四元数由四个元素组成，给定一个标量$q_{0}$和一个矢量$\vec{q}=(q_{x},q_{y},q_{z})$，可以将其表示成一个四元数$q=(q_{0},\vec{q})$，用复数的记号表示为$q=q_{0}+iq_{x}+jq_{y}+kq_{z}$，这里$i^2=j^2=k^2=-1$，$ij=k$，$jk=i$，$ki=j$，将两个四元数$q_{1}=(q_{10},\vec{q_{1}})$与$q_{2}=(q_{20},\vec{q_{2}})$的乘积展开能够得到如下关系成立

$$q_{1}\cdot{q_{2}}=(q_{10}q_{20}-\vec{q_{1}}\cdot\vec{q_{2}},\vec{q_{1}}\times\vec{q_{2}}+q_{20}\vec{q_{1}}+q_{10}\vec{q_{20}})$$

​ 四元数$q=(q_{0},\vec{q})$的共轭定义为：$\overline{q}=(q_{0},-\vec{q})$

​ 四元数的模定义为：$\left|{q}\right|^2=q\cdot\overline{q}=q^2+\left|{\vec{q}}\right|^2$

​ 四元数长度可分离的特性：$\left|{q\cdot{q'}}\right|^2=\left|{q}\right|^2\left|{q'}\right|^2$

用四元数表示对应向量之间的旋转关系

​ 回到上述推导得到的$(1)$式:

$$\vec{OR'}=cos{\theta}\vec{OR}+sin{\theta}\vec{\omega}\times\vec{OR}+(1-cos{\theta})(\vec{OR}\cdot\vec{\omega})\vec{\omega}$$
​ 现在将变量 $\vec{OR}$、 $\vec{OR'}$以及 $\vec{\omega}$重新用四元数表示:
$$r=(0,\vec{R})$$

$$r'=(0,\vec{R'})$$

$$q=(q_{0},\vec{q})=(cos\frac{\theta}{2},sin\frac{\theta}{2}\vec{\omega})$$

​ 根据四元数乘法的定义，它们之间的关系可以推导为

$$r'=q\cdot{r}\cdot{\overline{q}}$$
​ 将上述等式按照四元数乘法定义展开，即能够得到 $(1)$式。

利用四元数求解旋转矩阵

​ ${d_{i}}'=(0,\vec{{d_{i}}'})$,${d_{i}}=(0,\vec{{d_{i}}})$，为两组数据中的对应点，两组数据仅存在旋转，则存在关系：

$${{d_{i}}’=qd_{i}\overline{q}}$$
​ 若要求得空间中两组对应点之间的旋转四元数，则建立以下目标优化函数：

$$E^2=\sum{\left| {{d_{i}}’-qd_{i}\overline{q}}\right|}^2=\sum{\left| {{d_{i}}'-qd_{i}\overline{q}}\right|}^2\left|{q}\right|\left|{\overline{q}}\right|=\sum{\left|{{d_{i}}'}q-qd_{i}\right|}^2=\sum{\left|{O_{i}}\right|^2} \ \ \ \ \ \ (2)$$

​ 把$q$代入上式，则有:

$$O_{i}=[\vec{q}\cdot(\vec{d_{i}}-\vec{{d_{i}}'}),(\vec{{d_{i}}'}+\vec{d_{i}})\times\vec{q}+q_{0}(\vec{{d_{i}}'}-\vec{d_{i}})]$$
​ 其中$(\vec{{d_{i}}'}+\vec{d_{i}})\times\vec{q}$可以写成反对称矩阵的形式：$\vec{q}\cdot\vec{D_{i}}$，$\vec{D_{i}}$为对应于$(\vec{{d_{i}}'}+\vec{d_{i}})$的反对称矩阵的转置。

​ 令$O_{i}=\vec{q_{4}}\cdot{A_{i}}$，其中$\vec{q_{4}}$是四元数$q$转化成的四维向量；而

$$A_{i}=\left[ \begin{matrix} 0 & \vec{d_{i}}-\vec{{d_{i}}’}\\ (\vec{{d_{i}}’} -\vec{d_{i}})^T& D_{i} \end{matrix} \right]$$
​ 注意到$A_{i}$仅与采集到的数据有关；$(2)$式可写为

$$E^2=\sum{\left|O_{i}\right|^2}=\sum{\vec{q_{4}}A_{i}A_{i}^T\vec{q_{4}^T}}$$
​ $\vec{q_{4}}$与 $i$无关，所以上式变为：
$$\vec{q_{4}}(\sum{A_{i}A_{i}^T})\vec{q_{4}^T}=\vec{q_{4}}B\vec{q_{4}^T}$$
​ 其中，
$$B=\sum{A_{i}A_{i}^T}$$
​ 此时 $E^2$最小化问题就是典型的一次函数最小化问题，其最小解即为对应于 $B$的最小特征值对应的特征向量；得到$q_{4}$之后，即可得到四元数 $q=(cos\frac{\theta}{2},sin\frac{\theta}{2}\vec{\omega})$，从而得到旋转矩阵。