一、旋转矩阵，旋转向量，单位四元数的相互转换总结

---

前言

研究旋转矩阵，旋转向量，单位四元数，都是为了表达机器人的姿态。欧拉角在SLAM的应用中不多，就不涉及了。

---

一、要点

1. 旋转矩阵

• 旋转矩阵是行列式为1的正交矩阵，由单位正交基组成。
  正交矩阵的行列式为正负1
• 旋转矩阵是两个坐标系的过渡矩阵，其左乘坐标系1中的某向量的坐标，可以得到将此向量过渡到坐标系2后的坐标表示。
• n维空间的旋转矩阵构成特殊正交群。
• 优点：是计算非常方便，矩阵乘法非常简单。
• 缺点：1.表达方式冗余，旋转只有3个自由度，却要用9个量表示。2.带有正交，行列式为1的约束，不利于优化。

2. 旋转向量

• 旋转向量又称为轴角，其方向与旋转轴$\mathbf{n}$一致，而长度等于旋转角$\theta$。向量$\theta \mathbf{n}$即可表示旋转。
• 不严谨的说，这玩意就是李代数。
• 优点：表达紧凑；没有约束易于优化。
• 缺点：不能直接用来计算旋转一个向量，需要转换为旋转矩阵或四元数才能做这个计算；具有奇异性，模长大于$2\pi$时具有周期性。
  所谓奇异性，我理解的就是做不到一一对应，在这里是一个姿态可以对应多个旋转向量

3. 单位四元数

• 四元数的运算基本与复数的运算是类似的，唯一的区别是四元数的乘法不可交换（由于乘法公式的最后项有个叉乘）。
• 用四元数表示旋转的三个要点：
  • 实部唯一表示了旋转角；
  • 虚部所指方向即旋转轴的方向，（注意：虚部在${\mathbb{R}}^3$里不需要是一个单位向量）；
  • 实部和虚部一起要服从是单位四元数的约束，即模为1。
• 单位四元数的逆等于其共轭。
• 优点：没有奇异性，将要旋转的向量变成纯虚数后可以直接用四元数乘法计算。
• 缺点：不够直观。

---

二、旋转向量—>旋转矩阵（罗德里格斯公式）

在上图中，向量$\mathbf{v}$沿着旋转轴$\mathbf{n}$旋转$\theta$角，变为${\mathbf{v}}^{\prime }$。现在我们要将${\mathbf{v}}^{\prime }$分解成向量$\mathbf{v}$，旋转轴$\mathbf{n}$，旋转角$\theta$组成的量
$$\begin{array}{rl}{\mathbf{v}}^{\prime }& ={\mathbf{v}}_k+{\mathbf{v}}_p^{\prime }\\ & ={\mathbf{v}}_k+\mathbf{a}+\mathbf{b}\\ & =(\mathbf{v}\cdot \mathbf{n})\mathbf{n}+|{\mathbf{v}}_i|\sin \theta \mathbf{j}+{\mathbf{v}}_i\cos \theta \\ & =(\mathbf{v}\cdot \mathbf{n})\mathbf{n}+|(\mathbf{n}\times \mathbf{v})\times \mathbf{n}|\sin \theta \frac{\mathbf{n}\times \mathbf{v}}{|\mathbf{n}\times \mathbf{v}|}+((\mathbf{n}\times \mathbf{v})\times \mathbf{n})\cos \theta \\ & =(\mathbf{v}\cdot \mathbf{n})\mathbf{n}+|\mathbf{n}||\mathbf{v}|\sin \alpha |\mathbf{n}|\sin \frac{\pi }{2}\sin \theta \frac{\mathbf{n}\times \mathbf{v}}{|\mathbf{n}||\mathbf{v}|\sin \alpha }+((\mathbf{n}\times \mathbf{v})\times \mathbf{n})\cos \theta \\ & =(\mathbf{v}\cdot \mathbf{n})\mathbf{n}+(\mathbf{n}\times \mathbf{v})\sin \theta +(\mathbf{v}-(\mathbf{v}\cdot \mathbf{n})\mathbf{n})\cos \theta \\ & =(1-\cos \theta )(\mathbf{v}\cdot \mathbf{n})\mathbf{n}+{\mathbf{n}}^{\wedge }\mathbf{v}\sin \theta +\mathbf{v}\cos \theta \\ & =(1-\cos \theta )\mathbf{n}{\mathbf{n}}^{\mathrm{T}}\mathbf{v}+\sin \theta {\mathbf{n}}^{\wedge }\mathbf{v}+\cos \theta \mathbf{v}\\ & =[\cos \theta \mathbf{I}+(1-\cos \theta )\mathbf{n}{\mathbf{n}}^{\mathrm{T}}+\sin \theta {\mathbf{n}}^{\wedge }]\mathbf{v}\end{array}$$
要注意点乘得到的是标量；叉乘得到的是向量，且要用右手定则分析其方向。

