使用SymPy进行欧拉角与RPY角的符号与数值计算

1. 基本概念与公式

1.1 RPY角定义

RPY角(Roll-Pitch-Yaw)是一种描述三维姿态的泰特-布莱恩角表示法，由三个绕固定参考系坐标轴的旋转组成：

• 偏航角(Yaw, $\psi$)：绕固定Z轴的旋转
• 俯仰角(Pitch, $\theta$)：绕固定Y轴的旋转
• 滚转角(Roll, $\phi$)：绕固定X轴的旋转

标准旋转顺序为：先Yaw(z) → 再Pitch(y) → 最后Roll(x) ，数学表示为：
$$R_{RPY}=R_x(\phi )\cdot R_y(\theta )\cdot R_z(\psi )$$

1.2 旋转矩阵公式

基本旋转矩阵定义如下：

绕X轴旋转($\phi$)
$$R_x(\phi )=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \phi & -\sin \phi \\ 0& \sin \phi & \cos \phi \end{array}\right]$$

绕Y轴旋转($\theta$)
$$R_y(\theta )=\left[\begin{array}{ccc}\cos \theta & 0& \sin \theta \\ 0& 1& 0\\ -\sin \theta & 0& \cos \theta \end{array}\right]$$

绕Z轴旋转($\psi$)
$$R_z(\psi )=\left[\begin{array}{ccc}\cos \psi & -\sin \psi & 0\\ \sin \psi & \cos \psi & 0\\ 0& 0& 1\end{array}\right]$$

1.3 完整RPY旋转矩阵

将三个基本旋转矩阵相乘，得到完整的RPY旋转矩阵：
R R P Y = R x ( φ ) ⋅ R y ( θ ) ⋅ R z ( ψ ) = [ cos ⁡ θ cos ⁡ ψ cos ⁡ θ sin ⁡ ψ − sin ⁡ θ sin ⁡ φ sin ⁡ θ cos ⁡ ψ − cos ⁡ φ sin ⁡ ψ sin