别再死记硬背公式了！用Python代码实战理解无人机姿态的三种表示法（欧拉角、DCM、四元数）

用Python代码实战理解无人机姿态的三种表示法

无人机在空中飞行时，如何准确描述它的姿态是一个关键问题。想象一下，当你操控无人机进行翻滚、俯仰或偏航动作时，计算机需要精确理解这些动作在三维空间中的含义。这就是姿态表示法的用武之地——它们为计算机提供了一种数学语言来描述无人机的空间方位。

在无人机开发和飞控编程中，开发者通常会遇到三种主要的姿态表示方法：欧拉角、方向余弦矩阵(DCM)和四元数。每种方法都有其独特的优势和适用场景。欧拉角直观易懂但存在数学上的局限性；方向余弦矩阵直接明了但计算量较大；四元数计算高效但概念相对抽象。

本文将带你用Python代码实现这三种表示法之间的转换，通过实际编程来深入理解它们的特性和应用场景。我们会从基础概念出发，逐步构建完整的转换函数库，并通过可视化手段直观展示不同表示法的差异。特别地，我们会重点讨论实际编程中可能遇到的"坑"，比如奇异点处理、数值稳定性等问题。

1. 环境准备与基础概念

在开始编码之前，我们需要搭建合适的开发环境并理解一些基础数学概念。Python因其丰富的科学计算库而成为实现姿态算法的理想选择。

首先安装必要的Python库：

pip install numpy matplotlib scipy pyquaternion

这些库将帮助我们进行矩阵运算、可视化以及四元数计算。其中numpy提供高效的数组操作，matplotlib用于数据可视化，pyquaternion则封装了四元数的基本运算。

1.1 三维旋转的基本原理

无人机姿态本质上描述的是机体坐标系相对于地面坐标系的旋转。理解这一点至关重要。我们可以用多种数学工具来描述这种旋转关系：

• 旋转矩阵：3×3的正交矩阵，描述坐标系间的基向量变换
• 欧拉角：三个连续的旋转角度(滚转、俯仰、偏航)
• 四元数：四个参数的复数扩展，紧凑表示旋转

注意：所有姿态表示法都是等价的，只是表现形式不同。选择哪种表示法取决于具体应用场景。

下表对比了三种主要表示法的基本特性：

理解这些基本特性将帮助我们在实际应用中做出合理选择。例如，在需要频繁进行旋转叠加运算的场景中，四元数通常是首选；而在需要直观显示姿态角度的界面中，欧拉角更为合适。

2. 欧拉角表示与实现

欧拉角是最直观的姿态表示方法，它用三个角度来描述无人机相对于参考坐标系的方位。在航空领域，这三个角度通常被称为：

• 滚转角(Roll, φ)：绕机体前后轴的旋转
• 俯仰角(Pitch, θ)：绕机体左右轴的旋转
• 偏航角(Yaw, ψ)：绕机体垂直轴的旋转

2.1 欧拉角的旋转顺序

欧拉角的定义依赖于旋转的顺序。在航空领域，我们通常使用"Z-Y-X"顺序，即：

1. 首先绕Z轴(偏航)旋转ψ角度
2. 然后绕新的Y轴(俯仰)旋转θ角度
3. 最后绕新的X轴(滚转)旋转φ角度

这种顺序也被称为"航向-俯仰-滚转"顺序。让我们用Python实现这个旋转过程：

import numpy as np def euler_to_dcm(roll, pitch, yaw): """将欧拉角转换为方向余弦矩阵""" # 将角度转换为弧度 phi, theta, psi = np.radians(roll), np.radians(pitch), np.radians(yaw) # 计算各旋转矩阵 R_x = np.array([ [1, 0, 0], [0, np.cos(phi), -np.sin(phi)], [0, np.sin(phi), np.cos(phi)] ]) R_y = np.array([ [np.cos(theta), 0, np.sin(theta)], [0, 1, 0], [-np.sin(theta), 0, np.cos(theta)] ]) R_z = np.array([ [np.cos(psi), -np.sin(psi), 0], [np.sin(psi), np.cos(psi), 0], [0, 0, 1] ]) # 按Z-Y-X顺序组合旋转 return R_z @ R_y @ R_x

