PID控制四轴飞行器系统设计（单片机主控）

hoiii1872026-03-26 8:27

基于PID控制的四轴飞行器系统设计，使用单片机作为主控芯片，包含姿态解算、PID控制算法和电机驱动等核心功能。

系统总体设计

硬件架构

c 复制代码 
+---------------------+
|     遥控器接收机      | <-- PWM输入
+---------------------+
          |
          v
+---------------------+
|      单片机主控       | <-- STM32F103C8T6
| - 姿态解算          |
| - PID控制器         |
| - PWM输出           |
+---------------------+
          |
          v
+---------------------+
|      电机驱动电路      | <-- MOSFET/ESC
+---------------------+
          |
          v
+---------------------+
|      四旋翼电机       |
| - 电机1 (前左)       |
| - 电机2 (前右)       |
| - 电机3 (后左)       |
| - 电机4 (后右)       |
+---------------------+
          |
          v
+---------------------+
|      传感器模块       |
| - MPU6050 (IMU)      |
| - BMP280 (气压计)    |
| - HC-SR04 (超声波)   |
+---------------------+

软件架构

c 复制代码 
+-----------------------+
|      主控制循环        |
+-----------------------+
           |
           v
+-----------------------+
|      传感器数据采集     |
| - IMU数据读取          |
| - 气压计数据读取       |
| - 超声波数据读取       |
+-----------------------+
           |
           v
+-----------------------+
|      姿态解算模块       |
| - 加速度计数据处理     |
| - 陀螺仪数据处理       |
| - 磁力计数据处理       |
| - 卡尔曼滤波/互补滤波  |
+-----------------------+
           |
           v
+-----------------------+
|      PID控制器模块      |
| - Roll PID控制器       |
| - Pitch PID控制器      |
| - Yaw PID控制器        |
| - Altitude PID控制器   |
+-----------------------+
           |
           v
+-----------------------+
|      电机控制模块       |
| - PWM信号生成          |
| - 电机混控算法          |
+-----------------------+
           |
           v
+-----------------------+
|      通信接口模块       |
| - 遥控器信号解码       |
| - 调试信息输出         |
+-----------------------+

核心代码实现

主控制器代码 (STM32 HAL库)

c 复制代码 
#include "stm32f1xx_hal.h"
#include "mpu6050.h"
#include "pid_controller.h"
#include "motor_control.h"
#include "remote_control.h"

// 系统时钟配置
void SystemClock_Config(void);

// 全局变量
I2C_HandleTypeDef hi2c1;
UART_HandleTypeDef huart1;
TIM_HandleTypeDef htim2; // PWM输出

// 传感器数据结构
typedef struct {
    float accel[3];    // 加速度计数据 (m/s²)
    float gyro[3];     // 陀螺仪数据 (rad/s)
    float mag[3];      // 磁力计数据 (µT)
    float pressure;    // 气压 (Pa)
    float temperature; // 温度 (°C)
    float altitude;    // 高度 (m)
} SensorData;

// 姿态数据结构
typedef struct {
    float roll;        // 横滚角 (rad)
    float pitch;       // 俯仰角 (rad)
    float yaw;         // 偏航角 (rad)
    float roll_rate;   // 横滚角速度 (rad/s)
    float pitch_rate;  // 俯仰角速度 (rad/s)
    float yaw_rate;    // 偏航角速度 (rad/s)
} Attitude;

// PID控制器实例
PIDController roll_pid, pitch_pid, yaw_pid, alt_pid;

// 电机控制实例
MotorControl motors;

// 遥控器输入
RemoteControl rc_input;

// 系统状态
typedef enum {
    SYSTEM_IDLE,
    SYSTEM_CALIBRATING,
    SYSTEM_ARMED,
    SYSTEM_FLIGHT,
    SYSTEM_EMERGENCY_STOP
} SystemState;

SystemState system_state = SYSTEM_IDLE;