$(\mathbf{v}\cdot \mathbf{n})\mathbf{n}=\mathbf{n}{\mathbf{n}}^{\mathrm{T}}\mathbf{v}$可以从微观层面去证明，很简单。
上面的计算有很多细节值得思考，比如第三行的$|{\mathbf{v}}_i|\sin \theta \mathbf{j}$为什么要乘单位向量$\mathbf{j}$？${\mathbf{v}}_i\cos \theta$为什么又不乘？

上式中中括号里的内容是一个矩阵，就是旋转矩阵$\mathbf{R}$：
$$\begin{array}{c}\mathbf{R}=\cos \theta \mathbf{I}+(1-\cos \theta )\mathbf{n}{\mathbf{n}}^{\mathrm{T}}+\sin \theta {\mathbf{n}}^{\wedge }\end{array}$$

---

三、旋转矩阵—>旋转向量

将(1)式两边取迹：
$$\begin{array}{rl}\mathrm{t}r(\mathbf{R})& =3\cos \theta +(1-\cos \theta ){\mathbf{n}}^{\mathrm{T}}\mathbf{n}+\sin \theta \cdot 0\\ & =3\cos \theta +1-\cos \theta \\ & =2\cos \theta +1\end{array}$$
从旋转矩阵中得到旋转角：
$$\theta =\arccos \frac{\mathrm{t}r(\mathbf{R})-1}{2}$$
至于转轴$\mathbf{n}$，把它看作普通的向量，用它对应的旋转矩阵$\mathbf{R}$去左乘旋转它，它不会发生任何变化，于是有下式：
$$\mathbf{Rn}=\mathbf{n}$$
因此，转轴$\mathbf{n}$是矩阵$\mathbf{R}$的特征值1对应的特征向量，用求特征向量的方法去求即可，再归一化可以得到转轴$\mathbf{n}$:
$$\mathbf{Rn}=\mathbf{n}\text{s.t.}{\mathbf{n}}^{\mathrm{T}}\mathbf{n}=1$$

---

四、单位四元数—>旋转矩阵

