欧拉角和四元数的相互转换

设四元数为:
$$q=(w,x,y,z)$$

对应欧拉角为:

• 俯仰角(pitch):$\theta$
• 滚转角(roll):$\varphi$
• 偏航角(yaw):$\psi$
  

则四元数到欧拉角的转换公式为:

欧拉角到四元数

$$\begin{array}{rl}w& =\cos (\frac{\theta }{2})\cos (\frac{\varphi }{2})\cos (\frac{\psi }{2})+\sin (\frac{\theta }{2})\sin (\frac{\varphi }{2})\sin (\frac{\psi }{2})\\ x& =\sin (\frac{\theta }{2})\cos (\frac{\varphi }{2})\cos (\frac{\psi }{2})-\cos (\frac{\theta }{2})\sin (\frac{\varphi }{2})\sin (\frac{\psi }{2})\\ y& =\cos (\frac{\theta }{2})\sin (\frac{\varphi }{2})\cos (\frac{\psi }{2})+\sin (\frac{\theta }{2})\cos (\frac{\varphi }{2})\sin (\frac{\psi }{2})\\ z& =\cos (\frac{\theta }{2})\cos (\frac{\varphi }{2})\sin (\frac{\psi }{2})-\sin (\frac{\theta }{2})\sin (\frac{\varphi }{2})\cos (\frac{\psi }{2})\end{array}$$

四元数到欧拉角

$$\begin{array}{rl}\theta & =\arcsin (2(w\cdot y-z\cdot x))\\ \varphi & =\arctan 2(2(w\cdot x+y\cdot z),1-2(x^2+y^2))\\ \psi & =\arctan 2(2(w\cdot z+x\cdot y),1-2(y^2+z^2))\end{array}$$