int main(void) {
    // HAL库初始化
    HAL_Init();
    SystemClock_Config();
    
    // 外设初始化
    MX_GPIO_Init();
    MX_I2C1_Init();
    MX_USART1_UART_Init();
    MX_TIM2_Init();
    
    // PWM初始化
    HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1); // 电机1
    HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_2); // 电机2
    HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3); // 电机3
    HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4); // 电机4
    
    // 初始化MPU6050
    MPU6050_Init(&hi2c1);
    
    // 初始化PID控制器
    PID_Init(&roll_pid, 0.8, 0.1, 0.2, 0.1); // Kp, Ki, Kd, 积分限幅
    PID_Init(&pitch_pid, 0.8, 0.1, 0.2, 0.1);
    PID_Init(&yaw_pid, 1.0, 0.05, 0.1, 0.1);
    PID_Init(&alt_pid, 0.5, 0.01, 0.3, 0.2);
    
    // 初始化电机控制
    MotorControl_Init(&motors, &htim2);
    
    // 初始化遥控器
    RemoteControl_Init();
    
    // 系统自检
    SystemSelfTest();
    
    // 主循环
    uint32_t last_time = HAL_GetTick();
    while (1) {
        uint32_t current_time = HAL_GetTick();
        float dt = (current_time - last_time) / 1000.0f; // 转换为秒
        last_time = current_time;
        
        // 读取传感器数据
        SensorData sensor_data;
        MPU6050_ReadAccel(&sensor_data.accel[0], &sensor_data.accel[1], &sensor_data.accel[2]);
        MPU6050_ReadGyro(&sensor_data.gyro[0], &sensor_data.gyro[1], &sensor_data.gyro[2]);
        
        // 姿态解算
        Attitude attitude;
        AttitudeEstimation(&sensor_data, &attitude, dt);
        
        // 读取遥控器输入
        RemoteControl_Update(&rc_input);
        
        // 系统状态机
        switch (system_state) {
            case SYSTEM_IDLE:
                // 等待解锁指令
                if (rc_input.throttle < 10 && rc_input.yaw > 1800) {
                    system_state = SYSTEM_ARMED;
                    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_SET); // LED指示解锁
                }
                break;
                
            case SYSTEM_ARMED:
                // 等待油门最低且偏航回中
                if (rc_input.throttle < 10 && rc_input.yaw > 1400 && rc_input.yaw < 1600) {
                    system_state = SYSTEM_FLIGHT;
                    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_RESET); // LED指示飞行中
                }
                break;
                
            case SYSTEM_FLIGHT:
                // 检查紧急停止条件
                if (rc_input.throttle < 10 && rc_input.yaw < 1000) {
                    system_state = SYSTEM_EMERGENCY_STOP;
                    EmergencyStop();
                    break;
                }
                
                // PID控制计算
                float roll_error = radians(rc_input.roll - 1500) - attitude.roll;
                float pitch_error = radians(rc_input.pitch - 1500) - attitude.pitch;
                float yaw_error = radians(rc_input.yaw - 1500) - attitude.yaw;
                float alt_error = rc_input.throttle - attitude.altitude;
                
                float roll_output = PID_Compute(&roll_pid, roll_error, attitude.roll_rate, dt);
                float pitch_output = PID_Compute(&pitch_pid, pitch_error, attitude.pitch_rate, dt);
                float yaw_output = PID_Compute(&yaw_pid, yaw_error, attitude.yaw_rate, dt);
                float alt_output = PID_Compute(&alt_pid, alt_error, 0, dt); // 高度控制简化
                
                // 电机混控
                MotorControl_SetOutputs(&motors,
                    rc_input.throttle + roll_output + pitch_output + yaw_output,
                    rc_input.throttle - roll_output + pitch_output - yaw_output,
                    rc_input.throttle + roll_output - pitch_output - yaw_output,
                    rc_input.throttle - roll_output - pitch_output + yaw_output
                );
                break;
                
            case SYSTEM_EMERGENCY_STOP:
                // 紧急停止所有电机
                MotorControl_EmergencyStop(&motors);
                // 等待重启
                if (rc_input.throttle < 10 && rc_input.yaw > 1800) {
                    system_state = SYSTEM_IDLE;
                }
                break;
                
            case SYSTEM_CALIBRATING:
                // 传感器校准过程
                CalibrateSensors();
                system_state = SYSTEM_IDLE;
                break;
        }
        
        // 调试输出
        DebugOutput(&huart1, &attitude, &motors);
        
        // 控制循环频率 (500Hz)
        HAL_Delay(2);
    }
}

