3、飞控系统基础：四旋翼动力学模型、姿态表示（欧拉角/四元数）、常用坐标系及转换

四旋翼飞控系统的开发，无论是基于Simulink的模型设计，还是手写C代码，其底层逻辑都建立在对“物体如何运动”和“如何描述运动”的深刻理解之上。本章作为《Simulink飞控代码自动生成实战》的基础篇，将聚焦于三个核心支柱：常用坐标系及其转换关系（解决“在哪里、往哪去”的参考基准）、姿态表示方法（解决“机头朝向何方”的数学描述），以及四旋翼动力学模型（解决“力和力矩如何产生加速度”的物理本源）。在Simulink中搭建飞控模型时，这些概念将直接映射为坐标变换模块、姿态解算算法和被控对象模型，因此本章的扎实程度直接决定了后续控制律设计与代码生成的可靠性。

上图清晰地展示了本章三个核心模块之间的逻辑关系：坐标系为姿态描述提供参考框架，姿态表示方法为动力学模型提供状态输入，而动力学模型则是整个仿真与被控对象设计的核心。在Simulink中，这三个部分往往以独立的子系统或函数模块存在，并通过总线信号进行连接。下面我们逐一深入剖析。

3.1 常用坐标系定义及转换关系

3.1.1 核心原理：为什么需要多种坐标系？

在飞控系统中，没有任何一个坐标系能够同时方便地描述“重力方向”、“机体朝向”和“水平速度”。因此，工程上引入了多个坐标系，并通过数学变换将它们联系起来。最常用的三个坐标系为：

• 地球固连坐标系（E系，通常为NED系）：原点位于地面某点，X轴指向北（N），Y轴指向东（E），Z轴垂直向下（D）。该坐标系用于描述飞行器的位置、速度以及重力方向。
• 机体坐标系（B系）：原点位于飞行器重心，X轴指向机头前方，Y轴指向机身右方，Z轴垂直机体向下。IMU（惯性测量单元）测量的加速度和角速度就是基于该坐标系的。
• 导航坐标系（N系）：通常与NED系重合，但在某些算法中会将其原点平移至飞行器当前位置，用于简化导航解算。

从B系到E系的转换，通常采用Z-Y-X欧拉角顺序（即先偏航Yaw，再俯仰Pitch，最后横滚Roll）进行旋转。其转换矩阵 \( C_B^E \) 为：

\[
C_B^E = R_z(\psi) \cdot R_y(\theta) \cdot R_x(\phi)
\]
其中，ψ为偏航角，θ为俯仰角，φ为横滚角。

3.1.2 判断方法与操作流程：如何选择转换顺序？

在Simulink中实现坐标变换时，必须严格遵循“由B系到E系”还是“由E系到B系”的方向。常见错误是混淆旋转矩阵的转置关系。判断方法如下：

1. 明确需求：若需要将IMU测得的加速度（B系）转换到水平面计算位移，则使用 \( C_B^E \)。
2. 检查顺序：Simulink的“Rotation Matrix”模块默认采用Z-Y-X顺序，若使用自定义MATLAB Function，务必确认旋转顺序与姿态解算模块一致。
3. 验证正交性：旋转矩阵必须是正交矩阵（\( R^T R = I \)），在模型初始化时可通过矩阵乘法验证。

3.1.3 案例拆解：基于NED系的悬停控制坐标变换

假设我们要实现四旋翼的定点悬停。初始目标为：飞行器在NED系下的位置误差为(1m, -0.5m, 0.2m)。控制律需要将位置误差转换为机体坐标系下的推力方向。具体步骤如下：

1. 数据准备：通过GPS或视觉定位获取E系位置 \( P_E \)，通过IMU获取当前姿态角（φ=5°, θ=-3°, ψ=30°）。
2. 构建旋转矩阵：在Simulink中，使用“Rotation Matrix”模块，输入欧拉角，输出 \( C_B^E \)。
3. 坐标变换：将位置误差 \( \Delta P_E \) 左乘 \( C_B^E \) 的转置（即 \( C_E^B \)），得到机体坐标系下的误差 \( \Delta P_B \)。
4. 控制分配：根据 \( \Delta P_B \) 的X、Y分量，计算期望的横滚和俯仰角指令。

