02-03-01 无人机飞控系统原理

1. 核心概念

1.1 飞控系统概述

飞控系统（Flight Control System）是无人机的"大脑"，负责处理传感器数据、执行飞行控制算法、驱动电机，实现稳定飞行和自主导航。

核心特点：

• 实时性：控制周期<10ms
• 高可靠性：故障检测与容错
• 多传感器融合：IMU + GPS + 气压计 + 磁力计
• 自主控制：姿态稳定、高度保持、位置控制
• 扩展性：支持多种飞行器构型

1.2 飞行器构型

四旋翼（Quadcopter）：

              前
        M1(CW)  M2(CCW)
            ╲  ╱
             ╳
            ╱  ╲
       M4(CCW)  M3(CW)

X型布局：
- M1/M3：顺时针（CW）
- M2/M4：逆时针（CCW）
- 优点：结构简单、成本低
- 应用：消费级无人机

六旋翼（Hexacopter）：

       M1    M2
         ╲  ╱
      M6  ╳  M3
         ╱  ╲
       M5    M4

- 冗余设计（单电机故障仍可飞行）
- 载重能力强
- 应用：专业航拍、物流

固定翼：

       ───────
      │       │
   ───┴───────┴───
      │电机    │
      └────────┘

- 巡航效率高
- 续航时间长
- 应用：测绘、巡检

1.3 坐标系定义

机体坐标系（Body Frame）：

     X（前）
      ↑
      │
      │
      └────→ Y（右）
     ╱
    ╱
   Z（下）

俯仰（Pitch）：绕Y轴旋转
横滚（Roll）：绕X轴旋转
偏航（Yaw）：绕Z轴旋转

地理坐标系（NED Frame）：

North-East-Down：
- X：指向北
- Y：指向东
- Z：指向地心

2. 技术原理

2.1 四旋翼飞行原理

升力产生：

电机转速 → 螺旋桨旋转 → 空气向下加速 → 产生升力

升力公式：
F = ½ × ρ × A × V² × CL

其中：
- ρ：空气密度（1.225 kg/m³）
- A：旋翼面积
- V：气流速度
- CL：升力系数

实际简化：
F ≈ k × ω²

其中：
- k：电机/螺旋桨常数
- ω：电机角速度（rad/s）

六自由度控制：

俯仰（Pitch Forward）：
M1↑ M4↑ > M2↓ M3↓
→ 机头下压，前飞

横滚（Roll Right）：
M1↑ M2↑ > M3↓ M4↓
→ 右倾，右飞

偏航（Yaw Right）：
M1↑ M3↑ > M2↓ M4↓
→ 顺时针旋转

油门（Throttle Up）：
M1↑ M2↑ M3↑ M4↑
→ 整体升力增加

悬停（Hover）：
M1 = M2 = M3 = M4
→ 升力 = 重力

2.2 传感器系统

IMU（惯性测量单元）：

加速度计（Accelerometer）：
- 测量：三轴加速度
- 用途：计算姿态角（静态）
- 频率：1kHz
- 精度：±0.5°

陀螺仪（Gyroscope）：
- 测量：三轴角速度
- 用途：姿态解算（动态）
- 频率：1kHz
- 精度：0.01°/s

磁力计（Magnetometer）：
- 测量：地磁场方向
- 用途：偏航角校准
- 频率：100Hz
- 精度：±2°

气压计（Barometer）：

测量：大气压力
用途：高度估计
频率：50Hz

高度计算：
h = 44330 × (1 - (P/P0)^0.1903)

其中：
- P：当前气压
- P0：海平面气压（1013.25 hPa）
- h：高度（米）

精度：±0.5m（室外）

2.3 姿态解算算法

互补滤波（Complementary Filter）：

// 简单互补滤波
float alpha = 0.98;  // 高通滤波系数

void complementary_filter(float dt) {
    // 陀螺仪积分（高频）
    float angle_gyro = angle + gyro_rate * dt;

    // 加速度计计算（低频）
    float angle_acc = atan2(acc_y, acc_z) * RAD_TO_DEG;

    // 互补滤波融合
    angle = alpha * angle_gyro + (1 - alpha) * angle_acc;
}

卡尔曼滤波（Kalman Filter）：

// 卡尔曼滤波姿态解算
typedef struct {
    float angle;          // 角度估计
    float bias;           // 陀螺仪偏差
    float P[2][2];        // 误差协方差矩阵
} kalman_t;