// 姿态解算函数 (互补滤波)
void AttitudeEstimation(SensorData* data, Attitude* att, float dt) {
    // 加速度计计算角度 (弧度)
    float roll_acc = atan2(data->accel[1], data->accel[2]);
    float pitch_acc = atan2(-data->accel[0], sqrt(data->accel[1]*data->accel[1] + data->accel[2]*data->accel[2]));
    
    // 陀螺仪积分
    att->roll += data->gyro[0] * dt;
    att->pitch += data->gyro[1] * dt;
    att->yaw += data->gyro[2] * dt;
    
    // 互补滤波融合
    const float alpha = 0.98; // 滤波系数
    att->roll = alpha * att->roll + (1 - alpha) * roll_acc;
    att->pitch = alpha * att->pitch + (1 - alpha) * pitch_acc;
    
    // 保存角速度
    att->roll_rate = data->gyro[0];
    att->pitch_rate = data->gyro[1];
    att->yaw_rate = data->gyro[2];
    
    // 高度估算 (简化版)
    static float ref_pressure = 101325.0f; // 海平面气压
    att->altitude = (ref_pressure - data->pressure) / 12.0f; // 近似高度
}

// 紧急停止函数
void EmergencyStop(void) {
    MotorControl_EmergencyStop(&motors);
    HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_SET); // LED闪烁
    for (int i = 0; i < 10; i++) {
        HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_12);
        HAL_Delay(100);
    }
}

// 系统自检
void SystemSelfTest(void) {
    // 检查传感器连接
    if (!MPU6050_IsConnected(&hi2c1)) {
        ErrorHandler("MPU6050 not connected!");
    }
    
    // 检查遥控器信号
    if (!RemoteControl_IsSignalValid(&rc_input)) {
        ErrorHandler("RC signal lost!");
    }
    
    // 电机测试 (短时间转动)
    MotorControl_SetOutputs(&motors, 200, 200, 200, 200);
    HAL_Delay(500);
    MotorControl_SetOutputs(&motors, 0, 0, 0, 0);
}

// 错误处理
void ErrorHandler(char* message) {
    // 闪烁LED并显示错误信息
    while (1) {
        HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_12);
        HAL_UART_Transmit(&huart1, (uint8_t*)message, strlen(message), HAL_MAX_DELAY);
        HAL_Delay(1000);
    }
}

PID控制器实现 (pid_controller.c)

c 复制代码 
#include "pid_controller.h"
#include <math.h>

// PID初始化
void PID_Init(PIDController* pid, float Kp, float Ki, float Kd, float integral_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 = 1000.0f; // 默认输出限幅
}

// PID计算
float PID_Compute(PIDController* pid, float error, float derivative, float dt) {
    // 比例项
    float proportional = pid->Kp * error;
    
    // 积分项 (带抗饱和)
    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;
    }
    float integral = pid->Ki * pid->integral;
    
    // 微分项
    float derivative_term = pid->Kd * derivative;
    
    // 计算输出
    float output = proportional + integral + derivative_term;
    
    // 输出限幅
    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;
}

// 设置输出限幅
void PID_SetOutputLimit(PIDController* pid, float limit) {
    pid->output_limit = limit;
}

// 重置积分项
void PID_ResetIntegral(PIDController* pid) {
    pid->integral = 0.0f;
}

电机控制实现 (motor_control.c)

c 复制代码 
#include "motor_control.h"
#include "main.h"

// 电机控制初始化
void MotorControl_Init(MotorControl* mc, TIM_HandleTypeDef* htim) {
    mc->htim = htim;
    mc->outputs[0] = 0;
    mc->outputs[1] = 0;
    mc->outputs[2] = 0;
    mc->outputs[3] = 0;
    
    // 设置PWM频率为400Hz (2.5ms周期)
    mc->htim->Instance->ARR = 1999; // 20ms / 10kHz = 2000 ticks
    mc->htim->Instance->PSC = 71;   // 72MHz / (71+1) = 1MHz
}

