无人机飞控编程实战：从欧拉角到机体角速度的姿态解算

当你在调试无人机时，发现姿态数据出现异常波动；当机器人在快速转向时，控制系统突然变得不稳定——这些问题的根源往往在于姿态解算的精度和实时性。作为飞控开发者，我们每天都在与欧拉角、四元数、角速度这些概念打交道，但真正理解它们之间转换关系的工程师并不多。

1. 姿态表示的基础概念

刚体在三维空间中的姿态描述是飞控系统最基础也最关键的数学表达。我们常用的姿态表示方法主要有三种：欧拉角、旋转矩阵和四元数。每种方法都有其独特的优势和适用场景。

欧拉角是最直观的姿态表示方式，它用三个角度（滚转roll、俯仰pitch、偏航yaw）来描述物体相对于固定坐标系的朝向。在无人机领域，ZYX顺序的欧拉角最为常见：

# ZYX欧拉角定义
euler_angles = {'roll': phi, 'pitch': theta, 'yaw': psi}

但欧拉角存在著名的"万向节死锁"问题：当俯仰角θ=±90°时，滚转和偏航运动将失去区分度。这也是为什么在实际工程中，我们往往需要结合其他表示方法。

旋转矩阵是一个3×3的正交矩阵，描述了两个坐标系之间的变换关系。对于ZYX欧拉角，对应的旋转矩阵为：

% MATLAB计算旋转矩阵
R = rotz(psi) * roty(theta) * rotx(phi);

旋转矩阵虽然不会出现奇异点，但9个参数中只有3个是独立的，计算效率不高。

四元数由4个参数组成(q0,q1,q2,q3)，是工程上最常用的姿态表示方法。它既避免了奇异点，计算效率又高，非常适合嵌入式系统：

表示方法	参数数量	是否存在奇异点	计算复杂度	直观性
欧拉角	3	是	低	高
旋转矩阵	9	否	中	低
四元数	4	否	低	中

2. 角速度与欧拉角速率的本质区别

IMU传感器直接测量得到的是机体坐标系下的角速度ωb=[p,q,r]^T，而我们在控制系统中更常使用的是欧拉角速率Φ˙=[ϕ˙,θ˙,ψ˙]^T。这两者看似都描述旋转速度，但物理意义截然不同。

机体角速度ωb表示的是刚体旋转的瞬时角速度矢量在机体坐标系下的投影。它不依赖于任何特定的姿态表示方法，是一个客观存在的物理量。

欧拉角速率Φ˙则是欧拉角随时间的变化率，它依赖于特定的旋转顺序定义。由于欧拉角本身就是三个顺序旋转的组合，其变化率与实际的物理旋转之间需要通过转换矩阵建立联系。

两者之间的转换关系由以下矩阵方程描述：

[ p ]   [ 1   0       -sinθ    ][ ϕ˙ ]
[ q ] = [ 0   cosϕ   cosθsinϕ ][ θ˙ ]
[ r ]   [ 0  -sinϕ   cosθcosϕ ][ ψ˙ ]

这个转换矩阵的物理意义是：将欧拉角速率投影到机体坐标系下。当俯仰角θ接近±90°时，矩阵中的sinθ项会导致数值不稳定，这就是欧拉角表示法的局限性。

3. 工程实现：Python/Matlab代码详解

在实际飞控系统中，我们需要在角速度和欧拉角之间进行双向转换。下面给出可直接集成到项目中的实现代码。

3.1 欧拉角速率转机体角速度

Python实现：

import numpy as np

def euler_rate_to_body_rate(phi, theta, phi_dot, theta_dot, psi_dot):
    """
    将欧拉角速率转换为机体角速度
    参数:
        phi, theta: 当前滚转和俯仰角(弧度)
        phi_dot, theta_dot, psi_dot: 欧拉角速率(弧度/秒)
    返回:
        p, q, r: 机体坐标系下的角速度(弧度/秒)
    """
    sin_phi = np.sin(phi)
    cos_phi = np.cos(phi)
    sin_theta = np.sin(theta)
    cos_theta = np.cos(theta)
    
    p = phi_dot - psi_dot * sin_theta
    q = cos_phi * theta_dot + sin_phi * cos_theta * psi_dot
    r = -sin_phi * theta_dot + cos_phi * cos_theta * psi_dot
    
    return p, q, r

Matlab实现：

function [p, q, r] = eulerRateToBodyRate(phi, theta, phi_dot, theta_dot, psi_dot)
% 欧拉角速率转机体角速度
sin_phi = sin(phi);
cos_phi = cos(phi);
sin_theta = sin(theta);
cos_theta = cos(theta);

