飞控系统设计与实现

概述

飞行控制系统（Flight Control System, FCS）是无人机、航模等飞行器的核心大脑，负责感知飞行器状态、计算控制指令、驱动执行机构，实现稳定飞行和任务执行。

一个典型的飞控系统构成如下闭环控制链路：

1
2
3


传感器 → 状态估计 → 控制算法 → 控制分配 → 执行机构 → 飞行器运动 → 传感器
   ↑                                                                   ↓
   └───────────────────── 反馈闭环 ─────────────────────────────────────┘

本文将按照上述流程，详细介绍飞控系统的各个模块。

一、传感器模块

传感器是飞控系统的"感官"，负责采集飞行器的运动状态信息。

1.1 惯性测量单元（IMU）

IMU 是飞控最核心的传感器，包含：

加速度计（Accelerometer）

测量三轴线性加速度
可用于姿态解算（通过重力方向）
典型芯片：MPU6050、ICM-20602、BMI088

陀螺仪（Gyroscope）

测量三轴角速度
短期精度高，但存在漂移
与加速度计融合可得到稳定姿态

1
2


陀螺仪输出：ωx, ωy, ωz (角速度，rad/s)
加速度计输出：ax, ay, az (线性加速度，m/s²)

IMU 数据特点：

高采样率（通常 1kHz 以上）
存在噪声和偏置
需要滤波和标定

1.2 磁力计（Magnetometer）

测量地磁场强度，提供航向角（Yaw）
易受电机、电源干扰，需远离干扰源或软硬铁校准
典型芯片：HMC5883L、IST8310

1.3 气压计（Barometer）

测量大气压力，换算高度
垂直精度约 ±0.5m
用于定高飞行
典型芯片：BMP280、MS5611

1
2


高度计算：h = 44330 * (1 - (P/P0)^(1/5.255))
其中 P 为当前气压，P0 为海平面气压

1.4 GPS/GNSS

提供绝对位置（经纬度）和速度
支持自主导航、返航等功能
典型模块：u-blox M8N、M9N

GPS 数据：

位置：经度、纬度、海拔
速度：北向速度、东向速度、垂直速度
精度：民用 GPS 约 2.5m CEP

1.5 超声波/激光测距

近距离高度测量（<5m）
精准着陆、避障应用
典型模块：HC-SR04、TF-Luna

1.6 光流传感器（Optical Flow）

通过图像分析计算水平速度
室内无 GPS 环境定位
典型芯片：PMW3901

二、状态估计

传感器原始数据存在噪声、漂移、延迟等问题，需要通过状态估计算法融合，得到准确的状态信息。

2.1 姿态解算

姿态表示方法：

欧拉角（Euler Angles）

Roll（横滚）、Pitch（俯仰）、Yaw（偏航）
直观易懂，但存在万向锁问题

四元数（Quaternion）

无奇异性，计算高效
飞控中广泛使用

1
2


四元数：q = q0 + q1*i + q2*j + q3*k
姿态旋转矩阵由四元数计算得到

旋转矩阵（Rotation Matrix / DCM）

3×3 矩阵，无奇异性
计算量较大

2.2 互补滤波

最简单的姿态融合算法：

1
2
3
4
5
6
7
8


// 陀螺仪积分（高频可信）
θ_gyro = θ_prev + ω * dt

// 加速度计计算（低频可信）
θ_accel = atan2(ax, ay)

// 互补滤波融合
θ = α * θ_gyro + (1-α) * θ_accel

优点：计算量小，适合资源受限的 MCU。

2.3 卡尔曼滤波（Kalman Filter）

最优状态估计方法，考虑系统噪声和测量噪声：

预测步骤：

1
2


x̂⁻ = F * x̂ + B * u
P⁻ = F * P * Fᵀ + Q

更新步骤：

1
2
3


K = P⁻ * Hᵀ * (H * P⁻ * Hᵀ + R)⁻¹
x̂ = x̂⁻ + K * (z - H * x̂⁻)
P = (I - K * H) * P⁻

其中：