float kalman_update(kalman_t* k, float gyro_rate, float acc_angle, float dt) {
    // 预测
    k->angle += (gyro_rate - k->bias) * dt;
    k->P[0][0] += dt * (dt * k->P[1][1] - k->P[0][1] - k->P[1][0] + Q_angle);
    k->P[0][1] -= dt * k->P[1][1];
    k->P[1][0] -= dt * k->P[1][1];
    k->P[1][1] += Q_bias * dt;

    // 更新
    float S = k->P[0][0] + R_measure;
    float K[2];
    K[0] = k->P[0][0] / S;
    K[1] = k->P[1][0] / S;

    float y = acc_angle - k->angle;
    k->angle += K[0] * y;
    k->bias += K[1] * y;

    float P00_temp = k->P[0][0];
    float P01_temp = k->P[0][1];

    k->P[0][0] -= K[0] * P00_temp;
    k->P[0][1] -= K[0] * P01_temp;
    k->P[1][0] -= K[1] * P00_temp;
    k->P[1][1] -= K[1] * P01_temp;

    return k->angle;
}

3. 代码实现

3.1 姿态解算实现（C）

// attitude.c - 姿态解算模块

#include <math.h>
#include "attitude.h"

#define RAD_TO_DEG 57.29578f
#define DEG_TO_RAD 0.0174533f

// 姿态结构体
typedef struct {
    float roll;      // 横滚角
    float pitch;     // 俯仰角
    float yaw;       // 偏航角
} attitude_t;

// IMU数据结构体
typedef struct {
    float acc_x, acc_y, acc_z;      // 加速度（m/s²）
    float gyro_x, gyro_y, gyro_z;   // 角速度（rad/s）
    float mag_x, mag_y, mag_z;      // 磁力计（μT）
} imu_data_t;

// 互补滤波参数
static float alpha = 0.98f;
static attitude_t attitude = {0, 0, 0};

/**
 * 姿态解算 - 互补滤波
 */
void attitude_update_complementary(imu_data_t* imu, float dt) {
    // 陀螺仪积分
    float gyro_roll = attitude.roll + imu->gyro_x * dt * RAD_TO_DEG;
    float gyro_pitch = attitude.pitch + imu->gyro_y * dt * RAD_TO_DEG;

    // 加速度计计算角度
    float acc_roll = atan2f(imu->acc_y, imu->acc_z) * RAD_TO_DEG;
    float acc_pitch = atan2f(-imu->acc_x,
                             sqrtf(imu->acc_y * imu->acc_y +
                                   imu->acc_z * imu->acc_z)) * RAD_TO_DEG;

    // 互补滤波融合
    attitude.roll = alpha * gyro_roll + (1 - alpha) * acc_roll;
    attitude.pitch = alpha * gyro_pitch + (1 - alpha) * acc_pitch;

    // 偏航角（磁力计）
    float mag_x_h = imu->mag_x * cosf(attitude.pitch * DEG_TO_RAD) +
                    imu->mag_z * sinf(attitude.pitch * DEG_TO_RAD);
    float mag_y_h = imu->mag_x * sinf(attitude.roll * DEG_TO_RAD) *
                    sinf(attitude.pitch * DEG_TO_RAD) +
                    imu->mag_y * cosf(attitude.roll * DEG_TO_RAD) -
                    imu->mag_z * sinf(attitude.roll * DEG_TO_RAD) *
                    cosf(attitude.pitch * DEG_TO_RAD);

    attitude.yaw = atan2f(-mag_y_h, mag_x_h) * RAD_TO_DEG;
}

/**
 * 获取姿态角
 */
void attitude_get(attitude_t* out) {
    *out = attitude;
}

3.2 PID控制器实现

// pid.c - PID控制器

typedef struct {
    float kp;           // 比例系数
    float ki;           // 积分系数
    float kd;           // 微分系数
    float integral;     // 积分累积
    float prev_error;   // 上次误差
    float output_min;   // 输出下限
    float output_max;   // 输出上限
} pid_t;

/**
 * PID控制器更新
 */
float pid_update(pid_t* pid, float setpoint, float measurement, float dt) {
    // 计算误差
    float error = setpoint - measurement;

    // 积分项
    pid->integral += error * dt;

    // 积分限幅（防止积分饱和）
    if (pid->integral > 100.0f) pid->integral = 100.0f;
    if (pid->integral < -100.0f) pid->integral = -100.0f;

    // 微分项
    float derivative = (error - pid->prev_error) / dt;

    // PID输出
    float output = pid->kp * error +
                   pid->ki * pid->integral +
                   pid->kd * derivative;

    // 输出限幅
    if (output > pid->output_max) output = pid->output_max;
    if (output < pid->output_min) output = pid->output_min;

    // 保存当前误差
    pid->prev_error = error;

    return output;
}

/**
 * 三环PID控制（角速度环 + 角度环 + 位置环）
 */
typedef struct {
    pid_t angle_pid;       // 角度环PID
    pid_t rate_pid;        // 角速度环PID
} cascade_pid_t;