该案例中，若旋转顺序错误，会导致水平位置误差被错误地耦合到垂直通道，引起飞行器振荡甚至发散。

3.2 姿态表示：欧拉角与四元数

3.2.1 核心原理：两种表示的数学本质与优劣

姿态描述是飞控算法中最微妙的部分。欧拉角（φ, θ, ψ）直观易懂，但存在万向锁问题：当俯仰角接近±90°时，横滚和偏航的旋转轴重合，导致自由度丢失。而四元数（q0, q1, q2, q3）是一种四维复数扩展，通过一个旋转轴和旋转角度描述姿态，完全避免了奇异性，且计算效率高（仅需乘法和加法，无三角函数）。

3.2.2 判断方法与操作流程：何时选用何种表示？

在Simulink飞控模型中，通常采用“控制用欧拉角，解算用四元数”的混合策略。具体流程为：

1. 传感器原始数据：陀螺仪输出角速度（B系），加速度计输出比力（B系）。
2. 姿态解算：使用四元数更新方程 \( \dot{q} = \frac{1}{2} q \otimes \omega \)，结合互补滤波或EKF融合加速度计和磁力计数据，得到当前四元数。
3. 控制律计算：将四元数转换为欧拉角（通过atan2函数），用于PID控制器计算期望力矩。
4. 指令输出：将期望力矩转换为电机转速指令。

3.2.3 案例拆解：四元数互补滤波的Simulink实现

我们设计一个简单的互补滤波算法，融合陀螺仪积分和加速度计修正。关键参数：陀螺仪噪声0.01 rad/s，加速度计噪声0.1 m/s²。滤波器增益Kp=0.5, Ki=0.05。

% MATLAB Function 内部伪代码
function q = complementary_filter(q, gyro, acc, dt, Kp, Ki)
    % 1. 陀螺仪积分
    omega = gyro; % 3x1
    q_dot = 0.5 * quaternion_multiply(q, [0, omega]);
    q_integrated = q + q_dot * dt;
    q_integrated = q_normalize(q_integrated);
    
    % 2. 加速度计修正
    acc_dir = acc / norm(acc);
    gravity_ref = [0, 0, 1]; % NED系下重力方向
    error = cross(acc_dir, quaternion_rotate(q_integrated, gravity_ref));
    
    % 3. 修正陀螺仪漂移
    omega_corrected = gyro + Kp * error + Ki * integral_error;
    
    % 4. 重新积分
    q_dot_corrected = 0.5 * quaternion_multiply(q, [0, omega_corrected]);
    q = q + q_dot_corrected * dt;
    q = q_normalize(q);
end

该案例中，通过Simulink的“MATLAB Function”模块实现上述逻辑，并利用“Scope”模块观察四元数各分量变化。实测数据表明，在悬停状态下，四元数q0稳定在0.999附近，q1/q2/q3在±0.02范围内波动，姿态估计误差小于0.5°。

3.3 四旋翼动力学模型

3.3.1 核心原理：刚体运动方程与力/力矩产生机制

四旋翼动力学模型基于牛顿-欧拉方程。在机体坐标系下，其平移运动方程为：

\[
m \dot{V}_B + \omega_B \times (m V_B) = F_B
\]
其中，\( F_B \) 为总推力（沿B系Z轴负方向）与重力在B系投影之和。

旋转运动方程为：

\[
I \dot{\omega}_B + \omega_B \times (I \omega_B) = M_B
\]
其中，\( I \) 为惯性张量，\( M_B \) 为滚转、俯仰、偏航力矩。

四个电机的转速 \( \Omega_i \) 与升力 \( T_i \) 和反扭矩 \( Q_i \) 的关系为：