// 设置电机输出
void MotorControl_SetOutputs(MotorControl* mc, float m1, float m2, float m3, float m4) {
    // 限幅处理
    mc->outputs[0] = constrain(m1, 0, 1000);
    mc->outputs[1] = constrain(m2, 0, 1000);
    mc->outputs[2] = constrain(m3, 0, 1000);
    mc->outputs[3] = constrain(m4, 0, 1000);
    
    // 转换为PWM占空比 (0-1000 对应 0-2000 ticks)
    __HAL_TIM_SET_COMPARE(mc->htim, TIM_CHANNEL_1, (uint16_t)mc->outputs[0]);
    __HAL_TIM_SET_COMPARE(mc->htim, TIM_CHANNEL_2, (uint16_t)mc->outputs[1]);
    __HAL_TIM_SET_COMPARE(mc->htim, TIM_CHANNEL_3, (uint16_t)mc->outputs[2]);
    __HAL_TIM_SET_COMPARE(mc->htim, TIM_CHANNEL_4, (uint16_t)mc->outputs[3]);
}

// 紧急停止
void MotorControl_EmergencyStop(MotorControl* mc) {
    __HAL_TIM_SET_COMPARE(mc->htim, TIM_CHANNEL_1, 0);
    __HAL_TIM_SET_COMPARE(mc->htim, TIM_CHANNEL_2, 0);
    __HAL_TIM_SET_COMPARE(mc->htim, TIM_CHANNEL_3, 0);
    __HAL_TIM_SET_COMPARE(mc->htim, TIM_CHANNEL_4, 0);
    
    mc->outputs[0] = 0;
    mc->outputs[1] = 0;
    mc->outputs[2] = 0;
    mc->outputs[3] = 0;
}

// 辅助函数：限幅
float constrain(float value, float min, float max) {
    if (value < min) return min;
    if (value > max) return max;
    return value;
}

遥控器信号处理 (remote_control.c)

c 复制代码 
#include "remote_control.h"
#include "main.h"

// 遥控器初始化
void RemoteControl_Init(void) {
    // 初始化PWM输入捕获
    HAL_TIM_IC_Start_IT(&htim3, TIM_CHANNEL_1); // 通道1: 油门
    HAL_TIM_IC_Start_IT(&htim3, TIM_CHANNEL_2); // 通道2: 横滚
    HAL_TIM_IC_Start_IT(&htim3, TIM_CHANNEL_3); // 通道3: 俯仰
    HAL_TIM_IC_Start_IT(&htim3, TIM_CHANNEL_4); // 通道4: 偏航
}

// 更新遥控器输入
void RemoteControl_Update(RemoteControl* rc) {
    // 从输入捕获寄存器读取脉冲宽度 (单位: us)
    rc->throttle = HAL_TIM_ReadCapturedValue(&htim3, TIM_CHANNEL_1);
    rc->roll = HAL_TIM_ReadCapturedValue(&htim3, TIM_CHANNEL_2);
    rc->pitch = HAL_TIM_ReadCapturedValue(&htim3, TIM_CHANNEL_3);
    rc->yaw = HAL_TIM_ReadCapturedValue(&htim3, TIM_CHANNEL_4);
    
    // 检查信号有效性
    rc->signal_valid = (rc->throttle > 900 && rc->throttle < 2100 &&
                       rc->roll > 900 && rc->roll < 2100 &&
                       rc->pitch > 900 && rc->pitch < 2100 &&
                       rc->yaw > 900 && rc->yaw < 2100);
}

// 检查信号是否有效
bool RemoteControl_IsSignalValid(RemoteControl* rc) {
    return rc->signal_valid;
}

PID参数整定方法

1. 手动整定步骤

  1. 将Ki和Kd设为0，逐渐增大Kp直到系统开始振荡
  2. 将Kp减小到振荡时的50%
  3. 逐渐增加Ki直到消除稳态误差
  4. 适当增加Kd以抑制超调和振荡

2. Ziegler-Nichols方法

  1. 设置Ki=0, Kd=0
  2. 增加Kp直到系统产生持续振荡
  3. 记录临界增益Ku和振荡周期Tu
  4. 根据下表设置参数：
控制器类型 Kp Ki Kd
P 0.5*Ku - -
PI 0.45*Ku 0.54*Ku/Tu -
PID 0.6*Ku 1.2*Ku/Tu 0.075KuTu