用四元数$\mathbf{q}$对纯虚四元数数表示的空间点$\mathbf{p}=[0,{\mathbf{v}}_p^{\mathrm{T}}{]}^{\mathrm{T}}$进行旋转有：
$$\begin{array}{rl}{\mathbf{p}}^{{}^{\prime }}& =\mathbf{qp}{\mathbf{q}}^{-1}\\ & ={\mathbf{q}}^{+}{\mathbf{p}}^{+}{\mathbf{q}}^{-1}\\ & ={\mathbf{q}}^{+}({\mathbf{q}}^{-1}{)}^{\oplus }\mathbf{p}\\ & =\left[\begin{array}{cc}s& -{\mathbf{v}}^{\mathrm{T}}\\ \mathbf{v}& s\mathbf{I}+{\mathbf{v}}^{\wedge }\end{array}\right]\left[\begin{array}{cc}s& {\mathbf{v}}^{\mathrm{T}}\\ -\mathbf{v}& s\mathbf{I}+{\mathbf{v}}^{\wedge }\end{array}\right]\left[\begin{array}{c}0\\ {\mathbf{v}}_p\end{array}\right]\\ & =\left[\begin{array}{cc}{s}^2+{\mathbf{v}}^{\mathrm{T}}\mathbf{v}& s{\mathbf{v}}^{\mathrm{T}}-s{\mathbf{v}}^{\mathrm{T}}-{\mathbf{v}}^{\mathrm{T}}{\mathbf{v}}^{\wedge }\\ s\mathbf{v}-s\mathbf{v}-{\mathbf{v}}^{\wedge }\mathbf{v}& \mathbf{v}{\mathbf{v}}^{\mathrm{T}}+s^2\mathbf{I}+2s{\mathbf{v}}^{\wedge }+({\mathbf{v}}^{\wedge }{)}^2\end{array}\right]\left[\begin{array}{c}0\\ {\mathbf{v}}_p\end{array}\right]\\ & =\left[\begin{array}{cc}1& ({\mathbf{v}}^{\wedge }\mathbf{v}{)}^{\mathrm{T}}\\ 0& \mathbf{v}{\mathbf{v}}^{\mathrm{T}}+s^2\mathbf{I}+2s{\mathbf{v}}^{\wedge }+({\mathbf{v}}^{\wedge }{)}^2\end{array}\right]\left[\begin{array}{c}0\\ {\mathbf{v}}_p\end{array}\right]\\ & =\left[\begin{array}{cc}1& 0^{\mathrm{T}}\\ 0& \mathbf{v}{\mathbf{v}}^{\mathrm{T}}+s^2\mathbf{I}+2s{\mathbf{v}}^{\wedge }+({\mathbf{v}}^{\wedge }{)}^2\end{array}\right]\left[\begin{array}{c}0\\ {\mathbf{v}}_p\end{array}\right]\\ & =\left[\begin{array}{c}0\\ (\mathbf{v}{\mathbf{v}}^{\mathrm{T}}+s^2\mathbf{I}+2s{\mathbf{v}}^{\wedge }+({\mathbf{v}}^{\wedge }{)}^2){\mathbf{v}}_p\end{array}\right]\end{array}$$
上面可以看出被单位四元数旋转后的坐标点(纯虚四元数)依然是一个纯虚四元数
由此可得单位四元数到旋转矩阵的变换：
$$\begin{array}{c}\mathbf{R}=\mathbf{v}{\mathbf{v}}^{\mathrm{T}}+s^2\mathbf{I}+2s{\mathbf{v}}^{\wedge }+({\mathbf{v}}^{\wedge }{)}^2\end{array}$$

---

五、旋转矩阵—>单位四元数

将(3)式两边取迹：
$$\mathrm{t}r(\mathbf{R})={\mathbf{v}}^{\mathrm{T}}\mathbf{v}+3s^2+0+\mathrm{t}r(({\mathbf{v}}^{\wedge }{)}^2)$$

其中： $$\begin{array}{rl}({\mathbf{v}}^{\wedge }{)}^2& =\left[\begin{array}{ccc}0& -v_3& v_2\\ v_3& 0& -v_1\\ -v_2& v_1& 0\end{array}\right]\left[\begin{array}{ccc}0& -v_3& v_2\\ v_3& 0& -v_1\\ -v_2& v_1& 0\end{array}\right]\\ & =\left[\begin{array}{ccc}-v_2^2-v_3^2& \cdots & \cdots \\ \cdots & -v_1^2-v_3^2& \cdots \\ \cdots & \cdots & -v_1^2-v_2^2\end{array}\right]\end{array}$$

$$\begin{array}{rl}\mathrm{t}r(\mathbf{R})& =v_1^2+v_2^2+v_3^2+3s^2-2(v_1^2+v_2^2+v_3^2)\\ & =3s^2-(v_1^2+v_2^2+v_3^2)\\ & =3s^2-(1-s^2)\\ & =4s^2-1\end{array}$$
稍作变换(4)式可以从旋转矩阵$\mathbf{R}$中求得单位四元数的实部：
$$s=\sqrt{\frac{\mathrm{t}r(\mathbf{R})+1}{4}}$$

下面开始求虚部，将虚部当作待旋转的坐标点带入(3)式有：
$$\begin{array}{rl}\mathbf{Rv}& =(\mathbf{v}{\mathbf{v}}^{\mathrm{T}}+s^2\mathbf{I}+2s{\mathbf{v}}^{\wedge }+({\mathbf{v}}^{\wedge }{)}^2)\mathbf{v}\\ & =\mathbf{v}{\mathbf{v}}^{\mathrm{T}}\mathbf{v}+s^2\mathbf{v}+0+0\\ & =(1-s^2)\mathbf{v}+s^2\mathbf{v}\\ & =\mathbf{v}\end{array}$$