2.2 欧拉角的局限性

虽然欧拉角直观易懂，但它存在一个严重问题——万向节锁(Gimbal Lock)。当俯仰角θ接近±90度时，滚转和偏航轴会重合，导致失去一个自由度。

让我们通过代码演示这个问题：

def check_gimbal_lock(pitch): """检查万向节锁情况""" # 创建接近90度的俯仰角 roll1, yaw1 = 30, 45 # 初始欧拉角 dcm = euler_to_dcm(roll1, pitch, yaw1) # 从DCM反解欧拉角 pitch_calc = np.degrees(np.arcsin(-dcm[2, 0])) roll_calc = np.degrees(np.arctan2(dcm[2, 1], dcm[2, 2])) yaw_calc = np.degrees(np.arctan2(dcm[1, 0], dcm[0, 0])) return (roll_calc, pitch_calc, yaw_calc) # 测试接近90度的情况 print(check_gimbal_lock(89.9)) # 结果可能不符合预期

在实际应用中，我们需要特别注意这种情况，通常的解决方案是：

• 避免让俯仰角接近±90度
• 在必须处理大角度时切换到四元数表示
• 使用特殊算法处理奇异点附近的转换

3. 方向余弦矩阵(DCM)实现

方向余弦矩阵(DCM)是一个3×3的正交矩阵，它直接描述了机体坐标系相对于地面坐标系的旋转关系。DCM的每一列代表地面坐标系基向量在机体坐标系中的投影。

3.1 DCM的基本性质

一个有效的DCM具有以下数学特性：

1. 正交性：R^T = R^{-1}
2. 行列式为1：det(R) = 1
3. 列向量两两正交且为单位长度

让我们实现一些基本的DCM操作：

def is_valid_dcm(R): """检查矩阵是否是有效的DCM""" # 检查矩阵形状 if R.shape != (3, 3): return False # 检查正交性 identity = np.eye(3) if not np.allclose(R.T @ R, identity, atol=1e-6): return False # 检查行列式 if not np.isclose(np.linalg.det(R), 1.0, atol=1e-6): return False return True def dcm_to_euler(R): """将DCM转换为欧拉角""" assert is_valid_dcm(R), "输入的矩阵不是有效的DCM" # 计算俯仰角 pitch = np.degrees(np.arcsin(-R[2, 0])) # 计算滚转角 roll = np.degrees(np.arctan2(R[2, 1], R[2, 2])) # 计算偏航角 yaw = np.degrees(np.arctan2(R[1, 0], R[0, 0])) return roll, pitch, yaw

3.2 DCM的优缺点分析

DCM表示法的主要优势包括：

• 无奇异点问题，可以表示任意姿态
• 直接表示坐标系间的变换关系
• 便于进行向量坐标转换

然而，DCM也有明显缺点：

• 需要存储9个元素(尽管只有3个自由度)
• 数值积分时难以保证正交性
• 连续旋转运算效率较低

在实际应用中，DCM常用于：

• 需要频繁进行坐标系转换的场景
• 与其他系统接口时的数据交换格式
• 需要高精度姿态表示的场合

4. 四元数表示与实现

四元数是表示三维旋转最优雅的数学工具之一。它用一个实部和三个虚部来表示旋转，形式为q = w + xi + yj + zk。

4.1 四元数的基本运算

让我们实现四元数的基本操作：

