四元数和欧拉角转换

### 四元数与欧拉角之间的转换方法及公式 四元数和欧拉角是两种常用的旋转表示方式。四元数通过四个参数（一个标量和一个三维向量）来描述旋转，能够避免万向节死锁问题，适合插值和高效计算。欧拉角则通过三个角度（通常为滚转角、俯仰角、偏航角）描述绕坐标轴的连续旋转，直观但存在万向节死锁风险。以下分别介绍它们之间的转换方法及公式。 #### 1. 从欧拉角到四元数的转换 假设欧拉角使用 **ZYX 旋转顺序**（即先绕 z 轴旋转偏航角 ψ，再绕 y 轴旋转俯仰角 θ，最后绕 x 轴旋转滚转角 φ），则对应的四元数可以通过以下公式计算： $$ q = q_z \cdot q_y \cdot q_x $$ 其中： - $ q_x = \cos\left(\frac{\phi}{2}\right) + \sin\left(\frac{\phi}{2}\right) \cdot i $ - $ q_y = \cos\left(\frac{\theta}{2}\right) + \sin\left(\frac{\theta}{2}\right) \cdot j $ - $ q_z = \cos\left(\frac{\psi}{2}\right) + \sin\left(\frac{\psi}{2}\right) \cdot k $ 将它们相乘后，四元数的四个分量可表示为： $$ \begin{aligned} w &= \cos\left(\frac{\phi}{2}\right)\cos\left(\frac{\theta}{2}\right)\cos\left(\frac{\psi}{2}\right) + \sin\left(\frac{\phi}{2}\right)\sin\left(\frac{\theta}{2}\right)\sin\left(\frac{\psi}{2}\right) \\ x &= \sin\left(\frac{\phi}{2}\right)\cos\left(\frac{\theta}{2}\right)\cos\left(\frac{\psi}{2}\right) - \cos\left(\frac{\phi}{2}\right)\sin\left(\frac{\theta}{2}\right)\sin\left(\frac{\psi}{2}\right) \\ y &= \cos\left(\frac{\phi}{2}\right)\sin\left(\frac{\theta}{2}\right)\cos\left(\frac{\psi}{2}\right) + \sin\left(\frac{\phi}{2}\right)\cos\left(\frac{\theta}{2}\right)\sin\left(\frac{\psi}{2}\right) \\ z &= \cos\left(\frac{\phi}{2}\right)\cos\left(\frac{\theta}{2}\right)\sin\left(\frac{\psi}{2}\right) - \sin\left(\frac{\phi}{2}\right)\sin\left(\frac{\theta}{2}\right)\cos\left(\frac{\psi}{2}\right) \end{aligned} $$ #### 2. 从四元数到欧拉角的转换 假设四元数为 $ q = w + xi + yj + zk $，则对应的欧拉角（ZYX 顺序）可通过以下公式计算： $$ \begin{aligned} \phi &= \arctan2\left(2(w x + y z), 1 - 2(x^2 + y^2)\right) \\ \theta &= \arcsin\left(2(w y - z x)\right) \\ \psi &= \arctan2\left(2(w z + x y), 1 - 2(y^2 + z^2)\right) \end{aligned} $$ 上述公式中需要注意以下几点： - 使用 `arctan2` 而不是 `arctan` 以确保角度在正确象限。 - 当俯仰角 θ 接近 ±90°（即万向节死锁状态）时，需要特殊处理以避免数值不稳定。 #### 3. 代码实现示例 以下是一个 Python 实现的从欧拉角到四元数的转换函数： ```python import math def euler_to_quaternion(roll, pitch, yaw): cy = math.cos(yaw * 0.5) sy = math.sin(yaw * 0.5) cp = math.cos(pitch * 0.5) sp = math.sin(pitch * 0.5) cr = math.cos(roll * 0.5) sr = math.sin(roll * 0.5) w = cr * cp * cy + sr * sp * sy x = sr * cp * cy - cr * sp * sy y = cr * sp * cy + sr * cp * sy z = cr * cp * sy - sr * sp * cy return (w, x, y, z) ``` 以下是从四元数到欧拉角的转换函数： ```python def quaternion_to_euler(w, x, y, z): sinr_cosp = 2 * (w * x + y * z) cosr_cosp = 1 - 2 * (x * x + y * y) roll = math.atan2(sinr_cosp, cosr_cosp) sinp = 2 * (w * y - z * x) if abs(sinp) >= 1: pitch = math.copysign(math.pi / 2, sinp) # 处理万向节死锁情况 else: pitch = math.asin(sinp) siny_cosp = 2 * (w * z + x * y) cosy_cosp = 1 - 2 * (y * y + z * z) yaw = math.atan2(siny_cosp, cosy_cosp) return (roll, pitch, yaw) ``` #### 4. 注意事项 - 不同领域对欧拉角的旋转顺序和坐标轴定义可能不同（如航空航天使用 ZYX，而某些机器人系统使用 ZXY），因此在实际应用中需确保旋转顺序和坐标系定义一致。 - 万向节死锁问题在欧拉角中不可避免，因此在需要连续旋转或插值的应用中，建议优先使用四元数[^3]。 ---