x：状态向量（位置、速度、姿态等）
F：状态转移矩阵
H：观测矩阵
Q：过程噪声协方差
R：测量噪声协方差
K：卡尔曼增益

2.4 扩展卡尔曼滤波（EKF）

对于非线性系统，通过雅可比矩阵线性化：

1
2


x̂⁻ = f(x̂, u)          // 非线性状态转移
F = ∂f/∂x              // 雅可比矩阵

2.5 典型状态估计器

PX4 EKF2：

24维状态向量
融合 IMU、GPS、磁力计、气压计、光流
自动选择可用数据源

ArduPilot EKF3/EKF4：

多源融合
支持双 IMU 冗余

典型状态向量：

1

x = [position(3), velocity(3), attitude(4), gyro_bias(3), accel_bias(3), ...]ᵀ

三、控制算法

控制算法根据期望状态和当前状态的误差，计算控制输出。

3.1 级联 PID 控制

最常用的飞控控制结构，采用位置-速度-姿态级联：

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17


                    ┌──────────┐
    期望位置 ─────→ │ 位置环PID │ ─────→ 期望速度
                    └──────────┘
                                     ↓
                              ┌──────────┐
                    当前位置 ←│ 速度环PID │ ←── 当前速度
                              └──────────┘
                                     ↓ 期望姿态
                              ┌──────────┐
                              │ 姿态环PID │
                              └──────────┘
                                     ↓
                              ┌──────────┐
                              │ 角速度PID │
                              └──────────┘
                                     ↓
                                 控制输出

3.2 PID 控制器详解

位置环（外环）：

1
2


e_pos = pos_sp - pos
vel_sp = Kp_pos * e_pos

速度环：

1
2


e_vel = vel_sp - vel
acc_sp = Kp_vel * e_vel + Ki_vel * ∫e_vel dt + Kd_vel * de_vel/dt

姿态环：

1
2


e_att = att_sp - att
rate_sp = Kp_att * e_att

角速度环（内环）：

1
2


e_rate = rate_sp - rate
u = Kp_rate * e_rate + Ki_rate * ∫e_rate dt + Kd_rate * de_rate/dt

3.3 PID 参数整定

步骤：

先整定角速度环（内环）
- Ki = 0, Kd = 0，逐步增大 Kp 直到响应快速但不过冲
整定姿态环
- 保持内环参数，调整外环 Kp
整定速度环、位置环
- 由内向外逐层整定

经验值（四旋翼）：

1
2


角速度环：Kp ≈ 0.1~0.3, Ki ≈ 0.05~0.1, Kd ≈ 0.001~0.01
姿态环：  Kp ≈ 3~8

3.4 高级控制方法

LQR（线性二次调节器）

1
2


u = -K * x
K 通过求解 Riccati 方程得到

MPC（模型预测控制）

考虑约束的最优控制
计算量大，适合高端飞控

自适应控制

在线调整参数
应对模型不确定性

鲁棒控制（H∞）

抗扰动能力强
设计复杂

四、控制分配

控制算法输出的控制指令（力矩、推力）需要分配到各个执行机构。

4.1 混控矩阵

对于多旋翼，控制量与电机转速的关系：

1
2
3
4
5
6


┌   ┐   ┌              ┐   ┌    ┐
│ F │   │  k  k  k  k  │   │ ω₁² │
│ τx│ = │ -k  k  k -k  │ * │ ω₂² │
│ τy│   │ -k  k -k  k  │   │ ω₃² │
│ τz│   │ -k  k -k  k  │   │ ω₄² │
└   ┘   └              ┘   └    ┘

其中 k 为推力/力矩系数，与电机、螺旋桨特性相关。

4.2 X 型四旋翼混控

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12


电机布局（从上往下看）：

    M1(CW)    M3(CCW)
        \    /
         \  /
          \/
          /\
         /  \
        /    \
    M4(CCW)    M2(CW)

CW: 顺时针    CCW: 逆时针

混控公式：

1
2
3
4
5