def quaternion_normalize(q): """归一化四元数""" norm = np.linalg.norm(q) return q / norm def quaternion_from_euler(roll, pitch, yaw): """从欧拉角创建四元数""" # 转换为弧度并取半角 phi, theta, psi = np.radians(roll)/2, np.radians(pitch)/2, np.radians(yaw)/2 # 计算各三角函数值 c1, s1 = np.cos(phi), np.sin(phi) c2, s2 = np.cos(theta), np.sin(theta) c3, s3 = np.cos(psi), np.sin(psi) # 构造四元数 q = np.array([ c1*c2*c3 + s1*s2*s3, s1*c2*c3 - c1*s2*s3, c1*s2*c3 + s1*c2*s3, c1*c2*s3 - s1*s2*c3 ]) return quaternion_normalize(q) def quaternion_to_dcm(q): """将四元数转换为DCM""" w, x, y, z = q return np.array([ [1-2*(y**2+z**2), 2*(x*y-z*w), 2*(x*z+y*w)], [2*(x*y+z*w), 1-2*(x**2+z**2), 2*(y*z-x*w)], [2*(x*z-y*w), 2*(y*z+x*w), 1-2*(x**2+y**2)] ])

4.2 四元数的优势与应用

四元数在姿态表示中具有独特优势：

1. 紧凑性：只需4个参数，比DCM更节省内存
2. 无奇异点：可以表示任意旋转而不存在万向节锁问题
3. 计算高效：旋转组合可以通过四元数乘法高效完成
4. 插值平滑：适合用于动画和姿态估计

让我们看一个实际例子，比较不同表示法的计算效率：

import time def test_performance(): """测试不同表示法的计算效率""" angles = np.random.rand(1000, 3) * 360 - 180 # 生成随机欧拉角 # 测试欧拉角到DCM的转换 start = time.time() for angle in angles: euler_to_dcm(*angle) dcm_time = time.time() - start # 测试欧拉角到四元数的转换 start = time.time() for angle in angles: quaternion_from_euler(*angle) quat_time = time.time() - start return dcm_time, quat_time dcm_t, quat_t = test_performance() print(f"DCM转换平均时间: {dcm_t/1000:.6f}s, 四元数转换平均时间: {quat_t/1000:.6f}s")

在实际的无人机飞控系统中，四元数通常是内部计算的首选表示法，而欧拉角则用于用户界面显示和外部通信。

5. 姿态表示法的实际应用与可视化

理解不同姿态表示法的最好方式是通过可视化。我们将使用Matplotlib创建简单的3D动画来展示不同表示法的效果。

5.1 创建无人机简化模型

首先，我们定义一个简单的无人机3D模型：

from mpl_toolkits.mplot3d import Axes3D def create_drone_model(): """创建无人机简化3D模型""" # 定义无人机各部件(机身、机臂、螺旋桨) body = np.array([[0, 0, 0], [1, 0, 0]]) arms = [ np.array([[0.5, 0, 0], [0.5, 0.5, 0]]), np.array([[0.5, 0, 0], [0.5, -0.5, 0]]), np.array([[0.5, 0, 0], [0.5, 0, 0.5]]) ] props = [ np.array([0.5, 0.5, 0]), np.array([0.5, -0.5, 0]), np.array([0.5, 0, 0.5]) ] return {'body': body, 'arms': arms, 'props': props} def plot_drone(ax, model, R=None): """绘制无人机模型""" if R is not None: # 应用旋转 body = (R @ model['body'].T).T arms = [(R @ arm.T).T for arm in model['arms']] props = (R @ np.array(model['props']).T).T else: body, arms, props = model['body'], model['arms'], model['props'] # 绘制机身 ax.plot(body[:, 0], body[:, 1], body[:, 2], 'r-', linewidth=3) # 绘制机臂 for arm in arms: ax.plot(arm[:, 0], arm[:, 1], arm[:, 2], 'b-', linewidth=2) # 绘制螺旋桨 ax.scatter(props[:, 0], props[:, 1], props[:, 2], c='g', s=50) # 设置坐标轴 ax.set_xlim([-1, 1]) ax.set_ylim([-1, 1]) ax.set_zlim([-1, 1]) ax.set_xlabel('X') ax.set_ylabel('Y') ax.set_zlabel('Z')

5.2 动画演示不同姿态表示法

现在，我们可以创建动画来比较不同姿态表示法的效果：

