无人机飞控系统原理深度解析

无人机飞控系统原理深度解析

一、引言

无人机飞控系统（Flight Control System）是无人机的核心大脑，负责接收传感器数据、执行控制算法、输出控制指令，确保无人机稳定飞行并完成预定任务。本文将从底层原理到实际代码实现，全面解析无人机飞控系统的工作机制。

二、飞控系统架构

2.1 系统组成

飞控系统主要由以下几个核心模块组成：

• 传感器模块：IMU（惯性测量单元）、气压计、磁力计、GPS等
• 状态估计模块：姿态解算、位置估计、速度估计
• 控制器模块：PID控制器、姿态控制、位置控制
• 执行器模块：电机控制、PWM输出
• 通信模块：遥控接收、数传通信、数据记录

2.2 数据流程

传感器数据 → 数据滤波 → 状态估计 → 控制算法 → 执行器输出↑                                              ↓└──────────────── 反馈回路 ←──────────────────┘

三、核心算法详解

3.1 姿态解算算法

姿态解算是飞控系统的基础，常用的算法包括互补滤波、卡尔曼滤波和Mahony算法。

3.1.1 四元数姿态表示

// 四元数结构体定义
typedef struct {float q0, q1, q2, q3;  // 四元数的四个分量
} Quaternion;// 四元数归一化
void quaternion_normalize(Quaternion* q) {float norm = sqrt(q->q0*q->q0 + q->q1*q->q1 + q->q2*q->q2 + q->q3*q->q3);if (norm > 0.0f) {float inv_norm = 1.0f / norm;q->q0 *= inv_norm;q->q1 *= inv_norm;q->q2 *= inv_norm;q->q3 *= inv_norm;}
}// 四元数转欧拉角
void quaternion_to_euler(Quaternion* q, float* roll, float* pitch, float* yaw) {// Roll (x-axis rotation)float sinr_cosp = 2.0f * (q->q0 * q->q1 + q->q2 * q->q3);float cosr_cosp = 1.0f - 2.0f * (q->q1 * q->q1 + q->q2 * q->q2);*roll = atan2f(sinr_cosp, cosr_cosp);// Pitch (y-axis rotation)float sinp = 2.0f * (q->q0 * q->q2 - q->q3 * q->q1);if (fabsf(sinp) >= 1.0f)*pitch = copysignf(M_PI / 2.0f, sinp);else*pitch = asinf(sinp);// Yaw (z-axis rotation)float siny_cosp = 2.0f * (q->q0 * q->q3 + q->q1 * q->q2);float cosy_cosp = 1.0f - 2.0f * (q->q2 * q->q2 + q->q3 * q->q3);*yaw = atan2f(siny_cosp, cosy_cosp);
}

3.1.2 Mahony互补滤波算法

