别再死记硬背公式了！用Python+NumPy手把手实现无人机姿态转换（欧拉角/四元数/DCM）

用Python+NumPy实战无人机姿态转换：从理论到代码的完全指南

无人机在空中飞行时的每一个动作，本质上都是三维空间中的姿态变换。无论是平稳悬停还是急速转弯，背后都离不开欧拉角、四元数和方向余弦矩阵这三种核心数学表示方法的精确计算。传统教材往往陷入复杂的公式推导，而本文将带你用Python和NumPy从零构建完整的姿态转换工具链，让抽象的理论变成可运行、可调试的真实代码。

1. 环境准备与基础概念

在开始编码前，我们需要明确几个关键概念。姿态描述的本质是建立机体坐标系与地面坐标系之间的数学关系。想象无人机是一个刚体，我们需要用数字精确描述它的"朝向"。

安装必要的Python库：

pip install numpy matplotlib scipy

三种主流姿态表示法的核心特点：

提示：实际工程中常采用混合策略 - 用户界面显示用欧拉角，内部计算用四元数或DCM

2. 欧拉角与方向余弦矩阵的相互转换

欧拉角由三个角度组成：(roll, pitch, yaw)，分别代表绕x、y、z轴的旋转。在航空领域，我们通常采用Z-Y-X旋转顺序（偏航-俯仰-横滚）。

实现欧拉角到DCM的转换：

import numpy as np def euler_to_dcm(roll, pitch, yaw): """将欧拉角转换为方向余弦矩阵""" cr, sr = np.cos(roll), np.sin(roll) cp, sp = np.cos(pitch), np.sin(pitch) cy, sy = np.cos(yaw), np.sin(yaw) # Z-Y-X旋转顺序 dcm = np.array([ [cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr], [sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr], [ -sp, cp*sr, cp*cr] ]) return dcm

逆向转换需要特别注意奇异点问题（pitch=±90°）：

def dcm_to_euler(dcm): """方向余弦矩阵转欧拉角，处理奇异点""" pitch = -np.arcsin(dcm[2, 0]) # 检查是否接近奇异点（俯仰角接近±90度） if np.abs(np.abs(pitch) - np.pi/2) < 1e-6: # 奇异点情况 roll = 0 # 可以任意值，通常取0 yaw = np.arctan2(-dcm[1, 2], dcm[1, 1]) else: roll = np.arctan2(dcm[2, 1], dcm[2, 2]) yaw = np.arctan2(dcm[1, 0], dcm[0, 0]) return roll, pitch, yaw

3. 四元数与其他表示法的转换

四元数由四个分量组成(q0, q1, q2, q3)，其中q0是实部。它通过三维空间的旋转轴和旋转角度来表示姿态。

欧拉角转四元数的实现：

def euler_to_quaternion(roll, pitch, yaw): """欧拉角转四元数""" cr, sr = np.cos(roll/2), np.sin(roll/2) cp, sp = np.cos(pitch/2), np.sin(pitch/2) cy, sy = np.cos(yaw/2), np.sin(yaw/2) q0 = cr * cp * cy + sr * sp * sy q1 = sr * cp * cy - cr * sp * sy q2 = cr * sp * cy + sr * cp * sy q3 = cr * cp * sy - sr * sp * cy return np.array([q0, q1, q2, q3])

四元数转方向余弦矩阵：

def quaternion_to_dcm(q): """四元数转方向余弦矩阵""" q0, q1, q2, q3 = q return np.array([ [1-2*(q2**2+q3**2), 2*(q1*q2-q0*q3), 2*(q1*q3+q0*q2)], [ 2*(q1*q2+q0*q3), 1-2*(q1**2+q3**2), 2*(q2*q3-q0*q1)], [ 2*(q1*q3-q0*q2), 2*(q2*q3+q0*q1), 1-2*(q1**2+q2**2)] ])

4. 实战应用与可视化

现在我们将这些转换函数整合到一个无人机姿态系统中，并添加可视化功能。

创建姿态转换工具类：