float cascade_pid_update(cascade_pid_t* cpid,
                         float angle_setpoint,
                         float angle_measurement,
                         float rate_measurement,
                         float dt) {
    // 外环：角度控制
    float rate_setpoint = pid_update(&cpid->angle_pid,
                                     angle_setpoint,
                                     angle_measurement,
                                     dt);

    // 内环：角速度控制
    float output = pid_update(&cpid->rate_pid,
                             rate_setpoint,
                             rate_measurement,
                             dt);

    return output;
}

3.3 电机混控（Motor Mixing）

// motor_mixing.c - 电机混控

// 四旋翼X型布局混控矩阵
void motor_mixing_quad_x(float throttle, float roll, float pitch, float yaw,
                         float* motor) {
    /*
     * 混控矩阵：
     *        油门  横滚  俯仰  偏航
     * M1:    +1    -1    +1    -1
     * M2:    +1    +1    +1    +1
     * M3:    +1    +1    -1    -1
     * M4:    +1    -1    -1    +1
     */

    motor[0] = throttle - roll + pitch - yaw;  // M1 (左前)
    motor[1] = throttle + roll + pitch + yaw;  // M2 (右前)
    motor[2] = throttle + roll - pitch - yaw;  // M3 (右后)
    motor[3] = throttle - roll - pitch + yaw;  // M4 (左后)

    // 油门限幅（0-100%）
    for (int i = 0; i < 4; i++) {
        if (motor[i] > 100.0f) motor[i] = 100.0f;
        if (motor[i] < 0.0f) motor[i] = 0.0f;
    }
}

// 六旋翼混控
void motor_mixing_hexa_x(float throttle, float roll, float pitch, float yaw,
                         float* motor) {
    /*
     *        油门  横滚  俯仰  偏航
     * M1:    +1    0     +1    -1
     * M2:    +1    +0.87 +0.5  +1
     * M3:    +1    +0.87 -0.5  -1
     * M4:    +1    0     -1    +1
     * M5:    +1    -0.87 -0.5  -1
     * M6:    +1    -0.87 +0.5  +1
     */

    motor[0] = throttle + pitch - yaw;
    motor[1] = throttle + 0.87f * roll + 0.5f * pitch + yaw;
    motor[2] = throttle + 0.87f * roll - 0.5f * pitch - yaw;
    motor[3] = throttle - pitch + yaw;
    motor[4] = throttle - 0.87f * roll - 0.5f * pitch - yaw;
    motor[5] = throttle - 0.87f * roll + 0.5f * pitch + yaw;

    // 限幅
    for (int i = 0; i < 6; i++) {
        if (motor[i] > 100.0f) motor[i] = 100.0f;
        if (motor[i] < 0.0f) motor[i] = 0.0f;
    }
}

3.4 飞控主循环（Python示例）

from dataclasses import dataclass

@dataclass
class IMUData:
    """IMU数据"""
    acc: np.ndarray  # 加速度 [x, y, z]
    gyro: np.ndarray  # 角速度 [x, y, z]
    mag: np.ndarray  # 磁力计 [x, y, z]

@dataclass
class Attitude:
    """姿态角"""
    roll: float
    pitch: float
    yaw: float

class FlightController:
    """飞控主控制器"""

    def __init__(self):
        self.attitude = Attitude(0, 0, 0)

        # PID控制器
        self.roll_pid = PIDController(kp=4.0, ki=0.05, kd=0.5)
        self.pitch_pid = PIDController(kp=4.0, ki=0.05, kd=0.5)
        self.yaw_pid = PIDController(kp=2.0, ki=0.0, kd=0.3)

        self.loop_rate = 400  # 400Hz控制频率
        self.dt = 1.0 / self.loop_rate

    def update(self, imu: IMUData, rc_input: dict):
        """
        飞控主循环更新

        Args:
            imu: IMU数据
            rc_input: 遥控器输入 {'throttle', 'roll', 'pitch', 'yaw'}
        """
        # 1. 姿态解算
        self.attitude_update(imu)

        # 2. PID控制
        roll_output = self.roll_pid.update(
            setpoint=rc_input['roll'],
            measurement=self.attitude.roll,
            dt=self.dt
        )

        pitch_output = self.pitch_pid.update(
            setpoint=rc_input['pitch'],
            measurement=self.attitude.pitch,
            dt=self.dt
        )

        yaw_output = self.yaw_pid.update(
            setpoint=rc_input['yaw'],
            measurement=self.attitude.yaw,
            dt=self.dt
        )

        # 3. 电机混控
        motors = self.motor_mixing(
            throttle=rc_input['throttle'],
            roll=roll_output,
            pitch=pitch_output,
            yaw=yaw_output
        )

        return motors

    def attitude_update(self, imu: IMUData):
        """姿态解算（互补滤波）"""
        alpha = 0.98

        # 陀螺仪积分
        gyro_roll = self.attitude.roll + imu.gyro[0] * self.dt * 57.3
        gyro_pitch = self.attitude.pitch + imu.gyro[1] * self.dt * 57.3

        # 加速度计计算
        acc_roll = np.arctan2(imu.acc[1], imu.acc[2]) * 57.3
        acc_pitch = np.arctan2(-imu.acc[0],
                               np.sqrt(imu.acc[1]**2 + imu.acc[2]**2)) * 57.3

        # 融合
        self.attitude.roll = alpha * gyro_roll + (1 - alpha) * acc_roll
        self.attitude.pitch = alpha * gyro_pitch + (1 - alpha) * acc_pitch

        # 偏航角（磁力计）
        self.attitude.yaw = np.arctan2(-imu.mag[1], imu.mag[0]) * 57.3

    def motor_mixing(self, throttle, roll, pitch, yaw):
        """四旋翼X型混控"""
        motors = np.array([
            throttle - roll + pitch - yaw,  # M1
            throttle + roll + pitch + yaw,  # M2
            throttle + roll - pitch - yaw,  # M3
            throttle - roll - pitch + yaw   # M4
        ])

        # 限幅
        motors = np.clip(motors, 0, 100)

        return motors