由此可见虚部也是矩阵$\mathbf{R}$的特征值1对应的特征向量，即与旋转向量$\theta \mathbf{n}$在${\mathbb{R}}^3$中指向同一个方向，但是他们之间的模不一样，旋转向量的模是量转角$\theta$，这里虚部的模由$s^2+{\mathbf{v}}^{\mathrm{T}}\mathbf{v}=1$确定。故由旋转矩阵求虚部为
$$\mathbf{Rv}=\mathbf{v}\text{s.t.}{s}^2+{\mathbf{v}}^{\mathrm{T}}\mathbf{v}=1$$

---

六、单位四元数<—>旋转向量

由式(2)与式(4)可得：
$$\mathrm{t}r(\mathbf{R})=2\cos \theta +1=4s^2-1$$
即
$$\cos \theta =2s^2-1$$
又有三角函数关系
$$\cos \theta =2{\cos }^2\frac{\theta }{2}-1$$
于是有实部与转角的相互转换：
$$s=\cos \frac{\theta }{2}\text{或}\theta =2\arccos s$$
刚刚已经分析，单位四元数的虚部$\mathbf{v}$与旋转向量的转轴$\mathbf{n}$在${\mathbb{R}}^3$中指向同一个方向，区别是他们的模服从不同的约束。单位四元数转换为转轴$\mathbf{n}$：
$$\mathbf{n}=\frac{\mathbf{v}}{|\mathbf{v}|}=\frac{\mathbf{v}}{\sqrt{1-s^2}}=\frac{\mathbf{v}}{\sqrt{1-{\cos }^2\frac{\theta }{2}}}=\frac{\mathbf{v}}{\sin \frac{\theta }{2}}$$
旋转向量转换为虚部$\mathbf{v}$：
$$\mathbf{v}=\sin \frac{\theta }{2}\mathbf{n}=\sqrt{1-s^2}\mathbf{n}$$

---

总结

	旋转矩阵$\mathbf{R}$	旋转向量$\theta \mathbf{n}$	单位四元数$\mathbf{q}=[s,\mathbf{v}]$
旋转矩阵$\mathbf{R}$		$\mathbf{R}=\cos \theta \mathbf{I}+(1-\cos \theta )\mathbf{n}{\mathbf{n}}^{\mathrm{T}}+\sin \theta {\mathbf{n}}^{\wedge }$	$\mathbf{R}=\mathbf{v}{\mathbf{v}}^{\mathrm{T}}+s^2\mathbf{I}+2s{\mathbf{v}}^{\wedge }+({\mathbf{v}}^{\wedge }{)}^2$
旋转向量$\theta \mathbf{n}$	$$\begin{gathered} \theta =\arccos \frac{\mathrm{t}r(\mathbf{R})-1}{2} \\ \mathbf{Rn}=\mathbf{n}\text{s.t.}{\mathbf{n}}^{\mathrm{T}}\mathbf{n}=1 \end{gathered}$$		$$\begin{gathered} \theta =2\arccos s \\ \mathbf{n}=\frac{\mathbf{v}}{|\mathbf{v}|}=\frac{\mathbf{v}}{\sqrt{1-s^2}}=\frac{\mathbf{v}}{\sin \frac{\theta }{2}} \end{gathered}$$
单位四元数$\mathbf{q}=[s,\mathbf{v}]$	$$\begin{gathered} s=\sqrt{\frac{\mathrm{t}r(\mathbf{R})+1}{4}} \\ \mathbf{Rv}=\mathbf{v}\text{s.t.}{s}^2+{\mathbf{v}}^{\mathrm{T}}\mathbf{v}=1 \end{gathered}$$	$$\begin{gathered} s=\cos \frac{\theta }{2} \\ \mathbf{v}=\sin \frac{\theta }{2}\mathbf{n}=\sqrt{1-s^2}\mathbf{n} \end{gathered}$$