p = phi_dot - psi_dot * sin_theta;
q = cos_phi * theta_dot + sin_phi * cos_theta * psi_dot;
r = -sin_phi * theta_dot + cos_phi * cos_theta * psi_dot;
end

3.2 机体角速度转欧拉角速率

反向转换需要考虑奇异点问题，当cosθ≈0时需要特殊处理：

def body_rate_to_euler_rate(phi, theta, p, q, r):
    """
    将机体角速度转换为欧拉角速率
    参数:
        phi, theta: 当前滚转和俯仰角(弧度)
        p, q, r: 机体坐标系下的角速度(弧度/秒)
    返回:
        phi_dot, theta_dot, psi_dot: 欧拉角速率(弧度/秒)
    """
    sin_phi = np.sin(phi)
    cos_phi = np.cos(phi)
    tan_theta = np.tan(theta)
    cos_theta = np.cos(theta)
    
    # 处理奇异点(θ≈±90°)
    if abs(cos_theta) < 1e-6:
        cos_theta = 1e-6  # 避免除以零
        tan_theta = np.sign(np.sin(theta)) * 1e6
    
    phi_dot = p + sin_phi * tan_theta * q + cos_phi * tan_theta * r
    theta_dot = cos_phi * q - sin_phi * r
    psi_dot = (sin_phi / cos_theta) * q + (cos_phi / cos_theta) * r
    
    return phi_dot, theta_dot, psi_dot

注意：当俯仰角接近±90°时，转换公式会出现数值不稳定。在实际工程中，此时应切换到四元数表示法。

4. 工程实践中的关键问题与解决方案

4.1 奇异点处理策略

欧拉角表示法在θ=±90°时的奇异点问题是无法避免的，但我们可以通过以下策略减轻影响：

1. 四元数混合使用：在接近奇异区域切换到四元数表示
2. 角度限制：设计控制算法时限制俯仰角范围(如±85°)
3. 平滑过渡：在切换点附近采用加权平均过渡

4.2 数值计算优化

姿态解算通常运行在嵌入式系统中，需要考虑计算效率和数值稳定性：

• 三角函数查表法：预先计算sin/cos值表，牺牲精度换取速度
• 泰勒展开近似：小角度时用泰勒级数近似三角函数
• 定点数运算：在无FPU的MCU上使用定点数计算

4.3 实际调试技巧

在调试飞控系统时，以下几个技巧非常实用：

1. 数据可视化：实时绘制欧拉角和各轴角速度曲线
2. 单位测试：单独验证转换函数的正确性
3. 白盒测试：在奇异点附近注入测试信号
4. 性能分析：测量函数执行时间，优化关键路径

# 性能测试示例
import timeit

setup = '''
import numpy as np
from __main__ import euler_rate_to_body_rate
phi, theta = 0.1, 0.2
phi_dot, theta_dot, psi_dot = 0.01, 0.02, 0.03
'''

time = timeit.timeit('euler_rate_to_body_rate(phi,theta,phi_dot,theta_dot,psi_dot)', 
                    setup=setup, number=1000)
print(f"平均执行时间: {time*1000:.3f} 微秒")

5. 四元数法的对比与选择

虽然本文重点讨论欧拉角，但在实际工程中，四元数法往往更为常用。以下是两种方法的对比：

四元数微分方程避免了奇异点问题：

q˙ = 0.5 * q ⊗ ωb

其中⊗表示四元数乘法。四元数法的实现通常包括以下步骤：

1. 初始化四元数
2. 使用角速度更新四元数
3. 必要时将四元数转换为欧拉角(用于显示或控制)

% 四元数更新示例(Matlab)
function q_new = updateQuaternion(q, omega, dt)
% 四元数微分方程积分
omega_norm = norm(omega);
if omega_norm > 0
    axis = omega / omega_norm;
    delta_q = [cos(omega_norm*dt/2); axis*sin(omega_norm*dt/2)];
    q_new = quatmultiply(q', delta_q')';
else
    q_new = q;
end
end

在实际项目中，通常会根据应用场景混合使用多种表示方法：

• 传感器融合：使用四元数
• 控制算法：使用欧拉角(远离奇异点时)
• 用户界面：使用欧拉角(更直观)

姿态解算的精度直接影响到飞行控制的稳定性。一次在调试四旋翼时，我们发现飞机在快速横滚时会出现姿态估计漂移，最终发现正是欧拉角速率转换时的数值误差积累导致的。改用四元数为主、欧拉角为辅的混合方案后，问题得到完美解决。