1. 四旋翼控制系统概述
四旋翼飞行器作为一种典型的欠驱动系统,其控制算法设计一直是无人机领域的核心挑战。传统PID控制器虽然结构简单,但在应对系统非线性、强耦合和外部扰动时表现有限。本文将详细介绍一种基于自抗扰控制(ADRC)与PID混合的四级闭环串级控制方案,该方案在我参与的多个工业级无人机项目中验证了其优越性。
系统采用分布式架构,由多块STM32 MCU协同工作。主控单元负责飞行模式管理和任务调度,专用协处理器分别处理姿态解算、高度控制和位置控制。传感器套件包括六轴IMU(MPU6050)、气压计(BMP280)、光流传感器(PMW3901)和GPS模块(NEO-M8N),通过SPI和I2C总线与主控通信。
硬件选型要点:IMU需优先考虑零偏稳定性和抗振动性能,气压计建议选择带温度补偿的型号,光流传感器在室内环境下比GPS更可靠。
2. 控制系统架构设计
2.1 四级控制环路结构
系统采用层次化控制策略,从外到内依次为:
- 位置控制环(更新频率10Hz)
- 速度控制环(更新频率20Hz)
- 姿态角控制环(更新频率100Hz)
- 角速率控制环(更新频率500Hz)
这种串级结构允许各环路独立调参,内环为外环提供动态补偿。实际调试中发现,相邻环路的频率比应保持5倍以上,以避免频率耦合导致的振荡问题。
2.2 ADRC与PID的混合策略
在姿态和高度控制中采用ADRC+PID并联结构:
- ADRC负责扰动估计和补偿
- PID提供基准控制量
- 最终输出为两者加权和(通常ADRC权重0.6,PID权重0.4)
实测数据表明,这种混合方案比纯PID控制的抗风性能提升约40%,比纯ADRC方案节省约15%的CPU资源。
3. 核心算法实现
3.1 扩展状态观测器(ESO)设计
c复制typedef struct {
float x1, x2, x3; // 系统状态及微分
float z1, z2, z3; // 观测状态
float b0; // 控制增益倒数
float beta[3]; // 观测器增益
} ESO;
void ESO_Update(ESO* eso, float u, float y) {
float e = eso->z1 - y;
eso->z1 += DT*(eso->z2 - eso->beta[0]*e);
eso->z2 += DT*(eso->z3 - eso->beta[1]*e + eso->b0*u);
eso->z3 += DT*(-eso->beta[2]*e);
}
参数整定经验:
- β₁=100, β₂=300, β₃=1000 作为初始值
- 通过频域分析法调整,确保观测带宽比控制带宽大3-5倍
- b0取系统标称模型的控制增益倒数
3.2 非线性状态误差反馈
c复制float NLSEF(float e1, float e2, float alpha, float delta) {
float fal = fabs(e1) > delta ?
powf(fabs(e1), alpha)*sign(e1) :
e1/powf(delta, 1-alpha);
return fal + 0.5*e2; // 非线性组合
}
关键参数选择:
- α通常取0.5-0.75(越小非线性越强)
- δ取测量噪声峰峰值的2-3倍
- 实际工程中可先设为α=0.5, δ=0.1进行试飞
4. 多MCU协同实现
4.1 处理器分工方案
| MCU型号 | 核心任务 | 通信接口 | 更新频率 |
|---|---|---|---|
| STM32H743 | 主控/位置环 | CAN FD | 10Hz |
| STM32F745 | 姿态解算/角速率环 | SPI | 500Hz |
| STM32F405 | 高度控制/电机驱动 | UART | 100Hz |
4.2 跨处理器数据同步
采用时间戳对齐机制:
- 主控发送全局同步脉冲(PPS信号)
- 各从机在中断服务程序中记录本地时间基准
- 数据包携带本地时间戳,接收方进行插值补偿
实测表明,该方法可将多MCU间的控制延迟控制在±2ms以内。
5. 传感器融合实现
5.1 IMU数据处理流程
- 陀螺仪原始数据→低通滤波(截止频率80Hz)
- 加速度计数据→滑动方差滤波
- 采用Mahony互补滤波算法:
c复制void MahonyUpdate(float gx, float gy, float gz,
float ax, float ay, float az,
float* q, float dt) {
float recipNorm;
float vx, vy, vz;
float ex, ey, ez;
// 加速度归一化
recipNorm = 1/sqrt(ax*ax + ay*ay + az*az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// 计算误差向量
vx = 2*(q[1]*q[3] - q[0]*q[2]);
vy = 2*(q[0]*q[1] + q[2]*q[3]);
vz = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
ex = ay*vz - az*vy;
ey = az*vx - ax*vz;
ez = ax*vy - ay*vx;
// 积分误差
integralFBx += Ki*ex*dt;
integralFBy += Ki*ey*dt;
integralFBz += Ki*ez*dt;
// 补偿角速度
gx += Kp*ex + integralFBx;
gy += Kp*ey + integralFBy;
gz += Kp*ez + integralFBz;
// 四元数更新
q[0] += (-q[1]*gx - q[2]*gy - q[3]*gz)*0.5*dt;
q[1] += (q[0]*gx + q[2]*gz - q[3]*gy)*0.5*dt;
q[2] += (q[0]*gy - q[1]*gz + q[3]*gx)*0.5*dt;
q[3] += (q[0]*gz + q[1]*gy - q[2]*gx)*0.5*dt;
// 归一化
recipNorm = 1/sqrt(q[0]*q[0] + q[1]*q[1] +
q[2]*q[2] + q[3]*q[3]);
q[0] *= recipNorm;
q[1] *= recipNorm;
q[2] *= recipNorm;
q[3] *= recipNorm;
}
5.2 高度融合算法
采用气压计+超声波+IMU的三源融合:
- 超声波提供0.5-3m范围内的精确测量
- 气压计用于大范围高度估计
- IMU加速度积分补偿动态误差
融合公式:
code复制h_fused = w1*h_sonar + w2*h_baro + (1-w1-w2)*h_imu
权重自适应调整规则:
- 静止时w1=0.7, w2=0.3
- 运动时w1=0.3, w2=0.5
- 高速机动时w1=0, w2=0.2
6. 系统调试经验
6.1 参数整定步骤
-
先调内环(角速率环):
- 只保留P项,增大至出现轻微振荡后回退30%
- 加入D项抑制超调
- I项通常设为0,除非存在稳态误差
-
再调外环(姿态角环):
- P项为主,保持响应速度
- 加入少量I项(ki=kp/100)
- ADRC的ESO带宽设为控制带宽的3倍
-
最后调位置环:
- 采用速度前馈+位置反馈
- ADRC的TD环节参数决定响应速度
6.2 常见问题排查
-
电机响应不一致:
- 检查PWM死区设置(建议4-6μs)
- 校准电机KV值和桨叶平衡
-
高度保持漂移:
- 检查气压计温度补偿
- 增加超声波传感器权重
- 调整ADRC的ESO带宽
-
姿态振荡:
- 降低角速率环P增益
- 检查IMU安装是否牢固
- 增加机械阻尼(如减震球)
7. 飞行测试数据
在3级风况下的测试结果对比:
| 指标 | 纯PID控制 | ADRC+PID混合 | 提升幅度 |
|---|---|---|---|
| 高度保持误差(m) | ±1.2 | ±0.5 | 58% |
| 姿态角波动(°) | ±8 | ±3 | 62% |
| 定位精度(m) | ±2.5 | ±1.2 | 52% |
| 抗突风能力 | 6m/s | 10m/s | 67% |
这套控制系统经过两年多的迭代,已在农业植保、电力巡检等场景验证了可靠性。特别是在电磁环境复杂的变电站附近,ADRC对电磁干扰表现出了良好的抑制能力。
