欧拉角和四元数的转换

### 欧拉角与四元数之间的转换 #### 转换原理 欧拉角通常由三个角度组成，分别表示绕不同轴的旋转顺序（例如 ZYX 或 XYZ）。而四元数是一种紧凑且无奇异性的旋转表示方式。两者之间的转换涉及三角函数运算。 ##### 欧拉角转四元数 假设欧拉角为 \( (\text{roll}, \text{pitch}, \text{yaw}) \)，其单位为弧度，则对应的四元数计算如下： \[ q_w = \cos(\frac{\phi}{2}) \cdot \cos(\frac{\theta}{2}) \cdot \cos(\frac{\psi}{2}) + \sin(\frac{\phi}{2}) \cdot \sin(\frac{\theta}{2}) \cdot \sin(\frac{\psi}{2}) \] \[ q_x = \sin(\frac{\phi}{2}) \cdot \cos(\frac{\theta}{2}) \cdot \cos(\frac{\psi}{2}) - \cos(\frac{\phi}{2}) \cdot \sin(\frac{\theta}{2}) \cdot \sin(\frac{\psi}{2}) \] \[ q_y = \cos(\frac{\phi}{2}) \cdot \sin(\frac{\theta}{2}) \cdot \cos(\frac{\psi}{2}) + \sin(\frac{\phi}{2}) \cdot \cos(\frac{\theta}{2}) \cdot \sin(\frac{\psi}{2}) \] \[ q_z = \cos(\frac{\phi}{2}) \cdot \cos(\frac{\theta}{2}) \cdot \sin(\frac{\psi}{2}) - \sin(\frac{\phi}{2}) \cdot \sin(\frac{\theta}{2}) \cdot \cos(\frac{\psi}{2}) \] 其中： - \( \phi, \theta, \psi \) 分别对应 roll、pitch 和 yaw 的值[^1]。 ##### 四元数转欧拉角 给定一个四元数 \( q = (w, x, y, z) \)，则可以将其转化为欧拉角的形式： \[ \text{roll} = \arctan2(2(q_w q_x + q_y q_z), 1 - 2(q_x^2 + q_y^2)) \] \[ \text{pitch} = \arcsin(2(q_w q_y - q_z q_x)) \] \[ \text{yaw} = \arctan2(2(q_w q_z + q_x q_y), 1 - 2(q_y^2 + q_z^2)) \] 这些公式适用于常见的 ZYX 坐标系下的欧拉角定义[^3]。 #### Python 实现代码 以下是基于上述公式的 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) qw = cr * cp * cy + sr * sp * sy qx = sr * cp * cy - cr * sp * sy qy = cr * sp * cy + sr * cp * sy qz = cr * cp * sy - sr * sp * cy return [qw, qx, qy, qz] def quaternion_to_euler(w, x, y, z): t0 = +2.0 * (w * x + y * z) t1 = +1.0 - 2.0 * (x * x + y * y) roll = math.atan2(t0, t1) t2 = +2.0 * (w * y - z * x) t2 = max(-1.0, min(+1.0, t2)) # Clamp to valid range pitch = math.asin(t2) t3 = +2.0 * (w * z + x * y) t4 = +1.0 - 2.0 * (y * y + z * z) yaw = math.atan2(t3, t4) return [roll, pitch, yaw] ``` 以上代码实现了欧拉角到四元数以及四元数到欧拉角的双向转换功能[^2]。 ---