### Python 实现两架飞机相对速度计算 为了完成这一任务，需要将东北天（ENU）坐标系的速度转换到 XYZ 坐标系下，并利用姿态角构建旋转矩阵来描述飞机的姿态变化。以下是具体实现过程： #### 1. 构建旋转矩阵 根据提供的信息[^1]，对于右手坐标系中的旋转操作，需按照指定的欧拉角顺序构造旋转矩阵。假设给定的姿态角为滚转角 $\phi$、俯仰角 $\theta$ 和偏航角 $\psi$，可以通过以下方式构造旋转矩阵 $R_{\text{enu} \to xyz}$ 将 ENU 坐标系映射至 XYZ 坐标系。 $$ R_x(\phi) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\phi & -\sin\phi \\ 0 & \sin\phi & \cos\phi \end{bmatrix}, R_y(\theta) = \begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \end{bmatrix}, R_z(\psi) = \begin{bmatrix} \cos\psi & -\sin\psi & 0 \\ \sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{bmatrix}. $$ 最终的旋转矩阵可由上述三个基本旋转矩阵组合而成： $$ R_{\text{enu} \to xyz} = R_x(\phi) \cdot R_y(\theta) \cdot R_z(\psi). $$ #### 2. 转换速度矢量 设一架飞机在 ENU 坐标系下的速度为 $(v_E, v_N, v_U)$，则可通过如下公式将其变换至 XYZ 坐标系下： $$ [v_X, v_Y, v_Z]^T = R_{\text{enu} \to xyz} \cdot [v_E, v_N, v_U]^T. $$ 同理，另一架飞机的速度也可按此方式进行转换。 #### 3. 计算相对速度 两架飞机之间的相对速度可以直接通过两者在 XYZ 坐标系下的速度差得到： $$ [\Delta v_X, \Delta v_Y, \Delta v_Z]^T = [v_{X_1}, v_{Y_1}, v_{Z_1}]^T - [v_{X_2}, v_{Y_2}, v_{Z_2}]^T, $$ 其中，$(v_{X_1}, v_{Y_1}, v_{Z_1})$ 和 $(v_{X_2}, v_{Y_2}, v_{Z_2})$ 分别代表两架飞机在 XYZ 坐标系下的速度分量。 #### 4. Python 实现代码 下面是完整的 Python 实现代码： ```python import numpy as np def euler_to_rotation_matrix(phi, theta, psi): """ Convert Euler angles (roll, pitch, yaw) to a rotation matrix from ENU to XYZ. :param phi: Roll angle in radians. :param theta: Pitch angle in radians. :param psi: Yaw angle in radians. :return: Rotation matrix (3x3 array). """ Rx = np.array([ [1, 0, 0], [0, np.cos(phi), -np.sin(phi)], [0, np.sin(phi), np.cos(phi)] ]) Ry = np.array([ [np.cos(theta), 0, np.sin(theta)], [0, 1, 0], [-np.sin(theta), 0, np.cos(theta)] ]) Rz = np.array([ [np.cos(psi), -np.sin(psi), 0], [np.sin(psi), np.cos(psi), 0], [0, 0, 1] ]) return Rz @ Ry @ Rx # Combine rotations def enu_to_xyz(v_enu, phi, theta, psi): """ Transform velocity vector from ENU coordinates to XYZ coordinates using the given Euler angles. :param v_enu: Velocity vector in ENU frame (East-North-Up). :param phi: Roll angle in radians. :param theta: Pitch angle in radians. :param psi: Yaw angle in radians. :return: Velocity vector in XYZ frame. """ R = euler_to_rotation_matrix(phi, theta, psi) return R.dot(v_enu) def relative_velocity(v1_enu, eulerAngles1, v2_enu, eulerAngles2): """ Calculate the relative velocity between two aircrafts in XYZ coordinate system. :param v1_enu: First aircraft's velocity in ENU frame. :param eulerAngles1: First aircraft's Euler angles (roll, pitch, yaw). :param v2_enu: Second aircraft's velocity in ENU frame. :param eulerAngles2: Second aircraft's Euler angles (roll, pitch, yaw). :return: Relative velocity in XYZ frame. """ phi1, theta1, psi1 = eulerAngles1 phi2, theta2, psi2 = eulerAngles2 v1_xyz = enu_to_xyz(v1_enu, phi1, theta1, psi1) v2_xyz = enu_to_xyz(v2_enu, phi2, theta2, psi2) delta_v_xyz = v1_xyz - v2_xyz return delta_v_xyz # Example usage: if __name__ == "__main__": import math # Aircraft 1 data v1_enu = np.array([10, 20, 30]) # East-North-Up velocities of first aircraft roll1 = math.radians(10) # Roll angle in degrees converted to radians pitch1 = math.radians(-5) # Pitch angle in degrees converted to radians yaw1 = math.radians(30) # Yaw angle in degrees converted to radians # Aircraft 2 data v2_enu = np.array([8, 15, 25]) roll2 = math.radians(5) pitch2 = math.radians(-3) yaw2 = math.radians(25) rel_vel = relative_velocity(v1_enu, (roll1, pitch1, yaw1), v2_enu, (roll2, pitch2, yaw2)) print(f"Relative velocity in XYZ frame: {rel_vel}") ``` --- ###