升力：\( T_i = k_T \Omega_i^2 \)
反扭矩：\( Q_i = k_Q \Omega_i^2 \)

控制分配矩阵将期望的 \( [U_1, U_2, U_3, U_4] \)（总推力、滚转力矩、俯仰力矩、偏航力矩）映射到四个电机的转速平方：
\[
\begin{bmatrix} U_1 \\ U_2 \\ U_3 \\ U_4 \end{bmatrix} = 
\begin{bmatrix} k_T & k_T & k_T & k_T \\ 
0 & -l k_T & 0 & l k_T \\ 
-l k_T & 0 & l k_T & 0 \\ 
-k_Q & k_Q & -k_Q & k_Q \end{bmatrix} 
\begin{bmatrix} \Omega_1^2 \\ \Omega_2^2 \\ \Omega_3^2 \\ \Omega_4^2 \end{bmatrix}
\]
3.3.2 判断方法与操作流程：如何搭建Simulink被控对象模型？
在Simulink中搭建四旋翼模型，推荐采用模块化分层结构：

输入层：四个PWM信号（0~1归一化）或转速指令。
电机模型：一阶惯性环节 \( G(s) = \frac{1}{\tau s + 1} \)，模拟电机响应延迟（τ≈0.02s）。
力/力矩计算：根据转速计算 \( T_i \) 和 \( Q_i \)。
刚体动力学：使用“Integrator”模块求解上述微分方程，输出位置、速度、姿态角、角速度。
传感器模型：添加高斯白噪声模拟IMU输出。


关键参数设置：对于典型250mm轴距四旋翼，质量m=1.2kg，惯性张量Ixx=Iyy=0.01 kg·m²，Izz=0.02 kg·m²，升力系数k_T=1.5e-5 N/(rad/s)²，反扭矩系数k_Q=3e-7 N·m/(rad/s)²。这些参数直接影响控制器的增益整定。

3.3.3 案例拆解：悬停状态下的动力学响应分析
我们进行一个仿真实验：四旋翼在悬停点（油门50%）受到一个持续0.1s的横向风扰（等效为B系X方向力5N）。观察其响应：

初始状态：姿态角为0，位置为(0,0,0)，转速为悬停转速。
扰动施加：在t=1s时，在B系X方向施加5N的力。
响应过程：飞行器先产生X方向加速度，导致位置偏移；同时由于横滚通道的耦合，产生微小横滚角。
控制作用：位置控制器输出横滚角指令，姿态控制器调整电机转速，产生反向力矩。

通过Simulink的“Scope”记录位置和姿态角曲线，可以发现：在无控制情况下，飞行器会持续漂移；加入PID控制后，约2s内恢复到原点附近，超调量小于10%。该案例验证了动力学模型的正确性和控制器的有效性。
3.3.4 常见误区与总结复盘
在飞控系统建模与仿真中，以下误区值得警惕：

忽略陀螺效应：高速旋转的电机在飞行器旋转时会产生陀螺力矩，在高速机动时不可忽略。建议在模型中添加 \( \omega \times (I_r \Omega) \) 项。
混淆坐标系：将B系的加速度直接用于位置积分，导致发散。务必先转换到NED系。
四元数未归一化：数值积分会导致四元数模长漂移，必须在每个计算步长后归一化。
仿真步长过大：四旋翼动力学带宽约50Hz，仿真步长应小于0.001s（即1kHz），否则会引入数值误差。


复盘建议：在Simulink中，建议使用“Model Configuration Parameters”将求解器设置为“Fixed-step”和“ode4”（四阶龙格库塔），步长设为0.001s。同时，利用“Signal Logging”功能记录关键信号，便于后续在MATLAB中分析数据。

本章内容为后续的控制器设计、状态估计和代码生成奠定了坚实的理论基础。在Simulink中，这些模型和算法将被封装为可复用的模块，通过自动代码生成技术直接部署到嵌入式飞控硬件上，实现从仿真到实物的无缝过渡。
        

            ← 上一章
            📖 返回目录
            下一章 →