import matplotlib.pyplot as plt from matplotlib.animation import FuncAnimation def animate_attitude(): """创建姿态动画""" fig = plt.figure(figsize=(12, 5)) ax1 = fig.add_subplot(121, projection='3d') ax2 = fig.add_subplot(122, projection='3d') model = create_drone_model() def update(frame): # 计算当前角度 roll = 30 * np.sin(frame * 0.1) pitch = 45 * np.sin(frame * 0.15) yaw = 60 * np.sin(frame * 0.05) # 使用欧拉角直接旋转 R_euler = euler_to_dcm(roll, pitch, yaw) # 使用四元数旋转 q = quaternion_from_euler(roll, pitch, yaw) R_quat = quaternion_to_dcm(q) # 清除并重新绘制 ax1.clear() ax2.clear() plot_drone(ax1, model, R_euler) plot_drone(ax2, model, R_quat) ax1.set_title(f'欧拉角表示\nRoll: {roll:.1f}°, Pitch: {pitch:.1f}°, Yaw: {yaw:.1f}°') ax2.set_title('四元数表示') ani = FuncAnimation(fig, update, frames=100, interval=50) plt.tight_layout() plt.show() # 运行动画(在实际环境中取消注释) # animate_attitude()

通过这样的可视化，我们可以直观地看到不同姿态表示法最终描述的旋转是完全一致的，只是内部表示方式不同。

6. 实际应用中的注意事项

在实际的无人机飞控开发中，姿态表示法的选择和实现有许多需要注意的细节。以下是一些关键经验：

6.1 数值稳定性处理

在长时间运行的系统中，数值误差会逐渐累积。特别是对于DCM，需要定期进行正交化处理：

def dcm_renormalize(R): """重新正交化DCM""" # 对每一列进行Gram-Schmidt正交化 x = R[:, 0] y = R[:, 1] - np.dot(R[:, 1], x) * x z = np.cross(x, y) # 归一化 x = x / np.linalg.norm(x) y = y / np.linalg.norm(y) z = z / np.linalg.norm(z) return np.column_stack((x, y, z))

对于四元数，也需要定期归一化：

def quaternion_integrate(q, gyro, dt): """使用陀螺仪数据积分更新四元数""" # 角速度转换为四元数导数 p = np.array([0, *gyro]) q_dot = 0.5 * quaternion_multiply(q, p) # 前向欧拉积分 q_new = q + q_dot * dt # 归一化 return quaternion_normalize(q_new)

6.2 表示法之间的转换精度

不同表示法之间的转换可能引入数值误差，特别是在奇异点附近。以下是一些最佳实践：

• 尽量避免频繁在表示法之间转换
• 在必须转换时，选择数值稳定的算法
• 对关键操作进行单元测试，验证边界条件

例如，测试欧拉角与四元数转换的往返精度：

def test_conversion_accuracy(): """测试欧拉角与四元数转换的往返精度""" errors = [] for _ in range(1000): # 生成随机但不接近奇异点的欧拉角 angles = np.random.rand(3) * 170 - 85 # 避免±90度 # 转换为四元数再转回欧拉角 q = quaternion_from_euler(*angles) angles_back = dcm_to_euler(quaternion_to_dcm(q)) # 计算误差 error = np.degrees(np.linalg.norm(np.radians(np.array(angles) - np.array(angles_back)))) errors.append(error) return np.mean(errors) print(f"平均转换误差: {test_conversion_accuracy():.6f}度")

6.3 选择最合适的表示法

在实际项目中，通常会混合使用多种表示法：

• 传感器数据融合：常用四元数或DCM
• 控制算法：根据具体需求选择
• 用户界面：使用欧拉角
• 数据存储/通信：根据协议要求选择

下表总结了典型场景下的推荐选择：

在开发无人机飞控系统时，我通常会建立一个姿态库，内部使用四元数存储当前姿态，同时提供各种转换接口供不同模块调用。这种架构既保证了计算效率，又提供了足够的灵活性。