class PIDController:
    """PID控制器"""

    def __init__(self, kp, ki, kd):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        self.integral = 0
        self.prev_error = 0

    def update(self, setpoint, measurement, dt):
        """PID更新"""
        error = setpoint - measurement

        self.integral += error * dt
        self.integral = np.clip(self.integral, -100, 100)

        derivative = (error - self.prev_error) / dt

        output = self.kp * error + self.ki * self.integral + self.kd * derivative

        self.prev_error = error

        return np.clip(output, -100, 100)

# 使用示例
if __name__ == '__main__':
    fc = FlightController()

    # 模拟飞行
    for i in range(1000):
        # 读取IMU数据（模拟）
        imu = IMUData(
            acc=np.array([0, 0, 9.8]),
            gyro=np.array([0.01, -0.02, 0.005]),
            mag=np.array([25, -5, 40])
        )

        # 遥控器输入
        rc_input = {
            'throttle': 50,
            'roll': 0,
            'pitch': 0,
            'yaw': 0
        }

        # 更新飞控
        motors = fc.update(imu, rc_input)

        print(f"姿态: Roll={fc.attitude.roll:.2f}° "
              f"Pitch={fc.attitude.pitch:.2f}° "
              f"Yaw={fc.attitude.yaw:.2f}°")
        print(f"电机: {motors}")

        time.sleep(0.0025)  # 400Hz

4. 行业案例

案例1：DJI Phantom系列

飞控架构：

• 处理器：STM32F427（168MHz ARM Cortex-M4）
• IMU：MPU6000（加速度计+陀螺仪）+ HMC5883L（磁力计）
• 气压计：MS5611
• 控制频率：400Hz
• 传感器融合：扩展卡尔曼滤波（EKF）

实施效果：

• 悬停精度：垂直±0.5m，水平±1.5m
• 姿态控制精度：±0.02°
• 最大抗风：10m/s（5级风）

案例2：PX4开源飞控

技术特点：

• 平台：Pixhawk硬件
• RTOS：NuttX实时操作系统
• 算法：级联PID + EKF2状态估计
• 通信：MAVLink协议

实施效果：

• 支持多种飞行器构型
• 开源社区活跃
• 商业应用广泛

5. 性能优化

5.1 控制频率优化

多速率控制：

// 400Hz主循环
void main_loop_400hz() {
    // 高频：姿态控制
    attitude_control();
    motor_output();
}

// 100Hz循环
void loop_100hz() {
    // 中频：高度/位置控制
    altitude_control();
    position_control();
}

// 10Hz循环
void loop_10hz() {
    // 低频：任务管理
    mission_update();
    telemetry_send();
}

5.2 传感器校准

加速度计校准（六面法）：

def calibrate_accelerometer():
    """六面法校准加速度计"""
    samples = []

    print("将飞机放置在6个面，每个面采集100个样本")
    for face in ['上', '下', '左', '右', '前', '后']:
        input(f"按Enter开始采集{face}面...")
        face_samples = []

        for _ in range(100):
            acc = read_accelerometer()
            face_samples.append(acc)
            time.sleep(0.01)

        samples.append(np.mean(face_samples, axis=0))

    # 计算偏移和缩放
    offset = (samples[0] + samples[1]) / 2
    scale_z = 9.8 / ((samples[0][2] - samples[1][2]) / 2)

    print(f"偏移: {offset}")
    print(f"缩放: {scale_z}")

    return offset, scale_z

6. 协议对比

飞控协议对比

协议	开发者	开源	应用
MAVLink	DroneCode	✓	PX4/Ardupilot
MSP	MultiWii/Betaflight	✓	穿越机
ULog	PX4	✓	日志记录
DJI SDK	DJI	✗	DJI无人机