// Mahony算法参数
#define Kp 2.0f     // 比例增益
#define Ki 0.01f    // 积分增益
#define dt 0.01f    // 采样周期（100Hz）typedef struct {Quaternion q;           // 当前姿态四元数float integralFBx;      // 积分误差float integralFBy;float integralFBz;
} MahonyAHRS;// Mahony姿态更新算法
void mahony_update(MahonyAHRS* ahrs, float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {float recipNorm;float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;float hx, hy, bx, bz;float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;float halfex, halfey, halfez;float qa, qb, qc;// 使用IMU数据进行姿态更新if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {// 归一化加速度计测量值recipNorm = 1.0f / sqrtf(ax * ax + ay * ay + az * az);ax *= recipNorm;ay *= recipNorm;az *= recipNorm;// 归一化磁力计测量值if(!((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f))) {recipNorm = 1.0f / sqrtf(mx * mx + my * my + mz * mz);mx *= recipNorm;my *= recipNorm;mz *= recipNorm;}// 辅助变量以避免重复计算q0q0 = ahrs->q.q0 * ahrs->q.q0;q0q1 = ahrs->q.q0 * ahrs->q.q1;q0q2 = ahrs->q.q0 * ahrs->q.q2;q0q3 = ahrs->q.q0 * ahrs->q.q3;q1q1 = ahrs->q.q1 * ahrs->q.q1;q1q2 = ahrs->q.q1 * ahrs->q.q2;q1q3 = ahrs->q.q1 * ahrs->q.q3;q2q2 = ahrs->q.q2 * ahrs->q.q2;q2q3 = ahrs->q.q2 * ahrs->q.q3;q3q3 = ahrs->q.q3 * ahrs->q.q3;// 参考磁场方向hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));bx = sqrtf(hx * hx + hy * hy);bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));// 估计重力和磁场方向halfvx = q1q3 - q0q2;halfvy = q0q1 + q2q3;halfvz = q0q0 - 0.5f + q3q3;halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);// 误差是估计方向和测量方向的叉积halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);// 应用积分反馈if(Ki > 0.0f) {ahrs->integralFBx += Ki * halfex * dt;ahrs->integralFBy += Ki * halfey * dt;ahrs->integralFBz += Ki * halfez * dt;gx += ahrs->integralFBx;gy += ahrs->integralFBy;gz += ahrs->integralFBz;} else {ahrs->integralFBx = 0.0f;ahrs->integralFBy = 0.0f;ahrs->integralFBz = 0.0f;}// 应用比例反馈gx += Kp * halfex;gy += Kp * halfey;gz += Kp * halfez;}// 整合四元数变化率gx *= (0.5f * dt);gy *= (0.5f * dt);gz *= (0.5f * dt);qa = ahrs->q.q0;qb = ahrs->q.q1;qc = ahrs->q.q2;ahrs->q.q0 += (-qb * gx - qc * gy - ahrs->q.q3 * gz);ahrs->q.q1 += (qa * gx + qc * gz - ahrs->q.q3 * gy);ahrs->q.q2 += (qa * gy - qb * gz + ahrs->q.q3 * gx);ahrs->q.q3 += (qa * gz + qb * gy - qc * gx);// 归一化四元数quaternion_normalize(&ahrs->q);
}

3.2 PID控制算法

PID控制器是飞控系统中最基础也是最重要的控制算法。

// PID控制器结构体
typedef struct {float kp;           // 比例系数float ki;           // 积分系数float kd;           // 微分系数float integral;     // 积分累计float prev_error;   // 上次误差float integral_limit; // 积分限幅float output_limit;   // 输出限幅
} PIDController;// PID初始化
void pid_init(PIDController* pid, float kp, float ki, float kd, float integral_limit, float output_limit) {pid->kp = kp;pid->ki = ki;pid->kd = kd;pid->integral = 0.0f;pid->prev_error = 0.0f;pid->integral_limit = integral_limit;pid->output_limit = output_limit;
}// PID计算
float pid_compute(PIDController* pid, float setpoint, float measurement, float dt) {float error = setpoint - measurement;float derivative;float output;// 计算积分项（带抗积分饱和）pid->integral += error * dt;if (pid->integral > pid->integral_limit) {pid->integral = pid->integral_limit;} else if (pid->integral < -pid->integral_limit) {pid->integral = -pid->integral_limit;}// 计算微分项if (dt > 0.0f) {derivative = (error - pid->prev_error) / dt;} else {derivative = 0.0f;}// 计算PID输出output = pid->kp * error + pid->ki * pid->integral + pid->kd * derivative;// 输出限幅if (output > pid->output_limit) {output = pid->output_limit;} else if (output < -pid->output_limit) {output = -pid->output_limit;}// 保存当前误差pid->prev_error = error;return output;
}// 重置PID控制器
void pid_reset(PIDController* pid) {pid->integral = 0.0f;pid->prev_error = 0.0f;
}

3.3 串级PID控制

无人机通常采用串级PID控制结构，外环控制角度，内环控制角速度。

// 姿态控制器结构体
typedef struct {PIDController angle_pid[3];    // 角度环PID（roll, pitch, yaw）PIDController rate_pid[3];     // 角速度环PID
} AttitudeController;// 姿态控制器初始化
void attitude_controller_init(AttitudeController* ctrl) {// 角度环PID参数（示例值）pid_init(&ctrl->angle_pid[0], 4.5f, 0.0f, 0.0f, 100.0f, 250.0f);  // Rollpid_init(&ctrl->angle_pid[1], 4.5f, 0.0f, 0.0f, 100.0f, 250.0f);  // Pitchpid_init(&ctrl->angle_pid[2], 3.0f, 0.0f, 0.0f, 100.0f, 180.0f);  // Yaw// 角速度环PID参数（示例值）pid_init(&ctrl->rate_pid[0], 0.15f, 0.1f, 0.004f, 50.0f, 400.0f);  // Roll ratepid_init(&ctrl->rate_pid[1], 0.15f, 0.1f, 0.004f, 50.0f, 400.0f);  // Pitch ratepid_init(&ctrl->rate_pid[2], 0.2f, 0.1f, 0.0f, 50.0f, 400.0f);     // Yaw rate
}// 姿态控制计算
typedef struct {float roll, pitch, yaw, throttle;
} ControlOutput;void attitude_control_update(AttitudeController* ctrl,float target_roll, float target_pitch, float target_yaw,float current_roll, float current_pitch, float current_yaw,float roll_rate, float pitch_rate, float yaw_rate,float dt, ControlOutput* output) {// 外环：角度控制，输出期望角速度float target_roll_rate = pid_compute(&ctrl->angle_pid[0], target_roll, current_roll, dt);float target_pitch_rate = pid_compute(&ctrl->angle_pid[1], target_pitch, current_pitch, dt);float target_yaw_rate = pid_compute(&ctrl->angle_pid[2], target_yaw, current_yaw, dt);// 内环：角速度控制，输出控制量output->roll = pid_compute(&ctrl->rate_pid[0], target_roll_rate, roll_rate, dt);output->pitch = pid_compute(&ctrl->rate_pid[1], target_pitch_rate, pitch_rate, dt);output->yaw = pid_compute(&ctrl->rate_pid[2], target_yaw_rate, yaw_rate, dt);
}

四、电机控制与混控

4.1 四旋翼混控矩阵

四旋翼无人机需要将控制输出转换为四个电机的转速。

// 电机混控类型
typedef enum {QUAD_X,     // X型四旋翼QUAD_PLUS,  // +型四旋翼
} QuadType;// 电机输出结构
typedef struct {uint16_t motor[4];  // 四个电机的PWM值
} MotorOutput;// 混控矩阵计算
void motor_mixing(ControlOutput* ctrl, float throttle, QuadType type, MotorOutput* motors) {float motor_outputs[4];if (type == QUAD_X) {// X型四旋翼混控// Motor 1: 前右，顺时针motor_outputs[0] = throttle - ctrl->roll - ctrl->pitch + ctrl->yaw;// Motor 2: 后右，逆时针motor_outputs[1] = throttle - ctrl->roll + ctrl->pitch - ctrl->yaw;// Motor 3: 后左，顺时针motor_outputs[2] = throttle + ctrl->roll + ctrl->pitch + ctrl->yaw;// Motor 4: 前左，逆时针motor_outputs[3] = throttle + ctrl->roll - ctrl->pitch - ctrl->yaw;} else if (type == QUAD_PLUS) {// +型四旋翼混控// Motor 1: 前，顺时针motor_outputs[0] = throttle - ctrl->pitch + ctrl->yaw;// Motor 2: 右，逆时针motor_outputs[1] = throttle - ctrl->roll - ctrl->yaw;// Motor 3: 后，顺时针motor_outputs[2] = throttle + ctrl->pitch + ctrl->yaw;// Motor 4: 左，逆时针motor_outputs[3] = throttle + ctrl->roll - ctrl->yaw;}// 转换为PWM值（1000-2000us）for (int i = 0; i < 4; i++) {// 限幅if (motor_outputs[i] < 0) motor_outputs[i] = 0;if (motor_outputs[i] > 1000) motor_outputs[i] = 1000;// 映射到PWM范围motors->motor[i] = (uint16_t)(1000 + motor_outputs[i]);}
}// PWM输出函数（硬件相关）
void pwm_output(MotorOutput* motors) {// 设置定时器比较值，输出PWM信号TIM3->CCR1 = motors->motor[0];  // 通道1TIM3->CCR2 = motors->motor[1];  // 通道2TIM3->CCR3 = motors->motor[2];  // 通道3TIM3->CCR4 = motors->motor[3];  // 通道4
}

五、传感器数据处理

5.1 IMU数据滤波

// 低通滤波器结构
typedef struct {float alpha;        // 滤波系数float prev_output;  // 上次输出
} LowPassFilter;// 低通滤波器初始化
void lpf_init(LowPassFilter* lpf, float cutoff_freq, float sample_rate) {float rc = 1.0f / (2.0f * M_PI * cutoff_freq);float dt = 1.0f / sample_rate;lpf->alpha = dt / (rc + dt);lpf->prev_output = 0.0f;
}// 低通滤波
float lpf_apply(LowPassFilter* lpf, float input) {lpf->prev_output = lpf->alpha * input + (1.0f - lpf->alpha) * lpf->prev_output;return lpf->prev_output;
}// IMU数据结构
typedef struct {float acc[3];   // 加速度计数据float gyro[3];  // 陀螺仪数据float mag[3];   // 磁力计数据
} IMUData;// IMU滤波器组
typedef struct {LowPassFilter acc_lpf[3];LowPassFilter gyro_lpf[3];
} IMUFilters;// IMU数据滤波处理
void imu_filter_update(IMUFilters* filters, IMUData* raw, IMUData* filtered) {for (int i = 0; i < 3; i++) {filtered->acc[i] = lpf_apply(&filters->acc_lpf[i], raw->acc[i]);filtered->gyro[i] = lpf_apply(&filters->gyro_lpf[i], raw->gyro[i]);}// 磁力计通常不需要高频滤波memcpy(filtered->mag, raw->mag, sizeof(float) * 3);
}

5.2 传感器校准

// 传感器校准参数
typedef struct {float offset[3];    // 零偏float scale[3];     // 比例因子float rotation[9];  // 旋转矩阵（用于轴对齐）
} CalibrationParams;// 加速度计校准（六面校准法）
void accelerometer_calibration(IMUData* samples, int num_samples, CalibrationParams* cal) {// 简化示例：计算静止时的偏移float sum[3] = {0, 0, 0};for (int i = 0; i < num_samples; i++) {sum[0] += samples[i].acc[0];sum[1] += samples[i].acc[1];sum[2] += samples[i].acc[2];}// 计算平均值作为偏移（Z轴需要减去重力加速度）cal->offset[0] = sum[0] / num_samples;cal->offset[1] = sum[1] / num_samples;cal->offset[2] = sum[2] / num_samples - 9.81f;  // 假设Z轴向上// 比例因子初始化为1cal->scale[0] = cal->scale[1] = cal->scale[2] = 1.0f;
}// 陀螺仪零偏校准
void gyroscope_calibration(IMUData* samples, int num_samples,CalibrationParams* cal) {float sum[3] = {0, 0, 0};// 静止状态下采集数据for (int i = 0; i < num_samples; i++) {sum[0] += samples[i].gyro[0];sum[1] += samples[i].gyro[1];sum[2] += samples[i].gyro[2];}// 计算零偏cal->offset[0] = sum[0] / num_samples;cal->offset[1] = sum[1] / num_samples;cal->offset[2] = sum[2] / num_samples;
}// 应用校准参数
void apply_calibration(float* raw, CalibrationParams* cal, float* calibrated) {// 减去偏移float temp[3];temp[0] = (raw[0] - cal->offset[0]) * cal->scale[0];temp[1] = (raw[1] - cal->offset[1]) * cal->scale[1];temp[2] = (raw[2] - cal->offset[2]) * cal->scale[2];// 应用旋转矩阵（如果需要轴对齐）calibrated[0] = cal->rotation[0] * temp[0] + cal->rotation[1] * temp[1] + cal->rotation[2] * temp[2];calibrated[1] = cal->rotation[3] * temp[0] + cal->rotation[4] * temp[1] + cal->rotation[5] * temp[2];calibrated[2] = cal->rotation[6] * temp[0] + cal->rotation[7] * temp[1] + cal->rotation[8] * temp[2];
}

六、飞行模式实现

6.1 飞行模式定义

// 飞行模式枚举
typedef enum {MODE_STABILIZE,     // 自稳模式MODE_ALTITUDE_HOLD, // 定高模式MODE_POSITION_HOLD, // 定点模式MODE_AUTO,          // 自动模式MODE_RTL,           // 返航模式MODE_LAND,          // 自动降落MODE_ACRO           // 特技模式
} FlightMode;// 飞行状态结构
typedef struct {FlightMode mode;bool armed;             // 解锁状态float battery_voltage;  // 电池电压float altitude;         // 高度float climb_rate;       // 爬升率struct {float lat, lon;     // GPS坐标uint8_t num_sats;   // 卫星数量float hdop;         // 水平精度因子} gps;
} FlightStatus;

6.2 定高模式实现

// 高度控制器
typedef struct {PIDController altitude_pid;  // 高度环PIDController climb_rate_pid; // 爬升率环float target_altitude;float max_climb_rate;
} AltitudeController;// 定高控制
float altitude_hold_control(AltitudeController* ctrl,float current_altitude,float current_climb_rate,float dt) {// 高度环：计算目标爬升率float target_climb_rate = pid_compute(&ctrl->altitude_pid,ctrl->target_altitude,current_altitude, dt);// 限制爬升率if (target_climb_rate > ctrl->max_climb_rate) {target_climb_rate = ctrl->max_climb_rate;} else if (target_climb_rate < -ctrl->max_climb_rate) {target_climb_rate = -ctrl->max_climb_rate;}// 爬升率环：计算油门调整量float throttle_adjust = pid_compute(&ctrl->climb_rate_pid,target_climb_rate,current_climb_rate, dt);return throttle_adjust;
}// 气压计高度融合
float baro_altitude_fusion(float baro_alt, float accel_z, float* velocity_z, float dt) {static float fused_altitude = 0.0f;static float fused_velocity = 0.0f;// 预测步骤fused_altitude += fused_velocity * dt;fused_velocity += accel_z * dt;// 校正步骤（互补滤波）const float alpha = 0.98f;  // 融合系数float altitude_error = baro_alt - fused_altitude;fused_altitude += (1.0f - alpha) * altitude_error;fused_velocity += (1.0f - alpha) * altitude_error / dt * 0.1f;*velocity_z = fused_velocity;return fused_altitude;
}

6.3 定点模式实现

// 位置控制器
typedef struct {PIDController pos_pid[2];      // 位置环（X, Y）PIDController vel_pid[2];      // 速度环float target_position[2];      // 目标位置float max_velocity;            // 最大速度float max_lean_angle;          // 最大倾角
} PositionController;// GPS坐标转换为本地坐标
void gps_to_local(float lat, float lon, float ref_lat, float ref_lon,float* x, float* y) {const float earth_radius = 6371000.0f;  // 地球半径（米）// 简化的墨卡托投影*x = earth_radius * (lon - ref_lon) * M_PI / 180.0f * cosf(ref_lat * M_PI / 180.0f);*y = earth_radius * (lat - ref_lat) * M_PI / 180.0f;
}// 定点控制
void position_hold_control(PositionController* ctrl,float current_pos[2],float current_vel[2],float dt,float* target_roll,float* target_pitch) {float target_vel[2];float angle_output[2];// 位置环：计算目标速度for (int i = 0; i < 2; i++) {target_vel[i] = pid_compute(&ctrl->pos_pid[i],ctrl->target_position[i],current_pos[i], dt);// 限制速度if (target_vel[i] > ctrl->max_velocity) {target_vel[i] = ctrl->max_velocity;} else if (target_vel[i] < -ctrl->max_velocity) {target_vel[i] = -ctrl->max_velocity;}// 速度环：计算倾角angle_output[i] = pid_compute(&ctrl->vel_pid[i],target_vel[i],current_vel[i], dt);}// 转换为横滚和俯仰角度*target_roll = -angle_output[1];   // Y轴速度控制横滚*target_pitch = angle_output[0];   // X轴速度控制俯仰// 限制最大倾角if (*target_roll > ctrl->max_lean_angle) {*target_roll = ctrl->max_lean_angle;} else if (*target_roll < -ctrl->max_lean_angle) {*target_roll = -ctrl->max_lean_angle;}if (*target_pitch > ctrl->max_lean_angle) {*target_pitch = ctrl->max_lean_angle;} else if (*target_pitch < -ctrl->max_lean_angle) {*target_pitch = -ctrl->max_lean_angle;}
}

七、主控制循环

7.1 主循环架构

// 主控制循环（运行频率：400Hz）
void flight_control_loop(void) {static uint32_t last_time = 0;uint32_t current_time = get_system_time_us();float dt = (current_time - last_time) * 1e-6f;if (dt < 0.0025f) return;  // 限制最高频率400Hz// 1. 读取传感器数据IMUData raw_imu, filtered_imu;read_imu_data(&raw_imu);imu_filter_update(&imu_filters, &raw_imu, &filtered_imu);// 2. 姿态解算mahony_update(&ahrs, filtered_imu.gyro[0], filtered_imu.gyro[1], filtered_imu.gyro[2],filtered_imu.acc[0], filtered_imu.acc[1], filtered_imu.acc[2],filtered_imu.mag[0], filtered_imu.mag[1], filtered_imu.mag[2]);// 3. 获取当前姿态float roll, pitch, yaw;quaternion_to_euler(&ahrs.q, &roll, &pitch, &yaw);// 4. 读取遥控器输入RCInput rc_input;read_rc_input(&rc_input);// 5. 根据飞行模式处理控制ControlOutput control_output = {0};switch(flight_status.mode) {case MODE_STABILIZE:// 自稳模式：直接将遥控器输入映射为目标角度float target_roll = rc_input.roll * MAX_ANGLE;float target_pitch = rc_input.pitch * MAX_ANGLE;float target_yaw_rate = rc_input.yaw * MAX_YAW_RATE;attitude_control_update(&attitude_controller,target_roll, target_pitch, 0,roll, pitch, yaw,filtered_imu.gyro[0], filtered_imu.gyro[1], filtered_imu.gyro[2],dt, &control_output);break;case MODE_ALTITUDE_HOLD:// 定高模式float current_altitude = get_altitude();float climb_rate = get_climb_rate();// 高度控制float throttle_adjust = altitude_hold_control(&altitude_controller,current_altitude,climb_rate, dt);// 姿态控制同自稳模式attitude_control_update(&attitude_controller,rc_input.roll * MAX_ANGLE,rc_input.pitch * MAX_ANGLE, 0,roll, pitch, yaw,filtered_imu.gyro[0],filtered_imu.gyro[1],filtered_imu.gyro[2],dt, &control_output);// 应用油门调整control_output.throttle = HOVER_THROTTLE + throttle_adjust;break;case MODE_POSITION_HOLD:// 定点模式if (flight_status.gps.num_sats >= 6) {float current_pos[2], current_vel[2];get_position_and_velocity(current_pos, current_vel);float target_roll_angle, target_pitch_angle;position_hold_control(&position_controller,current_pos, current_vel, dt,&target_roll_angle, &target_pitch_angle);attitude_control_update(&attitude_controller,target_roll_angle, target_pitch_angle, 0,roll, pitch, yaw,filtered_imu.gyro[0],filtered_imu.gyro[1],filtered_imu.gyro[2],dt, &control_output);}break;// 其他模式...}// 6. 电机混控MotorOutput motor_output;motor_mixing(&control_output, rc_input.throttle * 1000, QUAD_X, &motor_output);// 7. 输出到电机if (flight_status.armed) {pwm_output(&motor_output);} else {// 未解锁，停止电机stop_motors();}// 8. 记录日志log_flight_data(current_time, &filtered_imu, roll, pitch, yaw,&control_output, &motor_output);// 9. 安全检查safety_check();last_time = current_time;
}// 安全检查函数
void safety_check(void) {// 检查电池电压if (flight_status.battery_voltage < MIN_VOLTAGE) {// 触发低电压保护initiate_emergency_landing();}// 检查姿态异常float roll, pitch, yaw;quaternion_to_euler(&ahrs.q, &roll, &pitch, &yaw);if (fabsf(roll) > MAX_SAFE_ANGLE || fabsf(pitch) > MAX_SAFE_ANGLE) {// 姿态超限，切换到自稳模式flight_status.mode = MODE_STABILIZE;}// 检查失控保护if (is_rc_lost()) {// 遥控信号丢失，执行失控保护if (flight_status.gps.num_sats >= 6) {flight_status.mode = MODE_RTL;  // 返航} else {flight_status.mode = MODE_LAND; // 原地降落}}
}

7.2 中断服务程序

// 定时器中断（1kHz，用于精确计时）
void TIM2_IRQHandler(void) {if (TIM2->SR & TIM_SR_UIF) {TIM2->SR &= ~TIM_SR_UIF;  // 清除中断标志system_tick_count++;// 每2.5ms触发一次主控制循环if (system_tick_count % 25 == 0) {trigger_control_loop = true;}}
}// UART中断（接收GPS数据）
void USART1_IRQHandler(void) {if (USART1->SR & USART_SR_RXNE) {uint8_t data = USART1->DR;gps_parse_byte(data);  // 解析GPS数据}
}// 外部中断（接收PPM/SBUS遥控信号）
void EXTI0_IRQHandler(void) {if (EXTI->PR & EXTI_PR_PR0) {EXTI->PR = EXTI_PR_PR0;  // 清除中断标志uint32_t current_time = get_system_time_us();ppm_decode(current_time);  // 解码PPM信号}
}

八、通信协议

8.1 MAVLink协议实现

// MAVLink消息处理
void mavlink_send_heartbeat(void) {mavlink_message_t msg;mavlink_heartbeat_t heartbeat;heartbeat.type = MAV_TYPE_QUADROTOR;heartbeat.autopilot = MAV_AUTOPILOT_GENERIC;heartbeat.base_mode = flight_status.armed ? MAV_MODE_FLAG_SAFETY_ARMED : 0;heartbeat.custom_mode = flight_status.mode;heartbeat.system_status = MAV_STATE_ACTIVE;mavlink_msg_heartbeat_encode(SYSTEM_ID, COMPONENT_ID, &msg, &heartbeat);mavlink_send_message(&msg);
}// 发送姿态数据
void mavlink_send_attitude(void) {mavlink_message_t msg;mavlink_attitude_t attitude;float roll, pitch, yaw;quaternion_to_euler(&ahrs.q, &roll, &pitch, &yaw);attitude.time_boot_ms = get_system_time_ms();attitude.roll = roll;attitude.pitch = pitch;attitude.yaw = yaw;attitude.rollspeed = filtered_imu.gyro[0];attitude.pitchspeed = filtered_imu.gyro[1];attitude.yawspeed = filtered_imu.gyro[2];mavlink_msg_attitude_encode(SYSTEM_ID, COMPONENT_ID,&msg, &attitude);mavlink_send_message(&msg);
}

九、参数调试与优化

9.1 PID参数整定

PID参数整定是飞控调试的关键步骤，通常按照以下顺序进行：

1. 角速度环调试（最内环）

   • 先将P值从小逐渐增大，直到出现轻微震荡
   • 将P值降低到震荡值的60-70%
   • 逐渐增加D值以减少超调
   • 最后微调I值消除稳态误差

2. 角度环调试（外环）

   • 在角速度环稳定的基础上调试
   • P值通常在3-6之间
   • 一般不需要I和D项

3. 位置环调试

   • 最外环，依赖于姿态控制的稳定性
   • 参数值通常较小，避免过度响应

9.2 性能优化建议

1. 采样频率优化

   • IMU采样：800-1000Hz
   • 姿态解算：400-500Hz
   • 控制循环：400Hz
   • GPS更新：5-10Hz
   • 气压计：20-50Hz

2. 滤波器设计

   • 陀螺仪：低通滤波器截止频率20-30Hz
   • 加速度计：低通滤波器截止频率10-15Hz
   • 气压计：移动平均或卡尔曼滤波

3. 计算优化

   • 使用定点数运算代替浮点数（在低端MCU上）
   • 预计算三角函数值表
   • 使用快速平方根算法

十、实际应用案例

10.1 自动起飞序列

// 自动起飞状态机
typedef enum {TAKEOFF_IDLE,TAKEOFF_MOTOR_SPIN,TAKEOFF_ASCENDING,TAKEOFF_COMPLETE
} TakeoffState;typedef struct {TakeoffState state;float target_altitude;float takeoff_speed;uint32_t start_time;
} TakeoffController;void auto_takeoff_update(TakeoffController* takeoff, float current_alt, float dt) {switch(takeoff->state) {case TAKEOFF_IDLE:// 等待起飞命令if (received_takeoff_command()) {takeoff->state = TAKEOFF_MOTOR_SPIN;takeoff->start_time = get_system_time_ms();}break;case TAKEOFF_MOTOR_SPIN:// 电机低速运转，准备起飞set_throttle(MOTOR_SPIN_THROTTLE);if (get_system_time_ms() - takeoff->start_time > 2000) {takeoff->state = TAKEOFF_ASCENDING;}break;case TAKEOFF_ASCENDING:// 上升到目标高度if (current_alt < takeoff->target_altitude - 0.5f) {// 继续上升float throttle = HOVER_THROTTLE + takeoff->takeoff_speed * THROTTLE_SCALE;set_throttle(throttle);} else {// 接近目标高度，减速float error = takeoff->target_altitude - current_alt;float throttle = HOVER_THROTTLE + error * 100.0f;set_throttle(throttle);if (fabsf(error) < 0.1f) {takeoff->state = TAKEOFF_COMPLETE;}}break;case TAKEOFF_COMPLETE:// 起飞完成，切换到悬停模式flight_status.mode = MODE_ALTITUDE_HOLD;altitude_controller.target_altitude = takeoff->target_altitude;takeoff->state = TAKEOFF_IDLE;break;}
}

10.2 紧急降落程序

void emergency_landing(void) {static float landing_throttle = HOVER_THROTTLE;static uint32_t landing_start_time = 0;if (landing_start_time == 0) {landing_start_time = get_system_time_ms();}// 保持水平姿态attitude_control_update(&attitude_controller,0, 0, 0,  // 目标角度全为0roll, pitch, yaw,gyro_x, gyro_y, gyro_z,dt, &control_output);// 逐渐降低油门uint32_t elapsed_time = get_system_time_ms() - landing_start_time;if (elapsed_time < 5000) {// 前5秒缓慢下降landing_throttle = HOVER_THROTTLE - (elapsed_time / 5000.0f) * (HOVER_THROTTLE * 0.3f);} else {// 5秒后加速下降landing_throttle = HOVER_THROTTLE * 0.7f - ((elapsed_time - 5000) / 10000.0f) * (HOVER_THROTTLE * 0.5f);}// 限制最小油门if (landing_throttle < MIN_THROTTLE + 50) {landing_throttle = MIN_THROTTLE + 50;}// 检测着陆if (get_altitude() < 0.2f && get_climb_rate() < 0.1f) {// 已着陆，关闭电机disarm_motors();landing_start_time = 0;}set_throttle(landing_throttle);
}

十一、总结

无人机飞控系统是一个复杂的实时控制系统，涉及传感器融合、姿态估计、控制理论、嵌入式编程等多个领域。本文从底层原理出发，详细介绍了飞控系统的核心算法和实现细节，包括：

1. 姿态解算：使用四元数和Mahony算法实现高效稳定的姿态估计
2. PID控制：通过串级PID实现精确的姿态和位置控制
3. 传感器处理：滤波、校准和数据融合技术
4. 飞行模式：多种飞行模式的实现策略
5. 安全机制：失控保护、低电压保护等安全功能

开发建议

1. 硬件选择

   • MCU：建议使用STM32F4系列或更高性能的处理器
   • IMU：MPU6050/MPU9250或更高精度的传感器
   • 气压计：MS5611或BMP280
   • GPS：UBLOX M8N或更新型号

2. 软件架构

   • 采用实时操作系统（如FreeRTOS）管理任务
   • 模块化设计，便于维护和扩展
   • 充分的错误处理和日志记录

3. 测试流程

   • 地面测试：先在地面固定状态下测试各项功能
   • 系留测试：用绳索限制飞行范围进行初步飞行测试
   • 逐步放开：从低高度、小范围逐步扩大测试范围

4. 安全注意事项

   • 始终在开阔、无人的区域进行测试
   • 准备紧急停机开关
   • 定期检查硬件连接和电池状态
   • 遵守当地无人机法规

通过深入理解这些原理和实现细节，开发者可以构建自己的飞控系统，或者在现有开源飞控（如PX4、ArduPilot）基础上进行定制开发。飞控开发是一个需要理论与实践相结合的领域，希望本文能为您的学习和开发提供帮助。

参考资源

• PX4开源飞控：https://px4.io/
• ArduPilot：https://ardupilot.org/
• Betaflight（穿越机飞控）：https://betaflight.com/
• MAVLink协议：https://mavlink.io/
• 《多旋翼飞行器设计与控制》- Quan Quan
• 《Quadcopter Dynamics and Control》- Randal Beard

---

注：本文提供的代码为教学示例，实际使用时需要根据具体硬件平台和需求进行调整。飞行器开发具有一定危险性，请确保充分的测试和安全措施。