1. 四旋翼无人机飞行控制系统概述
四旋翼无人机作为一种典型的垂直起降飞行器,其飞行控制系统是整个无人机设计的核心。与固定翼无人机不同,四旋翼通过四个电调控制电机转速变化来产生升力和力矩,实现姿态调整和位置控制。这种独特的飞行机制决定了其控制系统的复杂性。
在硬件架构上,STM32系列微控制器因其出色的实时性能和丰富的外设接口,成为飞行控制器的理想选择。我们通常会选用STM32F4或STM32H7系列,它们具备以下关键特性:
- 168MHz至400MHz的主频满足实时控制需求
- 硬件FPU加速浮点运算
- 多达16个定时器通道用于PWM输出
- 丰富的通信接口(USART、SPI、I2C等)
- 内置DMA减轻CPU负担
飞行控制软件需要实现的核心功能模块包括:
- 传感器数据采集与融合(IMU、气压计等)
- 姿态解算算法(互补滤波、卡尔曼滤波)
- 控制算法实现(PID控制、串级控制)
- 无线通信协议处理
- 安全保护机制(失控保护、低电保护)
2. 硬件系统设计与选型
2.1 STM32主控选型分析
在四旋翼飞行控制器设计中,STM32F405/407和STM32H743是常见选择。我们以STM32F405为例,其关键参数对比如下:
| 特性 | STM32F405RG | STM32F407ZG | STM32H743VI |
|---|---|---|---|
| 主频 | 168MHz | 168MHz | 400MHz |
| Flash | 1MB | 1MB | 2MB |
| RAM | 192KB | 192KB | 1MB |
| FPU | 有 | 有 | 双精度FPU |
| 定时器通道 | 14 | 14 | 22 |
| 价格(参考) | 中等 | 中等 | 较高 |
对于大多数四旋翼应用,STM32F405已经足够。其192KB RAM可以满足:
- 传感器数据缓存(约20KB)
- 算法运算中间变量(约50KB)
- 通信缓冲区(约30KB)
- 系统堆栈空间(约10KB)
2.2 传感器模块选型与接口设计
飞行控制系统需要以下核心传感器:
-
IMU模块:MPU6050(6轴)或ICM20602(6轴)是基础选择,更高端可用BMI088+IST8310组合
- 接口:I2C(400kHz)或SPI(8MHz)
- 数据更新率:建议≥500Hz
-
气压计:BMP280或MS5611
- 用于高度估计
- 接口:I2C
-
磁力计:IST8310或HMC5883L
- 用于航向参考
- 注意远离电机干扰
-
GPS模块:ublox NEO-M8N
- 提供位置信息
- 接口:UART(115200bps)
硬件连接示例:
c复制// SPI1配置 - 连接IMU
GPIO_InitStruct.Pin = GPIO_PIN_5|GPIO_PIN_6|GPIO_PIN_7;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF5_SPI1;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
// I2C1配置 - 连接气压计/磁力计
hi2c1.Instance = I2C1;
hi2c1.Init.ClockSpeed = 400000;
hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_2;
hi2c1.Init.OwnAddress1 = 0;
hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
hi2c1.Init.OwnAddress2 = 0;
hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
注意:IMU的SPI接口布线应尽量短,并远离电机电源线,避免电磁干扰导致数据异常。
3. 软件架构设计
3.1 实时任务调度设计
四旋翼控制系统对实时性要求极高,我们采用基于FreeRTOS的多任务架构:
-
高频任务(1000Hz):
- IMU数据读取
- 姿态解算
- 角速率环控制
-
中频任务(500Hz):
- 姿态环控制
- 电机控制输出
-
低频任务(100Hz):
- 高度控制
- 位置控制
- 传感器校准
-
后台任务(10Hz):
- 无线通信处理
- 状态监测
- 日志记录
任务优先级设置示例:
c复制xTaskCreate(IMUTask, "IMU", 256, NULL, 6, NULL);
xTaskCreate(AttitudeTask, "Attitude", 512, NULL, 5, NULL);
xTaskCreate(ControlTask, "Control", 512, NULL, 4, NULL);
xTaskCreate(CommTask, "Communication", 1024, NULL, 3, NULL);
3.2 传感器数据处理流程
传感器数据处理采用生产者-消费者模式:
-
数据采集层:
- 定时触发传感器读取
- 原始数据校验(CRC/范围检查)
- 写入环形缓冲区
-
数据处理层:
- 传感器校准(零偏/比例因子)
- 单位转换(ADC值→物理量)
- 数据时间对齐
-
数据融合层:
- 加速度计/陀螺仪互补滤波
- 姿态解算(四元数/欧拉角)
- 高度估计(气压计+加速度计)
关键数据结构示例:
c复制typedef struct {
float accel[3]; // m/s²
float gyro[3]; // rad/s
float mag[3]; // uT
float pressure; // hPa
uint32_t timestamp; // us
} SensorData_t;
typedef struct {
float quat[4]; // 四元数
float euler[3]; // 滚转/俯仰/偏航(rad)
float height; // 高度(m)
float velocity[3]; // 速度(m/s)
} StateEstimate_t;
4. 核心控制算法实现
4.1 姿态解算算法
四旋翼姿态解算常用Mahony互补滤波算法,其实现步骤如下:
-
归一化加速度计测量值:
c复制void normalizeVector(float v[3]) { float recipNorm = 1.0f / sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]); v[0] *= recipNorm; v[1] *= recipNorm; v[2] *= recipNorm; } -
计算误差向量:
c复制// 估计重力方向(从四元数) float estimatedG[3]; estimatedG[0] = 2.0f*(q1*q3 - q0*q2); estimatedG[1] = 2.0f*(q0*q1 + q2*q3); estimatedG[2] = q0*q0 - q1*q1 - q2*q2 + q3*q3; // 计算加速度计测量值与估计值的叉积误差 float error[3]; error[0] = accel[1]*estimatedG[2] - accel[2]*estimatedG[1]; error[1] = accel[2]*estimatedG[0] - accel[0]*estimatedG[2]; error[2] = accel[0]*estimatedG[1] - accel[1]*estimatedG[0]; -
积分误差修正陀螺仪偏差:
c复制// 比例积分项 gyroBias[0] += error[0] * Ki * dt; gyroBias[1] += error[1] * Ki * dt; gyroBias[2] += error[2] * Ki * dt; // 修正陀螺仪读数 gyro[0] += Kp*error[0] + gyroBias[0]; gyro[1] += Kp*error[1] + gyroBias[1]; gyro[2] += Kp*error[2] + gyroBias[2]; -
四元数更新:
c复制// 四元数微分方程 qDot[0] = 0.5f*(-q1*gyro[0] - q2*gyro[1] - q3*gyro[2]); qDot[1] = 0.5f*(q0*gyro[0] + q2*gyro[2] - q3*gyro[1]); qDot[2] = 0.5f*(q0*gyro[1] - q1*gyro[2] + q3*gyro[0]); qDot[3] = 0.5f*(q0*gyro[2] + q1*gyro[1] - q2*gyro[0]); // 积分更新 q0 += qDot[0] * dt; q1 += qDot[1] * dt; q2 += qDot[2] * dt; q3 += qDot[3] * dt; // 四元数归一化 normalizeQuaternion();
实际应用中,Kp取值通常在0.5-2.0之间,Ki取值在0.001-0.01之间,需要通过实际飞行测试调整。
4.2 PID控制器实现
四旋翼采用串级PID控制结构:
-
角速率环(内环):
- 控制周期:1ms
- 输入:期望角速率(来自姿态环)
- 输出:电机力矩
-
姿态环(外环):
- 控制周期:2ms
- 输入:期望姿态角
- 输出:期望角速率
PID实现代码示例:
c复制typedef struct {
float kp, ki, kd;
float integral;
float prev_error;
float output_limit;
} PIDController;
float PID_Update(PIDController* pid, float error, float dt) {
// 比例项
float proportional = pid->kp * error;
// 积分项(抗饱和处理)
pid->integral += error * dt;
if(pid->integral > pid->output_limit) pid->integral = pid->output_limit;
else if(pid->integral < -pid->output_limit) pid->integral = -pid->output_limit;
float integral = pid->ki * pid->integral;
// 微分项
float derivative = pid->kd * (error - pid->prev_error) / dt;
pid->prev_error = error;
// 输出限幅
float output = proportional + integral + derivative;
if(output > pid->output_limit) output = pid->output_limit;
else if(output < -pid->output_limit) output = -pid->output_limit;
return output;
}
典型参数范围:
- 角速率环:Kp=0.1-0.3, Ki=0.05-0.2, Kd=0.001-0.01
- 姿态环:Kp=3.0-6.0, Ki=0.1-0.5, Kd=0.5-2.0
5. 电机控制与混控算法
5.1 PWM信号生成
STM32通过定时器产生PWM信号控制电调:
-
定时器配置(以TIM1为例):
c复制htim1.Instance = TIM1; htim1.Init.Prescaler = 84-1; // 84MHz/84 = 1MHz htim1.Init.CounterMode = TIM_COUNTERMODE_UP; htim1.Init.Period = 2000-1; // 50Hz(20000us) htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; htim1.Init.RepetitionCounter = 0; HAL_TIM_PWM_Init(&htim1); -
PWM通道配置:
c复制TIM_OC_InitTypeDef sConfigOC; sConfigOC.OCMode = TIM_OCMODE_PWM1; sConfigOC.Pulse = 1000; // 初始1000us(电机停转) sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; HAL_TIM_PWM_ConfigChannel(&htim1, &sConfigOC, TIM_CHANNEL_1); -
启动PWM输出:
c复制
HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
5.2 混控算法
四旋翼混控将控制量分配到四个电机:
c复制void MixerUpdate(float throttle, float roll, float pitch, float yaw) {
// 基础油门量(0-1)
float baseThrottle = constrain(throttle, 0.0f, 1.0f);
// 各电机输出
float motor1 = baseThrottle - pitch + roll - yaw;
float motor2 = baseThrottle - pitch - roll + yaw;
float motor3 = baseThrottle + pitch - roll - yaw;
float motor4 = baseThrottle + pitch + roll + yaw;
// 输出限幅(0-1)
motor1 = constrain(motor1, 0.0f, 1.0f);
motor2 = constrain(motor2, 0.0f, 1.0f);
motor3 = constrain(motor3, 0.0f, 1.0f);
motor4 = constrain(motor4, 0.0f, 1.0f);
// 转换为PWM脉宽(1000-2000us)
uint16_t pwm1 = 1000 + (uint16_t)(motor1 * 1000);
uint16_t pwm2 = 1000 + (uint16_t)(motor2 * 1000);
uint16_t pwm3 = 1000 + (uint16_t)(motor3 * 1000);
uint16_t pwm4 = 1000 + (uint16_t)(motor4 * 1000);
// 更新PWM输出
__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, pwm1);
__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, pwm2);
__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, pwm3);
__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, pwm4);
}
实际飞行前必须进行电机转向测试,确保各电机转向正确:前端两电机逆时针,后端两电机顺时针,以抵消反扭力。
6. 系统调试与参数整定
6.1 传感器校准流程
-
陀螺仪校准:
- 无人机静止放置
- 采集1000个样本计算零偏
- 存储校准参数到Flash
c复制void calibrateGyro() { float bias[3] = {0}; for(int i=0; i<1000; i++) { readGyro(rawData); bias[0] += rawData[0]; bias[1] += rawData[1]; bias[2] += rawData[2]; HAL_Delay(2); } gyroBias[0] = bias[0]/1000.0f; gyroBias[1] = bias[1]/1000.0f; gyroBias[2] = bias[2]/1000.0f; saveToFlash(gyroBias); } -
加速度计校准:
- 六面校准法(每个面采集200个样本)
- 计算比例因子和零偏
-
磁力计校准:
- 三维空间旋转无人机
- 椭圆拟合校准算法
- 补偿硬铁和软铁误差
6.2 PID参数整定方法
-
角速率环整定:
- 先设Ki=0, Kd=0
- 增大Kp直到无人机开始振荡,然后减小20%
- 加入少量Kd(0.001-0.005)抑制超调
- 最后加入Ki消除稳态误差
-
姿态环整定:
- 保持角速率环参数不变
- 同样方法调整姿态环Kp
- Ki通常为Kp的1/10-1/5
-
高度环整定:
- 在姿态环稳定后调整
- Kp从0.5开始逐步增加
- 需要较大的积分项(0.1-0.3)
调试时建议使用遥控器上的拨杆开关实现"参数调节模式",可以实时调整PID参数并观察响应。
7. 飞行测试与安全措施
7.1 分阶段测试流程
-
地面测试:
- 电机转向检查
- 遥控器通道映射验证
- 传感器数据监控
-
系留测试:
- 无人机用绳索固定
- 测试基本升力控制
- 验证姿态控制方向
-
自由飞行测试:
- 低高度悬停测试(1-2米)
- 逐步增加控制难度
- 记录飞行数据用于分析
7.2 安全保护机制
-
失控保护:
- 接收机信号丢失检测
- 自动降落或保持最后指令
c复制void checkRxLoss() { if(HAL_GetTick() - lastRxTime > 200) { // 200ms无信号 enterFailSafeMode(); } } -
低电保护:
- 电压监测(分压电路+ADC)
- 分级报警和自动返航
-
姿态异常保护:
- 倾角超过45°自动切断动力
- 高度异常变化检测
-
电机堵转检测:
- 电流突然增大判断
- 立即停止对应电机
8. 开发工具与调试技巧
8.1 开发环境搭建
-
工具链选择:
- IDE:Keil MDK或STM32CubeIDE
- 调试器:ST-Link V2
- 串口工具:Tera Term或Putty
-
工程配置要点:
- 启用FPU支持
- 优化等级-O2
- 启用硬件浮点支持
c复制// 在Keil中配置: // Options for Target → C/C++ → Define: ARM_MATH_CM4,USE_HAL_DRIVER,STM32F405xx // Floating Point Hardware: Use FPU // Optimization: -O2
8.2 调试方法
-
实时数据监控:
- 通过串口发送关键变量
- 使用FreeRTOS的trace功能
-
故障诊断技巧:
- 优先检查电源稳定性
- 使用逻辑分析仪抓取PWM信号
- 分段测试传感器数据流
-
性能优化建议:
- 使用DMA传输传感器数据
- 关键函数使用内联汇编优化
- 合理分配任务优先级
c复制// 示例:使用DMA加速SPI传输
hspi1.hdmatx = &hdma_spi1_tx;
HAL_SPI_Transmit_DMA(&hspi1, txData, length);
9. 进阶功能扩展
9.1 无线通信协议
-
遥控器通信:
- SBUS协议解析
- PPM信号解码
-
数传模块:
- MAVLink协议实现
- 遥测数据传输
c复制// MAVLink消息发送示例 mavlink_message_t msg; mavlink_msg_heartbeat_pack(1, 200, &msg, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC, MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, 0, MAV_STATE_ACTIVE); uint8_t buf[MAVLINK_MAX_PACKET_LEN]; uint16_t len = mavlink_msg_to_send_buffer(buf, &msg); HAL_UART_Transmit(&huart2, buf, len, 100);
9.2 自主飞行功能
-
定点悬停:
- GPS+气压计融合
- 位置PID控制
-
航点飞行:
- 路径规划算法
- 航点存储与读取
-
视觉辅助:
- 光流传感器集成
- 简单视觉识别
10. 常见问题解决方案
10.1 姿态解算发散
可能原因及解决:
-
传感器安装不水平:
- 重新校准加速度计
- 物理调整IMU安装位置
-
电磁干扰:
- 增加磁屏蔽
- 远离电源线布线
-
参数不当:
- 调整互补滤波系数
- 降低陀螺仪权重
10.2 电机响应不一致
处理方法:
-
单独测试每个电机:
- 检查PWM信号一致性
- 测试推力曲线
-
电调校准:
- 统一校准所有电调
- 设置相同的油门行程
-
动态补偿:
- 在混控器中添加电机补偿系数
- 根据飞行数据在线调整
10.3 飞行中振荡问题
解决方案:
-
降低PID增益:
- 先降低D项,再降P项
- 小步长调整(5%-10%)
-
机械检查:
- 检查桨叶平衡
- 加固机架结构
-
传感器滤波:
- 增加低通滤波
- 提高传感器采样率