// 输入：thrust, roll, pitch, yaw
motor[0] = thrust - roll - pitch + yaw;  // M1
motor[1] = thrust + roll + pitch + yaw;  // M2
motor[2] = thrust + roll - pitch - yaw;  // M3
motor[3] = thrust - roll + pitch - yaw;  // M4

4.3 旋翼/固定翼混控

对于垂直起降（VTOL）飞行器，需要处理旋翼和舵面：

1
2


过渡模式：旋翼推力 + 固定翼舵面
固定翼模式：主要靠舵面 + 推进电机

4.4 饱和处理

当控制需求超出执行机构能力时：

优先级分配：

首先保证总推力（防止失控坠机）
然后保证姿态控制（稳定性）
最后考虑航向控制

1
2


// 饱和限幅
motor[i] = constrain(motor[i], MIN_THROTTLE, MAX_THROTTLE);

五、执行机构

执行机构将控制信号转换为机械动作。

5.1 无刷电机（BLDC）

特点：

高功率密度
需配合电调（ESC）使用
KV 值决定转速特性

电机参数：

1
2


KV = 转速(RPM) / 电压(V)
推力 ∝ 电压 * 电流 * 效率

选型参考：

机型	电机KV	螺旋桨	电池
穿越机	2300-2600	5寸	4S-6S
航拍机	900-1000	10寸	4S-6S
大型机	300-500	15-20寸	6S-12S

5.2 电子调速器（ESC）

功能：

将直流电转换为三相交流电驱动电机
接收 PWM/OneShot/MultiShot/DShot 信号
部分支持双向通信（回传转速、温度）

通信协议对比：

协议	频率	延迟
PWM	50-400Hz	2-20ms
OneShot125	1-4kHz	0.25-1ms
DShot	8-32kHz	0.03-0.125ms

DShot 优势：

数字信号，抗干扰
无需校准
支持 ESC 回传信息

5.3 舵机（Servo）

用于：

固定翼舵面控制
云台控制
起落架收放

控制信号：

1
2
3


PWM 周期：20ms
脉宽范围：1000-2000μs
中位：1500μs

5.4 螺旋桨

参数：

直径 × 螺距（如 1045 = 10寸直径，4.5寸螺距）
正桨/反桨（成对使用）
材质：塑料/尼龙/碳纤维

选型原则：

1
2


大直径 + 低KV电机 = 高效率、大载重
小直径 + 高KV电机 = 高速度、快速响应

六、飞行器运动

执行机构产生的力和力矩驱动飞行器运动。

6.1 动力学模型

牛顿-欧拉方程：

1
2
3
4
5


平动：
m * ẍ = F_thrust * R * e₃ - m * g * e₃

转动：
I * ω̇ = τ - ω × (I * ω)

其中：

m：质量
R：姿态旋转矩阵
I：转动惯量矩阵
ω：角速度
τ：控制力矩

6.2 坐标系

常用坐标系：

惯性坐标系（NED/ENU）
- NED：北-东-地（航空常用）
- ENU：东-北-天（ROS 常用）
机体系（Body Frame）
- 原点在飞行器重心
- X 轴指向前方
- Z 轴指向下方
坐标系转换：

1

v_body = Rᵀ * v_inertial

6.3 运动方程状态空间表示

1
2
3
4
5
6
7
8


状态向量：x = [p, v, q, ω]ᵀ
         位置(3) + 速度(3) + 四元数(4) + 角速度(3)

状态方程：
ṗ = v
v̇ = R(q) * (F/m) - g*e₃
q̇ = 0.5 * q ⊗ ω
ω̇ = I⁻¹ * (τ - ω × I*ω)

6.4 气动效应

对于固定翼或高速飞行：

升力： L = 0.5 * ρ * V² * S * CL

阻力： D = 0.5 * ρ * V² * S * CD

侧力： Y = 0.5 * ρ * V² * S * CY

其中：

ρ：空气密度
V：空速
S：参考面积
CL, CD, CY：气动系数

七、闭环反馈

传感器测量飞行器的实际运动状态，反馈到状态估计模块，形成闭环。

