1. 四轴飞行器飞控系统概述
四轴飞行器的飞行控制系统(Flight Controller,简称飞控)是整个飞行器的核心大脑。想象一下,如果让你同时控制四个电机的转速来保持飞行器稳定悬停,这几乎是不可能完成的任务。飞控正是为了解决这个难题而存在的,它需要以每秒数百次的速度读取传感器数据、计算姿态、调整电机输出,让飞行器能够稳定飞行。
1.1 飞控的核心功能
飞控的主要工作可以概括为:让飞行器从当前姿态转变为期望姿态,并保持稳定。这个过程中涉及四个关键模块:
-
传感器模块:相当于飞行器的感官系统,包括:
- 加速度计:测量三个轴向的加速度(包括重力)
- 陀螺仪:测量三个轴向的角速度
- 磁力计(可选):测量地磁场方向
- 气压计(可选):测量高度变化
-
姿态解算模块:将原始传感器数据转换为可理解的姿态角度。这里面临的主要挑战是:
- 加速度计短期噪声大但长期稳定
- 陀螺仪短期精确但存在漂移
- 需要融合两者优势获得准确姿态
-
控制算法模块:通常采用PID控制器,根据期望姿态与实际姿态的差异计算出修正量。现代飞控多采用级联PID结构:
- 外环(角度环):确保姿态准确性
- 内环(角速度环):快速响应扰动
-
电机输出模块:将控制量分配到四个电机,需要考虑:
- 电机布局(X型或+型)
- 电机转向配置
- 混控算法
1.2 实时性要求
飞控对实时性的要求极高,通常控制周期为1-5ms(200-1000Hz)。这意味着:
- 所有传感器数据读取必须在1ms内完成
- 姿态解算和控制算法计算必须在1ms内完成
- 电机PWM输出更新必须在1ms内完成
在实际实现中,这通常通过定时器中断来实现。例如STM32系列MCU可以使用定时器触发中断,在中断服务例程中完成整个控制流程。
注意:中断服务函数应该尽可能简短,避免复杂计算。可以将耗时操作(如复杂的滤波计算)放在主循环中,通过标志位与中断通信。
2. 坐标系与姿态表示
2.1 机体坐标系定义
在四轴飞行器控制中,我们使用右手坐标系:
- X轴:指向飞行器前方(通常与某个电机方向一致)
- Y轴:指向飞行器右侧
- Z轴:指向飞行器下方
这种定义下,正旋转方向遵循右手定则:大拇指指向轴的正方向,四指弯曲方向为正旋转方向。
2.2 欧拉角表示
飞行器的姿态通常用三个欧拉角描述:
-
俯仰角(Pitch):绕Y轴旋转
- 正值:机头上仰
- 负值:机头下俯
- 典型范围:-90° ~ +90°
-
横滚角(Roll):绕X轴旋转
- 正值:右侧下沉
- 负值:左侧下沉
- 典型范围:-180° ~ +180°
-
偏航角(Yaw):绕Z轴旋转
- 正值:机头右偏
- 负值:机头左偏
- 典型范围:-180° ~ +180°
2.3 四元数表示
欧拉角虽然直观,但存在万向节死锁问题。在实际飞控中,我们使用四元数进行内部计算。四元数由四个分量组成:q = [q0, q1, q2, q3],其中:
- q0:实部,表示旋转的cos(θ/2)
- q1,q2,q3:虚部,表示旋转轴的sin(θ/2)
四元数的优势:
- 避免万向节死锁
- 计算效率高(只需四则运算)
- 插值平滑(适合姿态估计)
3. 传感器数据处理
3.1 IMU传感器特性
现代飞控通常使用6轴IMU(3轴加速度计+3轴陀螺仪),部分高端飞控还会加入3轴磁力计构成9轴IMU。
加速度计特性:
- 测量范围:±2g ~ ±16g(典型±8g)
- 输出噪声:0.1mg/√Hz ~ 1mg/√Hz
- 带宽:通常50-400Hz
- 零偏稳定性:0.5mg ~ 2mg
陀螺仪特性:
- 测量范围:±250dps ~ ±2000dps(典型±1000dps)
- 输出噪声:0.005dps/√Hz ~ 0.05dps/√Hz
- 带宽:通常50-400Hz
- 零偏稳定性:5°/h ~ 50°/h
3.2 传感器校准
传感器校准对飞控性能至关重要,主要包括:
-
加速度计校准:
- 六面法校准:将飞行器分别朝六个面静止放置
- 记录各位置输出,计算零偏和比例因子
- 公式:A_calib = Scale * (A_raw - Offset)
-
陀螺仪校准:
- 静止状态下采集数据(通常1000个点)
- 计算平均值作为零偏
- 公式:ω_calib = ω_raw - Offset
-
磁力计校准(如有):
- 需要三维空间旋转设备
- 使用椭圆拟合算法校准
- 补偿硬铁和软铁误差
3.3 传感器数据滤波
原始传感器数据需要适当滤波:
-
加速度计滤波:
- 低通滤波:截止频率5-20Hz
- 中值滤波:去除脉冲噪声
- 示例代码:
c复制#define ALPHA 0.2f // 滤波系数 filtered_accel = ALPHA * raw_accel + (1-ALPHA) * filtered_accel;
-
陀螺仪滤波:
- 通常不滤波(保持高频响应)
- 必要时使用轻微低通(截止频率50-100Hz)
注意:滤波会引入相位延迟,需要权衡噪声抑制和响应速度。
4. 姿态解算算法
4.1 Mahony算法原理
Mahony算法是一种基于互补滤波的姿态解算算法,其核心思想是:
- 使用陀螺仪积分获得短期精确的姿态变化
- 使用加速度计校正陀螺仪的长期漂移
- 通过PI控制器融合两者优势
算法流程:
- 归一化加速度计测量值
- 根据当前四元数估计重力方向
- 计算测量重力与估计重力的误差(叉乘)
- 用误差修正陀螺仪读数(PI控制)
- 使用修正后的角速度更新四元数
- 四元数归一化
4.2 Mahony算法实现
以下是精简版的Mahony算法实现:
c复制void MahonyAHRSupdateIMU(float gx, float gy, float gz,
float ax, float ay, float az,
float dt)
{
// 1. 归一化加速度计
float norm = sqrt(ax*ax + ay*ay + az*az);
if (norm == 0.0f) return;
ax /= norm; ay /= norm; az /= norm;
// 2. 估计重力方向
float halfvx = q1*q3 - q0*q2;
float halfvy = q0*q1 + q2*q3;
float halfvz = q0*q0 - 0.5f + q3*q3;
// 3. 计算误差(叉乘)
float halfex = ay*halfvz - az*halfvy;
float halfey = az*halfvx - ax*halfvz;
float halfez = ax*halfvy - ay*halfvx;
// 4. 误差积分(I项)
integralFBx += twoKi * halfex * dt;
integralFBy += twoKi * halfey * dt;
integralFBz += twoKi * halfez * dt;
// 5. 修正陀螺仪(P项 + I项)
gx += twoKp * halfex + integralFBx;
gy += twoKp * halfey + integralFBy;
gz += twoKp * halfez + integralFBz;
// 6. 四元数积分
gx *= 0.5f * dt; gy *= 0.5f * dt; gz *= 0.5f * dt;
float 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;
// 7. 四元数归一化
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 /= norm; q1 /= norm; q2 /= norm; q3 /= norm;
}
4.3 参数调节
Mahony算法有两个关键参数:
-
Kp(比例增益):
- 典型值:0.5-5.0
- 太小:收敛慢,姿态响应迟钝
- 太大:对加速度计噪声敏感,可能震荡
-
Ki(积分增益):
- 典型值:0-0.1
- 用于消除陀螺仪零偏
- 太大可能导致低频震荡
调节建议:
- 初始设置Ki=0,只调节Kp
- 找到合适的Kp后,再调节Ki消除稳态误差
- 飞行测试时观察姿态稳定性
5. PID控制算法实现
5.1 PID控制器结构
飞控中通常使用级联PID控制:
-
外环(角度环):
- 输入:期望角度与实际角度误差
- 输出:期望角速度
- 特点:响应较慢,确保稳态精度
-
内环(角速度环):
- 输入:期望角速度与实际角速度误差
- 输出:电机控制量
- 特点:响应快,抑制扰动
5.2 离散PID实现
以下是实用的PID控制器实现:
c复制typedef struct {
float kp, ki, kd; // PID参数
float integral; // 积分项
float last_error; // 上次误差
float output; // 输出
float integral_limit; // 积分限幅
float output_limit; // 输出限幅
} PID_Controller;
void PID_Update(PID_Controller* pid, float error, float dt)
{
// P项
float p_term = pid->kp * error;
// I项
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 i_term = pid->ki * pid->integral;
// D项
float derivative = (error - pid->last_error) / dt;
float d_term = pid->kd * derivative;
pid->last_error = error;
// 总和
pid->output = p_term + i_term + d_term;
// 输出限幅
if (pid->output > pid->output_limit)
pid->output = pid->output_limit;
else if (pid->output < -pid->output_limit)
pid->output = -pid->output_limit;
}
5.3 参数整定方法
PID参数整定步骤:
-
内环(角速度环)先调:
- 初始设置:P=0.3, I=0, D=0
- 测试方法:手动扰动飞行器,观察恢复特性
- 目标:快速响应,1-2次振荡后稳定
-
外环(角度环)后调:
- 初始设置:P=3.0, I=0, D=0
- 测试方法:给阶跃输入,观察响应
- 目标:无明显超调,响应时间适中
-
典型参数范围:
- 角速度环:P=0.1-1.0, I=0-0.1, D=0-10
- 角度环:P=1.0-10.0, I=0-0.5, D=0-5
调参技巧:每次只调整一个参数,小幅度变化(±20%),记录每次修改的效果。
6. 电机混控与输出
6.1 电机布局与转向
典型X型四轴电机布局:
code复制 M2(CCW)
↑
M1(CW) M3(CW)
↓
M4(CCW)
- CW:顺时针旋转
- CCW:逆时针旋转
- 对角电机转向相同,以抵消反扭矩
6.2 混控算法
将PID输出分配到四个电机的基本混控公式:
c复制void MotorMixing(float throttle, float pitch_out, float roll_out, float yaw_out)
{
// 基础油门 + 姿态修正
motor[0] = throttle + pitch_out - roll_out - yaw_out; // M1
motor[1] = throttle + pitch_out + roll_out + yaw_out; // M2
motor[2] = throttle - pitch_out - roll_out + yaw_out; // M3
motor[3] = throttle - pitch_out + roll_out - yaw_out; // M4
// 输出限幅(0-100%)
for(int i=0; i<4; i++) {
if(motor[i] < 0.0f) motor[i] = 0.0f;
if(motor[i] > 1.0f) motor[i] = 1.0f;
}
}
6.3 PWM输出生成
将混控结果转换为实际的PWM信号:
c复制void PWM_Output(float motor_cmd[4])
{
// 假设PWM分辨率为16位(0-65535)
// 最小油门(确保电机启动):约10%
// 最大油门:100%
uint16_t pwm[4];
for(int i=0; i<4; i++) {
pwm[i] = 6553 + (uint16_t)(motor_cmd[i] * 58982);
TIM_SetCompareX(TIMx, pwm[i]); // 具体寄存器取决于硬件
}
}
注意:实际应用中需要考虑电机死区、非线性补偿等问题。
7. 飞行模式与状态机
7.1 常见飞行模式
-
手动模式(Manual):
- 完全由遥控器控制
- 飞控仅提供基本姿态稳定
- 适合有经验的飞手
-
自稳模式(Stabilize):
- 松开摇杆自动回平
- 保持水平姿态
- 适合初学者
-
定高模式(Altitude Hold):
- 自动保持高度
- 用户控制水平移动
- 需要高度传感器(气压计/TOF)
-
定点模式(Position Hold):
- 自动保持位置
- 需要位置传感器(光流/GPS)
- 对新手最友好
7.2 状态机设计
飞控状态机典型状态:
mermaid复制stateDiagram-v2
[*] --> IDLE
IDLE --> ARMING: 解锁指令
ARMING --> TAKEOFF: 油门超过阈值
TAKEOFF --> STABILIZE: 达到安全高度
STABILIZE --> ALT_HOLD: 模式切换
ALT_HOLD --> POS_HOLD: 模式切换
POS_HOLD --> RETURN: 触发返航
RETURN --> LAND: 到达返航点
LAND --> IDLE: 着陆完成
any --> FAILSAFE: 异常情况
状态转换条件示例:
- 解锁:特定摇杆组合 + 安全检查通过
- 起飞:油门超过55% + 姿态稳定
- 模式切换:遥控器开关位置变化
- 返航:遥控器指令或信号丢失
8. 高度与位置控制
8.1 高度控制实现
高度控制通常采用级联PID:
-
外环(高度环):
- 输入:期望高度与实际高度差
- 输出:期望垂直速度
-
内环(速度环):
- 输入:期望垂直速度与实际垂直速度差
- 输出:油门修正量
垂直速度获取方法:
- 高度微分:
velocity = (current_height - last_height) / dt - 加速度积分:
velocity += accel_z * dt - 传感器融合:结合两者优势
8.2 位置控制实现
位置控制需要额外的水平位置传感器:
-
室内定位:
- 光流传感器:检测地面纹理移动
- 超声波/TOF:测量高度辅助
- 需要补偿旋转引起的虚假位移
-
室外定位:
- GPS:提供绝对位置
- 精度通常1-3米(民用)
- RTK GPS可提高到厘米级
位置控制也是级联PID:
- 位置环 → 期望速度
- 速度环 → 期望角度
- 角度环 → 电机输出
9. 调试与参数整定
9.1 调试工具
-
地面站软件:
- Mission Planner
- QGroundControl
- Betaflight Configurator
-
数据记录与分析:
- 记录传感器原始数据
- 记录PID输出
- 绘制时域/频域曲线
-
参数调节接口:
- 无线调参(蓝牙/WiFi)
- USB连接调参
- 遥控器辅助调参
9.2 调参流程
-
硬件检查:
- 电机转向正确
- 螺旋桨安装正确
- 传感器方向正确
-
基本测试:
- 电机响应测试(卸桨)
- 传感器数据验证
- 遥控器通道映射
-
PID调参:
- 先内环后外环
- 先P后I再D
- 小步调整,记录效果
-
飞行测试:
- 低空悬停测试
- 阶跃响应测试
- 抗扰动测试
9.3 常见问题解决
-
起飞翻转:
- 检查电机顺序
- 检查螺旋桨方向
- 检查传感器方向
-
高频振荡:
- 降低角速度环P
- 增加角速度环D
- 检查机械振动
-
漂移问题:
- 校准加速度计
- 调整Mahony参数
- 检查传感器安装
-
响应迟钝:
- 增加角速度环P
- 减小角速度环D
- 检查控制周期
10. 安全注意事项
-
测试安全:
- 始终卸桨进行初步测试
- 使用安全绳进行初步飞行测试
- 在开阔无人的场地飞行
-
故障保护:
- 设置失控保护
- 低电量保护
- 传感器故障检测
-
法律合规:
- 遵守当地无人机法规
- 注意飞行高度限制
- 避开禁飞区域
-
开发建议:
- 使用版本控制系统
- 详细记录参数修改
- 建立检查清单
11. 进阶发展方向
-
传感器融合:
- 卡尔曼滤波
- 互补滤波改进
- 多传感器冗余
-
控制算法:
- 自适应PID
- 模糊控制
- 模型预测控制
-
自主飞行:
- 路径规划
- 避障算法
- 视觉导航
-
集群控制:
- 多机协同
- 编队飞行
- 通信协议
12. 实战经验分享
在实际飞控开发中,有几个特别容易忽视但非常重要的细节:
-
时间一致性:
- 确保所有传感器数据时间对齐
- 使用硬件定时器精确控制周期
- 避免变量时间延迟
-
单位统一:
- 角度统一用弧度或度
- 时间统一用秒
- 明确每个变量的物理单位
-
浮点处理:
- 避免在中断中进行浮点运算(某些MCU)
- 使用快速数学库
- 注意NaN和Inf检查
-
调试接口:
- 预留丰富的调试输出
- 设计可调节的日志级别
- 实现参数实时调节
-
复位处理:
- 记录复位原因(看门狗/异常/手动)
- 实现安全恢复机制
- 保存关键状态信息
飞行控制是一个需要理论与实践紧密结合的领域。建议的学习路径是:先通过模拟器(如Gazebo+ROS)验证算法,再在实机上逐步测试。记住,好的飞控工程师不仅要有扎实的理论基础,还要有丰富的调试经验和耐心。