旋转向量_旋转矩阵_四元数_欧拉角

0.引言

经常查，总结一下。推导看原论文。

1.旋转矩阵与旋转向量

1.1.旋转向量转旋转矩阵(指数映射）

旋转向量(角速度乘以$\Delta t$：
$$\mathbf{v}=\mathbf{\omega }\Delta t=\varphi \mathbf{u}$$旋转向量转旋转矩阵，指数映射：$$\mathrm{Exp}(\mathbf{v})\triangleq \exp \left([\mathbf{v}{]}_{\times }\right)$$即是：$$\mathbf{R}=e^{\varphi [\mathbf{u}{]}_{\times }}=\mathbf{I}+\varphi [\mathbf{u}{]}_{\times }+\frac{1}{2}{\varphi }^2[\mathbf{u}{]}_{\times }^2+\frac{1}{3!}{\varphi }^3[\mathbf{u}{]}_{\times }^3+\frac{1}{4!}{\varphi }^4[\mathbf{u}{]}_{\times }^4+\dots$$整理得（罗德里格斯公式）：
$$\mathbf{R}=\mathbf{I}\cos \varphi +[\mathbf{u}{]}_{\times }\sin \varphi +{\mathbf{u}\mathbf{u}}^{\top }(1-\cos \varphi )$$

1.2.旋转矩阵转旋转向量(对数映射)

$$\log :SO(3)\to \mathfrak{s}\mathfrak{o}(3);\mathbf{R}\mapsto \log (\mathbf{R})=[\mathbf{u}\varphi {]}_{\times }$$$$\log (\mathbf{R})\triangleq (\log (\mathbf{R}){)}^{\vee }$$$$\varphi =\arccos \left(\frac{\mathrm{trace}(\mathbf{R})-1}{2}\right)$$$$\mathbf{u}=\frac{{\left(\mathbf{R}-{\mathbf{R}}^{\top }\right)}^{\vee }}{2\sin \varphi }$$

2.四元数与旋转向量

2.1.旋转向量转四元数(指数映射)

旋转向量(角速度乘以$\Delta t$：
$$\mathbf{v}=\mathbf{\omega }\Delta t=\varphi \mathbf{u}$$
指数运算(为啥要除以2参考原论文推导)：
$$\mathrm{Exp}(\mathbf{v})\triangleq \exp (\mathbf{v}/2)$$$$\mathbf{q}\triangleq \mathrm{Exp}(\varphi \mathbf{u})=e^{\varphi \mathbf{u}/2}=\cos \frac{\varphi }{2}+\mathbf{u}\sin \frac{\varphi }{2}=\left[\begin{array}{c}\cos (\varphi /2)\\ \mathbf{u}\sin (\varphi /2)\end{array}\right]$$即是：$$q={\left[\cos \frac{\varphi }{2},u_x\sin \frac{\varphi }{2},u_y\sin \frac{\varphi }{2},u_z\sin \frac{\varphi }{2}\right]}^T$$

2.2.四元数转旋转向量(对数映射)

$$\log :S^3\to {\mathbb{H}}_p;\mathbf{q}\mapsto \log (\mathbf{q})=\mathbf{u}\theta$$$$\log (\mathbf{q})\triangleq 2\log (\mathbf{q})$$$$\begin{array}{rl}\varphi & =2\arctan \left(\Vert {\mathbf{q}}_v\Vert ,q_w\right)\\ \mathbf{u}& ={\mathbf{q}}_v/\Vert {\mathbf{q}}_v\Vert \end{array}$$

3.四元数与旋转矩阵

$$\mathbf{q}\otimes \mathbf{x}\otimes {\mathbf{q}}^{\ast }=\mathbf{R}\mathbf{x}$$

3.1.四元数转旋转矩阵

$$\mathbf{q}\otimes \mathbf{x}\otimes {\mathbf{q}}^{\ast }={\left[{\mathbf{q}}^{\ast }\right]}_R[\mathbf{q}{]}_L\left[\begin{array}{l}0\\ \mathbf{x}\end{array}\right]=\left[\begin{array}{c}0\\ \mathbf{R}\mathbf{x}\end{array}\right]$$

$$\mathbf{R}=\left[\begin{array}{ccc}1-2q_2^2-2q_3^2& 2q_1{q}_2+2q_0{q}_3& 2q_1{q}_3-2q_0{q}_2\\ 2q_1{q}_2-2q_0{q}_3& 1-2q_1^2-2q_3^2& 2q_2{q}_3+2q_0{q}_1\\ 2q_1{q}_3+2q_0{q}_2& 2q_2{q}_3-2q_0{q}_1& 1-2q_1^2-2q_2^2\end{array}\right]$$$$\mathbf{R}=\left(q_w^2-\left({\mathbf{q}}_v^{\top }{\mathbf{q}}_{}\right)\right)\mathbf{I}+2{\mathbf{q}}_v{\mathbf{q}}_v^{\top }+2q_w{\left[{\mathbf{q}}_v\right]}_{\times }$$

3.2.旋转矩阵转四元数

$$q_0=\frac{\sqrt{\mathrm{tr}(R)+1}}{2},q_1=\frac{m_{23}-m_{32}}{4q_0},q_2=\frac{m_{31}-m_{13}}{4q_0},q_3=\frac{m_{12}-m_{21}}{4q_0}$$

4.欧拉角与旋转矩阵

4.1.欧拉角转旋转矩阵

某次旋转绕固定坐标轴X-Y-Z旋转(γ,β,α)或者说绕自身坐标轴Z-Y-X旋转(α,β,γ)，对应的旋转矩阵如下

$$R=R_Z(\alpha )R_Y(\beta )R_X(\gamma )=\left(\begin{array}{ccc}cos\alpha cos\beta & cos\alpha sin\beta sin\gamma -sin\alpha cos\gamma & cos\alpha sin\beta cos\gamma +sin\alpha sin\gamma \\ sin\alpha cos\beta & cos\alpha cos\gamma +sin\alpha sin\beta sin\gamma & sin\alpha sin\beta cos\gamma -sin\gamma cos\alpha \\ -sin\beta & cos\beta sin\gamma & cos\beta cos\gamma \end{array}\right)$$

4.2.旋转矩阵转欧拉角

有旋转矩阵

$$R=\left(\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right)$$

可求出各轴的旋转角为

$$\begin{array}{c}{\theta }_Z=atan2(r_{21},r_{11})\\ {\theta }_Y=atan2(-r_{31},\sqrt{r_{31}^2+r_{33}^2})\\ {\theta }_X=atan2(r_{32},r_{33})\end{array}$$

atan2为C++中函数，atan2(y,x)的做法：当 x 的绝对值比 y 的绝对值大时使用 atan(y/x)；反之使用 atan(x/y)。这样就保证了数值稳定性。需要注意的是，旋转顺序必须要是Z、Y、X。

5.欧拉角与四元数

5.1.欧拉角转四元素

将Z-Y-X欧拉角（或RPY角：绕固定坐标系的X-Y-Z依次旋转α,β,γ角）转换为四元数：

$$\text{q}=\left(\begin{array}{c}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right)=\left(\begin{array}{c}cos(\frac{\alpha }{2})cos(\frac{\beta }{2})cos(\frac{\gamma }{2})+sin(\frac{\alpha }{2})sin(\frac{\beta }{2})sin(\frac{\gamma }{2})\\ sin(\frac{\alpha }{2})cos(\frac{\beta }{2})cos(\frac{\gamma }{2})-cos(\frac{\alpha }{2})sin(\frac{\beta }{2})sin(\frac{\gamma }{2})\\ cos(\frac{\alpha }{2})sin(\frac{\beta }{2})cos(\frac{\gamma }{2})+sin(\frac{\alpha }{2})cos(\frac{\beta }{2})sin(\frac{\gamma }{2})\\ cos(\frac{\alpha }{2})cos(\frac{\beta }{2})sin(\frac{\gamma }{2})-sin(\frac{\alpha }{2})sin(\frac{\beta }{2})cos(\frac{\gamma }{2})\end{array}\right)$$

5.2.四元数转欧拉角

由四元数 $q=(q0,q1,q2,q3)$ 或 $q=(w,x,y,z)$ 到欧拉角的转换为

$$\begin{gathered} \alpha =arctan(\frac{2(q_0{q}_1+q_2{q}_3)}{1-2(q_1^2+q_2^2)}) \\ \beta =arcsin(2(q_0{q}_2-q_1{q}_3)) \\ \gamma =arctan(\frac{2(q_0{q}_3+q_1{q}_2)}{1-2(q_2^2+q_3^2)}) \end{gathered}$$

由于arctan和arcsin的取值范围在−π/2 - π/2之间，只有180°，而绕某个轴旋转时范围是360°，因此要使用atan2函数代替arctan函数。

$$\begin{gathered} \alpha =atan2(2(q_0{q}_1+q_2{q}_3),1-2(q_1^2+q_2^2)) \\ \beta =arcsin(2(q_0{q}_2-q_1{q}_3)) \\ \gamma =atan2(2(q_0{q}_3+q_1{q}_2),1-2(q_2^2+q_3^2)) \end{gathered}$$
最后这个网站 可以动态展示四元数与欧拉角的对应关系和变化情况，便于理解。