7.1 闭环控制框图

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11


              ┌──────────────────────────────────────────────────────┐
              │                    飞控系统                           │
              │                                                      │
              │   ┌─────┐   ┌─────┐   ┌─────┐   ┌─────┐   ┌─────┐   │
  期望状态 ──→│   │状态 │→  │控制 │→  │控制 │→  │执行 │→  │飞行器│   │
              │   │估计 │   │算法 │   │分配 │   │机构 │   │运动 │   │
              │   └─────┘   └─────┘   └─────┘   └─────┘   └─────┘   │
              │       ↑                                         │   │
              │       │              传感器                      │   │
              │       └─────────────────────────────────────────┘   │
              └──────────────────────────────────────────────────────┘

7.2 采样与延迟

采样率设计：

模块	典型采样率
IMU	1-8 kHz
姿态解算	200-500 Hz
控制律	200-1000 Hz
GPS	5-10 Hz
遥测	1-50 Hz

延迟补偿：

传感器延迟
计算延迟
执行延迟

需要在状态估计中补偿，否则影响控制稳定性。

7.3 安全机制

失控保护（FailSafe）：

1
2
3
4
5
6
7


if (信号丢失 || 电池低电压 || 姿态异常) {
    执行预设安全动作：
    - 返航（RTL）
    - 悬停
    - 降落
    - 解锁停止
}

地理围栏（GeoFence）：

1
2
3


if (position outside fence) {
    enforce_boundary();
}

姿态限制：

1
2
3


if (tilt_angle > MAX_TILT) {
    limit_control_output();
}

八、系统实现示例

8.1 软件架构

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35


// 主循环伪代码
void flight_control_loop() {
    while (1) {
        // 1. 读取传感器
        read_imu(&imu_data);
        read_gps(&gps_data);
        read_baro(&baro_data);
        
        // 2. 状态估计
        ekf_update(&imu_data, &gps_data, &baro_data, &state);
        
        // 3. 读取遥控输入
        read_rc_input(&rc_input);
        
        // 4. 计算期望状态
        compute_setpoint(&rc_input, &state, &setpoint);
        
        // 5. 控制律计算
        position_control(&setpoint, &state, &attitude_sp);
        attitude_control(&attitude_sp, &state, &rate_sp);
        rate_control(&rate_sp, &state, &control_output);
        
        // 6. 控制分配
        mixer_update(&control_output, motor_output);
        
        // 7. 输出到执行机构
        write_motors(motor_output);
        
        // 8. 状态监控
        check_failsafe(&state);
        
        // 等待下一个周期
        wait_for_next_cycle();
    }
}

8.2 开源飞控参考

项目	特点	适用场景
PX4	模块化、功能全	专业无人机、研发
ArduPilot	功能丰富、支持多平台	各种飞行器
Betaflight	高性能、低延迟	竞速穿越机
iNav	导航功能强	远航、固定翼

8.3 开发工具

仿真：

Gazebo + PX4 SITL
AirSim（微软）
jMAVSim

地面站：

QGroundControl（PX4）
Mission Planner（ArduPilot）
Betaflight Configurator

调试：

实时波形：PlotJuggler
日志分析：FlightPlot、ULog

总结

飞控系统是一个典型的闭环控制系统，各个模块紧密协作：

1

传感器采集 → 状态估计融合 → 控制算法计算 → 控制分配 → 执行机构驱动 → 飞行器响应 → 传感器反馈

关键要点：

传感器选择需考虑精度、采样率、成本
状态估计是系统精度的基础，EKF 是主流方法
级联 PID 是简单可靠的控制方案
控制分配需要考虑饱和与优先级
执行机构的选型影响系统性能
安全机制是飞控不可或缺的部分

参考资料

PX4 Development Guide
ArduPilot Documentation
Kalman and Bayesian Filters in Python
《无人机设计与控制》- 全权
《Quadrotor Dynamics and Control》- Raffaello D’Andrea

✈️ 飞控开发是一个系统工程，需要控制理论、嵌入式开发、空气动力学等多学科知识。欢迎交流讨论！