3. 四轴飞行器典型PID参数范围

  • Roll/Pitch控制 :
    • Kp: 0.5-2.0
    • Ki: 0.01-0.1
    • Kd: 0.1-0.5
  • Yaw控制 :
    • Kp: 1.0-3.0
    • Ki: 0.05-0.2
    • Kd: 0.1-0.3
  • 高度控制 :
    • Kp: 0.3-1.0
    • Ki: 0.01-0.05
    • Kd: 0.2-0.8

参考代码 PID-四轴飞行类-单片机主控的四旋翼飞行器 www.youwenfan.com/contentcss/122644.html

系统优化技术

1. 传感器数据融合

c 复制代码 
// 使用Mahony滤波进行姿态解算
void MahonyAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float dt) {
    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;
    
    // 归一化加速度计测量值
    recipNorm = invSqrt(ax * ax + ay * ay + az * az);
    ax *= recipNorm;
    ay *= recipNorm;
    az *= recipNorm;
    
    // 归一化磁力计测量值
    recipNorm = invSqrt(mx * mx + my * my + mz * mz);
    mx *= recipNorm;
    my *= recipNorm;
    mz *= recipNorm;
    
    // 辅助变量计算
    q0q0 = q0 * q0;
    q0q1 = q0 * q1;
    q0q2 = q0 * q2;
    q0q3 = q0 * q3;
    q1q1 = q1 * q1;
    q1q2 = q1 * q2;
    q1q3 = q1 * q3;
    q2q2 = q2 * q2;
    q2q3 = q2 * q3;
    q3q3 = q3 * 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 = sqrt(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 (twoKi > 0.0f) {
        integralFBx += twoKi * halfex * dt;
        integralFBy += twoKi * halfey * dt;
        integralFBz += twoKi * halfez * dt;
        
        gx += integralFBx;
        gy += integralFBy;
        gz += integralFBz;
    } else {
        integralFBx = 0.0f;
        integralFBy = 0.0f;
        integralFBz = 0.0f;
    }
    
    // 应用比例反馈
    gx += twoKp * halfex;
    gy += twoKp * halfey;
    gz += twoKp * halfez;
    
    // 积分四元数导数
    gx *= (0.5f * dt);
    gy *= (0.5f * dt);
    gz *= (0.5f * dt);
    
    qa = q0;
    qb = q1;
    qc = q2;
    q0 += (-qb * gx - qc * gy - q3 * gz);
    q1 += (qa * gx + qc * gz - q3 * gy);
    q2 += (qa * gy - qb * gz + q3 * gx);
    q3 += (qa * gz + qb * gy - qc * gx);
    
    // 归一化四元数
    recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 *= recipNorm;
    q1 *= recipNorm;
    q2 *= recipNorm;
    q3 *= recipNorm;
    
    // 转换为欧拉角
    attitude.roll = atan2(2*(q0*q1 + q2*q3), 1 - 2*(q1*q1 + q2*q2));
    attitude.pitch = asin(2*(q0*q2 - q3*q1));
    attitude.yaw = atan2(2*(q0*q3 + q1*q2), 1 - 2*(q2*q2 + q3*q3));
}

2. 抗风扰控制

c 复制代码 
// 风扰观测器
void WindDisturbanceObserver(Attitude* att, float* wind_estimate) {
    static float integral[3] = {0};
    const float L = 0.1f; // 观测器增益
    
    for (int i = 0; i < 3; i++) {
        // 计算角加速度
        float angular_accel = (att->gyro[i] - att->gyro_prev[i]) / dt;
        
        // 风扰估计
        wind_estimate[i] = L * (angular_accel - (att->gyro[i] - att->gyro_desired[i]));
        
        // 积分项
        integral[i] += wind_estimate[i] * dt;
        
        // 限幅
        if (integral[i] > 10.0f) integral[i] = 10.0f;
        if (integral[i] < -10.0f) integral[i] = -10.0f;
        
        wind_estimate[i] += integral[i];
        
        // 保存当前值
        att->gyro_prev[i] = att->gyro[i];
    }
}

// 在PID计算中使用风扰补偿
float PID_Compute_WithWindCompensation(PIDController* pid, float error, float derivative, float dt, float wind_comp) {
    // 标准PID计算
    float output = PID_Compute(pid, error, derivative, dt);
    
    // 风扰补偿
    output += wind_comp;
    
    return output;
}

3. 低电量保护

c 复制代码 
// 电池电压监测
#define VOLTAGE_DIVIDER_R1 10000.0f
#define VOLTAGE_DIVIDER_R2 4700.0f
#define ADC_REF_VOLTAGE 3.3f
#define ADC_RESOLUTION 4095.0f

float ReadBatteryVoltage(ADC_HandleTypeDef* hadc) {
    HAL_ADC_Start(hadc);
    HAL_ADC_PollForConversion(hadc, HAL_MAX_DELAY);
    uint16_t adc_value = HAL_ADC_GetValue(hadc);
    float voltage = (adc_value * ADC_REF_VOLTAGE / ADC_RESOLUTION) * 
                   (VOLTAGE_DIVIDER_R1 + VOLTAGE_DIVIDER_R2) / VOLTAGE_DIVIDER_R2;
    return voltage;
}

// 低电量处理函数
void LowBatteryProtection(float voltage) {
    if (voltage < 10.5f) { // 3S锂电池低于10.5V
        // 降低最大输出功率
        for (int i = 0; i < 4; i++) {
            motors.outputs[i] *= 0.7f;
        }
        
        // 发送警告
        char warning[50];
        sprintf(warning, "LOW BATTERY: %.1fV", voltage);
        HAL_UART_Transmit(&huart1, (uint8_t*)warning, strlen(warning), HAL_MAX_DELAY);
    }
    
    if (voltage < 9.5f) { // 严重低电量
        // 紧急降落
        system_state = SYSTEM_EMERGENCY_STOP;
    }
}

安全保护机制

1. 看门狗定时器

c 复制代码 
IWDG_HandleTypeDef hiwdg;

void IWDG_Init(void) {
    hiwdg.Instance = IWDG;
    hiwdg.Init.Prescaler = IWDG_PRESCALER_32; // 32分频 (约1ms计数)
    hiwdg.Init.Reload = 1000; // 1秒超时
    if (HAL_IWDG_Init(&hiwdg) != HAL_OK) {
        Error_Handler();
    }
}

void FeedWatchdog(void) {
    HAL_IWDG_Refresh(&hiwdg);
}

// 在主循环中定期喂狗
while (1) {
    // ...主控制代码...
    FeedWatchdog();
    HAL_Delay(100);
}

2. 失控保护

c 复制代码 
// 失控保护函数
void FailsafeProcedure(void) {
    // 1. 记录最后已知良好状态
    static Attitude last_good_attitude;
    static RemoteControl last_good_rc;
    
    // 2. 尝试恢复控制
    if (system_state == SYSTEM_FLIGHT) {
        // 尝试回到悬停状态
        rc_input.throttle = 1500;
        rc_input.roll = 1500;
        rc_input.pitch = 1500;
        rc_input.yaw = 1500;
        
        // 激活高度保持模式
        alt_pid.enabled = true;
        alt_pid.setpoint = last_good_attitude.altitude;
    }
    
    // 3. 如果仍然失控，执行紧急降落
    if (HAL_GetTick() - last_valid_signal_time > 500) { // 500ms无信号
        EmergencyLanding();
    }
}

void EmergencyLanding(void) {
    // 缓慢降低高度
    float descent_rate = 0.5f; // m/s
    float target_altitude = 0.0f;
    
    while (attitude.altitude > 0.5f) {
        alt_pid.setpoint = attitude.altitude - descent_rate * 0.1f;
        HAL_Delay(100);
    }
    
    // 着陆后关闭电机
    MotorControl_EmergencyStop(&motors);
    system_state = SYSTEM_IDLE;
}

3. 地理围栏

c 复制代码 
typedef struct {
    float min_lat;
    float max_lat;
    float min_lon;
    float max_lon;
    float max_altitude;
} GeoFence;

GeoFence fence = {
    .min_lat = 31.123456,
    .max_lat = 31.234567,
    .min_lon = 121.123456,
    .max_lon = 121.234567,
    .max_altitude = 120.0f // 米
};

void CheckGeoFence(float lat, float lon, float alt) {
    if (lat < fence.min_lat || lat > fence.max_lat ||
        lon < fence.min_lon || lon > fence.max_lon ||
        alt > fence.max_altitude) {
        
        // 触发地理围栏警报
        TriggerGeoFenceAlert();
        
        // 自动返航或降落
        if (system_state == SYSTEM_FLIGHT) {
            ReturnToHome();
        }
    }
}

void ReturnToHome(void) {
    // 获取当前位置和家位置
    GPSData home = GetHomePosition();
    GPSData current = GetCurrentPosition();
    
    // 计算返航路径
    float bearing = CalculateBearing(current.lat, current.lon, home.lat, home.lon);
    float distance = CalculateDistance(current.lat, current.lon, home.lat, home.lon);
    
    // 设置导航模式
    navigation_mode = MODE_RTH;
    nav_target_bearing = bearing;
    nav_target_distance = distance;
    
    // 爬升到安全高度
    alt_pid.setpoint = 50.0f; // 50米安全高度
    
    // 当到达安全高度后飞向家的位置
    // ...
}

部署与飞行测试

1. 硬件组装清单

部件 型号 数量
主控板 STM32F103C8T6 1
IMU传感器 MPU6050 1
气压计 BMP280 1
电子调速器 SimonK 30A 4
无刷电机 2212 920KV 4
螺旋桨 1045 4
机架 F450 1
电池 3S 2200mAh LiPo 1
遥控器 FlySky FS-i6 1
接收机 FS-iA6B 1

2. 飞行测试流程

  1. 地面测试 ：
    • 检查所有连接
    • 测试电机转向
    • 校准遥控器
    • 校准IMU传感器
  2. 悬停测试 （离地1米）：
    • 测试姿态稳定性
    • 微调PID参数
    • 验证失控保护
  3. 基础机动测试 ：
    • 前后左右平移
    • 左右转向
    • 升降练习
  4. 高级机动测试 ：
    • 8字飞行
    • 急停急转
    • 抗风测试
  5. 长距离测试 ：
    • 逐步增加飞行距离
    • 测试图传和控制距离
    • 验证地理围栏功能

3. 常见问题排查

问题现象 可能原因 解决方案
飞行器自旋 电机转向错误 PID参数不平衡 交换任意两个电机连线 调整Roll/Pitch PID参数
飞行不稳定 IMU校准不准 振动过大 重新校准IMU 增加减震措施
无法起飞 电机转向错误 桨叶装反 检查电机转向 确认桨叶安装方向
遥控器失控 信号干扰 电池电量低 更换频段 检查电池电压
飞行时间短 电池老化 电机效率低 更换电池 清洁电机轴承

进阶开发方向

  1. GPS导航与自主飞行

    c 复制代码 
    void GPSNavigationTask(void) {
    // 获取GPS数据
    GPSData current_pos = GPS_GetPosition();

    // 计算路径点
    Waypoint next_wp = Mission_GetNextWaypoint();

    // 计算导航指令
    float bearing = CalculateBearing(current_pos.lat, current_pos.lon, 
                                    next_wp.lat, next_wp.lon);
    float distance = CalculateDistance(current_pos.lat, current_pos.lon, 
                                      next_wp.lat, next_wp.lon);

    // 转换为控制指令
    rc_input.roll = 1500 + (bearing - attitude.yaw) * 500 / M_PI;
    rc_input.throttle = 1500 + (next_wp.altitude - attitude.altitude) * 10;

    // 到达判断
    if (distance < 5.0f) { // 5米范围内认为到达
        Mission_CompleteWaypoint();
    }
}

  2. 视觉避障系统

    c 复制代码 
    void ObstacleAvoidance(void) {
    // 获取深度相机数据
    DepthCameraData depth_data = DepthCamera_GetData();

    // 检测前方障碍物
    float min_distance = 1000.0f;
    for (int i = 0; i < DEPTH_WIDTH; i++) {
        for (int j = DEPTH_HEIGHT/3; j < 2*DEPTH_HEIGHT/3; j++) {
            float dist = depth_data.depth_map[j*DEPTH_WIDTH+i];
            if (dist > 0 && dist < min_distance) {
                min_distance = dist;
            }
        }
    }

    // 避障决策
    if (min_distance < OBSTACLE_THRESHOLD) {
        // 计算避障方向
        float avoid_direction = CalculateAvoidDirection(depth_data);

        // 叠加到控制指令
        rc_input.roll += avoid_direction * 200;
        rc_input.throttle += 100; // 轻微上升
    }
}

  3. 集群协同飞行

    c 复制代码 
    void SwarmCoordination(void) {
    // 获取邻居无人机位置
    UAVInfo neighbors[MAX_NEIGHBORS];
    int neighbor_count = Communication_GetNeighbors(neighbors);

    // 计算群体中心
    Position swarm_center = CalculateSwarmCenter(neighbors, neighbor_count);

    // 计算排斥力
    Vector3 repulsion_force = CalculateRepulsionForce(neighbors, neighbor_count);

    // 计算队形偏移
    Position formation_offset = Formation_GetOffset(my_id);

    // 合成目标位置
    Position target_position = swarm_center + formation_offset + repulsion_force;

    // 转换为控制指令
    NavigateTo(target_position);
}

  4. 机器学习控制器

    c 复制代码 
    # 使用TensorFlow Lite部署神经网络控制器
import tensorflow as tf

# 加载训练好的模型
interpreter = tf.lite.Interpreter(model_path="quadcopter_controller.tflite")
interpreter.allocate_tensors()

# 获取输入输出索引
input_details = interpreter.get_input_details()
output_details = interpreter.get_output_details()

def ml_controller(attitude, rc_input):
    # 准备输入数据
    input_data = np.array([[attitude.roll, attitude.pitch, attitude.yaw,
                           rc_input.roll, rc_input.pitch, rc_input.yaw]],
                          dtype=np.float32)

    # 设置输入张量
    interpreter.set_tensor(input_details[0]['index'], input_data)

    # 推理
    interpreter.invoke()

    # 获取输出
    output_data = interpreter.get_tensor(output_details[0]['index'])

    # 解析输出 [m1, m2, m3, m4]
    return output_data[0]

这个完整的四轴飞行器系统设计涵盖了从硬件选型到软件实现的各个方面，包含了PID控制算法、姿态解算、传感器融合和安全保护机制等关键技术。

相关推荐
恶魔泡泡糖3 小时前
stm32核心板子使用验证与串口下载
stm32·单片机·嵌入式硬件
wsoz3 小时前
串口仿真协议(RFCOMM)
单片机·嵌入式·蓝牙协议栈·rfcomm
会编程的小孩3 小时前
指针应用 单片机
单片机·嵌入式硬件
HalvmånEver3 小时前
Linux：基于 UDP Socket 的实战项目 --简单双向通信程序
linux·单片机·udp
FreakStudio12 小时前
不用装软件！这款MicroPython浏览器 IDE :让你在手机上也能调试树莓派 Pico
python·单片机·嵌入式·电子diy·tinyml
yuan1999715 小时前
STM32远程升级系统（Bootloader + 上位机）
stm32·单片机·嵌入式硬件
Heartache boy15 小时前
野火STM32_HAL库版课程笔记-ADC多通道采集热敏、光敏、反射传感器（轮询）
笔记·stm32·单片机
AI+程序员在路上16 小时前
嵌入式软件技术大全
linux·开发语言·arm开发·单片机
秀秀更健康18 小时前
STM32的程序下载不进去----VDDA悬空
stm32·单片机·嵌入式硬件
热门推荐
01GitHub 镜像站点02Qwen3.5 开源全解析：从 0.8B 到 397B，代际升级 + 全场景选型指南03围棋-html版本04班级宠物园部署指南05小黑课堂计算机二级WPSoffice题库软件下载安装教程（2026年3月最新版）06UV安装并设置国内源07OpenClaw 使用和管理 MCP 完全指南08“wsl --install -d Ubuntu-22.04”下载慢，中国地区离线安装 Ubuntu 22.04 WSL方法（亲测2025年5月6日）09机器学习深度学习二——GAN网络10OpenClaw Control UI安全上下文访问配置