class DroneAttitude: def __init__(self, representation='euler', **kwargs): """初始化姿态表示""" if representation == 'euler': self.roll, self.pitch, self.yaw = kwargs['roll'], kwargs['pitch'], kwargs['yaw'] self.quaternion = euler_to_quaternion(self.roll, self.pitch, self.yaw) self.dcm = euler_to_dcm(self.roll, self.pitch, self.yaw) elif representation == 'quaternion': self.quaternion = kwargs['quaternion'] self.dcm = quaternion_to_dcm(self.quaternion) self.roll, self.pitch, self.yaw = dcm_to_euler(self.dcm) elif representation == 'dcm': self.dcm = kwargs['dcm'] self.roll, self.pitch, self.yaw = dcm_to_euler(self.dcm) self.quaternion = euler_to_quaternion(self.roll, self.pitch, self.yaw) def visualize(self): """使用Matplotlib可视化当前姿态""" from mpl_toolkits.mplot3d import Axes3D import matplotlib.pyplot as plt fig = plt.figure(figsize=(10, 8)) ax = fig.add_subplot(111, projection='3d') # 绘制地面坐标系 ax.quiver(0, 0, 0, 1.5, 0, 0, color='r', label='X (地面)') ax.quiver(0, 0, 0, 0, 1.5, 0, color='g', label='Y (地面)') ax.quiver(0, 0, 0, 0, 0, 1.5, color='b', label='Z (地面)') # 绘制机体坐标系 body_x = self.dcm @ np.array([1, 0, 0]) body_y = self.dcm @ np.array([0, 1, 0]) body_z = self.dcm @ np.array([0, 0, 1]) ax.quiver(0, 0, 0, *body_x, color='r', linestyle='--', label='X (机体)') ax.quiver(0, 0, 0, *body_y, color='g', linestyle='--', label='Y (机体)') ax.quiver(0, 0, 0, *body_z, color='b', linestyle='--', label='Z (机体)') ax.set_xlim([-1, 1]) ax.set_ylim([-1, 1]) ax.set_zlim([-1, 1]) ax.set_title(f'无人机姿态可视化\nRoll: {np.degrees(self.roll):.1f}°, Pitch: {np.degrees(self.pitch):.1f}°, Yaw: {np.degrees(self.yaw):.1f}°') ax.legend() plt.tight_layout() plt.show()

使用示例：

# 创建一个45度横滚、30度俯仰的无人机姿态 drone = DroneAttitude(representation='euler', roll=np.radians(45), pitch=np.radians(30), yaw=0) print("方向余弦矩阵:\n", drone.dcm) print("四元数:", drone.quaternion) drone.visualize()

5. 性能优化与工程实践

在实际无人机系统中，姿态转换的计算效率和数值稳定性至关重要。以下是几个关键优化点：

1. 避免重复计算三角函数：

   # 不好的做法 x = np.sin(angle) * np.cos(angle) # 好的做法 s, c = np.sin(angle), np.cos(angle) x = s * c

2. 四元数归一化处理：

   def normalize_quaternion(q): """保持四元数单位长度""" norm = np.sqrt(q[0]**2 + q[1]**2 + q[2]**2 + q[3]**2) return q / norm

3. 使用矩阵运算替代循环：

   # 批量处理多个姿态转换 def batch_euler_to_dcm(angles): """批量欧拉角转DCM""" rolls, pitches, yaws = angles[:, 0], angles[:, 1], angles[:, 2] cr, sr = np.cos(rolls), np.sin(rolls) cp, sp = np.cos(pitches), np.sin(pitches) cy, sy = np.cos(yaws), np.sin(yaws) dcms = np.empty((len(angles), 3, 3)) dcms[:, 0, 0] = cy * cp dcms[:, 0, 1] = cy * sp * sr - sy * cr # ... 其他元素赋值 return dcms

4. 奇异点处理策略：

   • 在接近奇异点时自动切换到四元数表示
   • 添加小量扰动避免完全垂直
   • 使用历史数据进行平滑过渡

6. 集成到无人机控制系统

将姿态转换模块嵌入到简单的无人机仿真循环中：

class DroneSimulator: def __init__(self): self.attitude = DroneAttitude(representation='euler', roll=0, pitch=0, yaw=0) self.angular_velocity = np.zeros(3) # 机体坐标系下的角速度 def update(self, dt, gyro_measurements): """更新无人机姿态""" # 1. 获取角速度测量值（通常来自陀螺仪） p, q, r = gyro_measurements # 滚转、俯仰、偏航角速度 # 2. 使用四元数微分方程更新姿态 q0, q1, q2, q3 = self.attitude.quaternion qdot = 0.5 * np.array([ [-q1, -q2, -q3], [ q0, -q3, q2], [ q3, q0, -q1], [-q2, q1, q0] ]) @ np.array([p, q, r]) # 3. 积分更新四元数 new_quaternion = self.attitude.quaternion + qdot * dt self.attitude = DroneAttitude( representation='quaternion', quaternion=normalize_quaternion(new_quaternion) ) # 4. 更新角速度（简单模型） self.angular_velocity = 0.9 * self.angular_velocity + 0.1 * gyro_measurements

在仿真循环中使用：

sim = DroneSimulator() dt = 0.01 # 10ms时间步长 for t in np.arange(0, 10, dt): # 模拟陀螺仪测量（添加一些随机噪声） gyro = np.array([0.1, 0.05, 0.02]) + np.random.normal(0, 0.01, 3) sim.update(dt, gyro) # 每秒钟可视化一次 if int(t / 1) > int((t - dt) / 1): sim.attitude.visualize()

这个完整的实现展示了如何将数学理论转化为实际可运行的无人机控制系统代码。通过可视化，你可以直观地看到无人机姿态随时间的变化，这正是工程实践中调试和理解姿态系统的强大工具。