STM32+MPU6050固定翼增稳飞控：从姿态解算到PID调参与救机

做了几年固定翼航模从最开始用别人的飞控到后来自己拿着 STM32 和 MPU6050 从零搓一套增稳飞控中间踩过的坑比炸过的机还多。这篇东西就是把我在 STM32 上实现固定翼增稳飞控的完整思路拿出来聊聊包括姿态解算怎么搞、双环 PID 怎么调、一键救机怎么设计以及实际调试中那些容易让人抓狂的问题怎么排查。如果你正准备做一个基于 STM32 的飞控项目或者手头有一架固定翼想加装增稳功能这篇文章应该能帮你少走不少弯路。1. 项目概述与整体架构为什么用 STM32 做固定翼增稳1.1 固定翼增稳飞控的核心问题固定翼飞机不像多旋翼那样天生稳定它在飞行中会受到气流扰动、扭矩效应、重心变化等多种因素影响。尤其一些小尺寸的泡沫机、KT 板机稍微遇到点风就晃得厉害新手操作时经常出现俯仰振荡、滚转侧滑甚至直接进入螺旋状态。增稳飞控要解决的核心问题就是在不改变遥控器手动操作习惯的前提下自动抑制飞行姿态的异常变化让飞机飞起来更稳、更听话。它所做的不是“自动驾驶”而是把遥控器给的杆量与飞机的实际姿态差距做一个误差修正然后输出到舵面本质上是给飞机加了一层阻尼和稳定修正。和多旋翼飞控相比固定翼增稳有几个明显差异。多旋翼靠四个电机差速出力控制姿态响应快但控制周期要求很高固定翼则靠舵面偏转改变气动力矩有延迟、有非线性不同速度下舵面效率差别很大这让控制参数的设计更有挑战。增稳系统起到的更多是“辅助”作用操控权始终在人手里。1.2 系统架构与关键器件选型思路整套系统我是这样规划的主控芯片负责姿态解算、控制律计算、信号采集和舵机输出IMU 传感器负责采集角速度和加速度遥控器接收机提供手动控制信号舵机和电调执行控制指令。主控我选了 STM32F103C8T6。这个芯片在 STM32 生态里属于“国民级”型号72MHz 主频、64KB Flash、20KB RAM对于固定翼飞控来说完全够用。关键资源包括多个定时器可以输出带死区控制的 PWM、硬件 I2C/SPI 接口、丰富的 GPIO 和 ADC。如果采用更高端的 F405可以用浮点运算单元加速四元数计算但经过验证F103 在 1kHz 控制周期下跑完整套姿态解算和 PID 运算CPU 占用率也只有 30% 左右性价比很高。传感器用的是 MPU6050六轴含三轴陀螺仪和三轴加速度计。它在航模和机器人项目里太常见了I2C 接口好写驱动自带数字低通滤波器DLPF姿态解算精度足够。选它主要出于两个考量一是资料多、部署简单出问题容易排查二是固定翼飞行中对姿态角精度的需求大约在正负 1 到 2 度MPU6050 在融合算法配合下完全能达到。系统输入方面遥控器 PWM 信号通过定时器输入捕获读取这是最常用的方法。STM32 定时器的输入捕获模式可以直接测量 PWM 脉宽分辨率能到 1 微秒换算成遥控器数据就是 1000 到 2000 之间的标准数值。输出则用定时器 PWM 模式控制舵机和电调用固定的 50Hz 频率驱动舵机电调如果是航模标准协议同样是 50Hz PWM。1.3 代码工程结构与任务调度飞控代码我按照“传感器层、算法层、控制层、应用层”四层来组织。这种分层的思路在后期调参和排障时收益特别高每个模块独立测试互不干扰。传感器层封装了 MPU6050 的底层驱动I2C 读写、寄存器配置、原始数据读取、零偏校准。算法层是姿态解算模块负责把陀螺仪和加速度计的原始数据融合成四元数和欧拉角。控制层是双环 PID 和舵面混控输出接收姿态角和遥控器目标值计算舵面指令。应用层则是救机逻辑、遥控器信号解析、状态判断这些功能。任务调度用定时器中断做 1kHz 时基也就是每 1 毫秒执行一次控制主循环。在这个循环里按顺序做遥控器信号解析、姿态解算、PID 计算、舵机输出。1kHz 的控制频率对固定翼来说绰绰有余舵机本身的机械响应频率通常只有 30 到 50Hz控制频率再高也发挥不出来。2. 姿态解算从 MPU6050 原始数据到欧拉角的完整流程2.1 为什么用四元数表达姿态姿态解算的目标是实时计算飞机在空间中的朝向通常用三个欧拉角横滚角 roll、俯仰角 pitch、偏航角 yaw表示。但在实际计算中直接用欧拉角做积分会遇到一个问题万向锁Gimbal Lock。当俯仰角接近正负 90 度时横滚和偏航的旋转轴重叠系统自由度丢失数学上会出现奇点导致姿态无法正确更新。四元数是一种由四个实数组成的超复数用来描述三维空间中的旋转。它的最大优势是没有奇点计算效率高只有乘法和加法非常适合嵌入式实时系统。四元数表达的旋转可以理解为绕某个轴旋转某个角度形式紧凑且光滑。我在项目中维护一个单位四元数 q (q0, q1, q2, q3)它实时表示飞机在世界坐标系中的朝向然后每隔一段时间从四元数解算出欧拉角供 PID 控制使用。四元数转欧拉角的公式是固定的到了控制层只需要关心角度值不必关心四元数本身的更新细节。2.2 MPU6050 数据采集与预处理MPU6050 的驱动本质上是标准的 I2C 通信。初始化时需要配置陀螺仪量程我设置为正负 2000dps、加速度计量程正负 8g、数字低通滤波器带宽设置为 42Hz 左右。这个带宽的选择有讲究滤波太窄会让信号滞后影响控制实时性太宽又会引入高频噪声。初始化的最后一步要做零偏校准。陀螺仪存在零偏误差静止时读数不是零而是有一个固定偏移。如果不校准积分后姿态会持续漂移。我的校准逻辑是在上电后飞机保持静止 2 秒连续采样 500 组陀螺仪数据求平均得到三个轴的零偏值并保存后续每次读取都减去这个零偏。加速度计虽然不需要零偏校准那么严格但数据中包含大量高频振动噪声尤其是电机和螺旋桨转动时必须经过滤波。我只在解算前做了一次简单的滑动平均滤波把窗口设成 4 个采样点实测降噪效果不错又不引入明显延迟。2.3 基于互补滤波的姿态融合算法姿态融合要解决的核心矛盾是陀螺仪短期精度高但长期会漂移加速度计长期稳定但短期噪声大。两者的特性正好互补于是就有了互补滤波的思想——对陀螺仪积分结果用高通滤波对加速度计姿态估计用低通滤波相加后得到最优估计。以横滚角为例。陀螺仪积分给出高频可信的角速度积分结果加速度计通过重力方向算出静态倾斜角。但加速度计算出的角度在飞行中振动大时噪声特别严重如果直接拿来用会造成角度抖动表现在舵面上就是舵面狂抖。具体实现采用 Mahony 互补滤波算法在开源飞控中非常流行的方案核心思想是利用加速度计测量值与当前四元数推算出的重力方向之间的误差通过 PI 修正器补偿陀螺仪零偏误差然后对修正后的角速度做四元数积分。算法流程简化如下读取陀螺仪角速度减去零偏读取加速度计归一化向量用当前四元数推算重力方向向量两者做向量叉积得到误差误差经 PI 补偿后叠加到陀螺仪角速度上最后用四元数微分方程更新四元数并做归一化。这段逻辑写成 C 语言大致如下void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) { float recipNorm; float halfvx, halfvy, halfvz; float halfex, halfey, halfez; float qa, qb, qc; // 加速度计归一化 recipNorm invSqrt(ax*ax ay*ay az*az); ax * recipNorm; ay * recipNorm; az * recipNorm; // 由四元数推算重力方向 halfvx q1*q3 - q0*q2; halfvy q0*q1 q2*q3; halfvz q0*q0 - 0.5f q3*q3; // 误差 测量值与推算值的叉积 halfex ay*halfvz - az*halfvy; halfey az*halfvx - ax*halfvz; halfez ax*halfvy - ay*halfvx; // PI 修正陀螺仪 integralFBx twoKi * halfex * dt; integralFBy twoKi * halfey * dt; integralFBz twoKi * halfez * dt; gx twoKp * halfex integralFBx; gy twoKp * halfey integralFBy; gz twoKp * halfez integralFBz; // 四元数微分更新 qa q0; qb q1; qc q2; q0 (-qb*gx - qc*gy - q3*gz) * dt; q1 (qa*gx qc*gz - q3*gy) * dt; q2 (qa*gy - qb*gz q3*gx) * dt; q3 (qa*gz qb*gy - qc*gx) * dt; // 归一化四元数 recipNorm invSqrt(q0*q0 q1*q1 q2*q2 q3*q3); q0 * recipNorm; q1 * recipNorm; q2 * recipNorm; q3 * recipNorm; }Kp 和 Ki 两个参数决定了融合效果Kp 越大加速度计对姿态估计的修正作用越强角度收敛越快但噪声也越大Kp 太小则陀螺仪漂移修正不过来。我项目中 Kp 取了 2.0 左右Ki 取 0.01静止时角度稳定在正负 0.5 度以内动态飞行时也没有明显的滞后感。2.4 姿态解算的周期与性能整个姿态解算在 1kHz 定时器中断里执行每次调用 Mahony 算法大约耗时 15 到 20 微秒F103 在 72MHz 下加上 MPU6050 读取通过硬件 I2C 调用、模拟 I2C 的话耗时稍长。这个性能余量意味着即使后续加更多传感器数据融合主控也完全不会出现算力瓶颈。实际调试经验姿态解算跑起来之后第一件事就是做静态测试。把飞机固定好用手掰动机身模拟不同姿态观察串口输出的横滚、俯仰角是否与实际情况一致、是否稳定无漂移。这一步过了再上电调否则后续控制调试会陷入“分不清是传感器问题还是控制问题”的泥潭。3. 双环 PID 在固定翼增稳中的工程实现3.1 内环角速度环与外环角度环的配合逻辑固定翼增稳控制我用的是经典的双环级联 PID外环是角度环内环是角速度环。这两个环的关系可以打个比方角度环是“驾驶员”看到飞机偏离目标角度告诉舵机“需要转多少”角速度环是“执行者”感受到飞机正在以多快的角速度旋转及时控制舵面使转动平稳。外环角度环的输入是“目标角度与当前角度的误差”输出是“目标角速度”。内环角速度环的输入是“目标角速度与当前角速度的误差”输出才是最终的“舵面指令”。为什么多一层内环因为角速度环能大幅抑制外界扰动如突风同时让角度环的控制对象更平稳。如果没有内环直接让角度环输出舵面遇到阵风时舵面反应会很迟钝或者超调猛烈。在固定翼上横滚通道和俯仰通道分别用两组双环 PID。横滚通道控制副翼舵面俯仰通道控制升降舵面。偏航通道可以只做角速度阻尼也就是 P 控制因为固定翼的航向主要通过横滚转弯间接实现直接输出方向舵只用来消除逆偏航。3.2 双环 PID 的控制律表达式与代码框架外环角度环采用比例控制加微分阻尼基本表达式为// 外环角度误差 - 目标角速度 float target_rate Kp_angle * (desired_angle - current_angle);内环角速度环则是标准 PID// 内环角速度误差 - 舵面输出 float rate_error target_rate - current_rate; integral_rate rate_error * dt; output Kp_rate * rate_error Ki_rate * integral_rate Kd_rate * (rate_error - last_rate_error) / dt;实际面对工程问题时会有几个细节。内环积分项必须加限幅否则强大扰动持续作用时输出饱和造成“积分饱和”现象恢复响应变慢还容易引发振荡。微分项对噪声非常敏感我通常对微分项做一阶低通滤波滤掉高频抖动成分后再参与计算。代码层面我把内环 PID 和外环 PID 放进了同一个 1kHz 控制循环先算外环再算内环。输出最终换算成舵机脉宽映射到 1000 到 2000 微秒范围中间 1500 微秒是中立位置。void Control_Loop(void) { // 1. 读取遥控器目标值 rc_roll Read_RC_CH1(); // 副翼通道对应目标横滚角 rc_pitch Read_RC_CH2(); // 升降通道 // 2. 姿态解算更新 attitude_update(); // 3. 外环角度环 float target_roll_rate roll_kp * (rc_roll - roll_angle); float target_pitch_rate pitch_kp * (rc_pitch - pitch_angle); // 4. 内环角速度环 float droll PID_Compute(roll_rate_pid, target_roll_rate, gyro_roll); float dpitch PID_Compute(pitch_rate_pid, target_pitch_rate, gyro_pitch); // 5. 输出混控到舵面 Servo_Output(0, 1500 droll); // 副翼 Servo_Output(1, 1500 dpitch); // 升降 }纯手动模式时遥控器杆量直接映射到舵面也就是不做任何增稳增稳模式时才是上面这套逻辑。切换逻辑非常简单通过遥控器一个辅助通道判断模式。3.3 PID 参数整定的实操步骤参数整定是整个项目最耗时的环节也是新手最容易崩溃的地方。我的整定流程严格遵循“由内到外、由简到繁”的顺序。先关闭外环直接给内环一个角速度目标值调内环 P。从小数值开始横滚 0.1 级别逐渐增大观察飞机或手动转动飞机对目标角速度的响应。内环 P 太小响应迟钝P 太大飞机会出现高频抖动——手握住机身能感觉到持续的震动。内环调稳后加入外环角度环。外环 P 决定“对角度偏差的反应强度”继续从小到大试。如果飞机出现低频来回摆动机身周期性晃动说明外环 P 太激进如果推杆后飞机姿态恢复缓慢说明外环 P 不够。实际整定中我发现一组容易成功的参数起点横滚外环 Kp 取 5 到 7内环 Kp 取 1.5 到 2.5内环 Ki 取 0.1 左右俯仰方向因为飞机纵向稳定性通常优于横向参数可以略小。最终值根据机型、重心位置不同而调整不同飞机之间的差别非常大。一个小技巧在整定时用串口把目标角度、当前角度、舵面输出三个量同时打印出来用曲线工具画出来看。数据比手感可靠得多能清楚看到响应延迟、超调量、振荡频率这些关键信息。4. 一键救机功能的设计与实现4.1 救机需求的来源与触发条件设计固定翼常见的失控情况有几类进入失速螺旋、俯仰振荡发散、姿态角度过大导致机头朝下高速俯冲、倒飞状态没有意识到进而打反舵加剧失控。一键救机要做的就是在这些危险状态下按一个键让飞控自动采取动作把飞机恢复为平飞或者爬升状态把控制权还给飞行员。我设计的救机触发条件分两种。一种是遥控器主动触发设定一个专用通道飞行员拨到救机档位就立即启动另一种是姿态异常自动触发当检测到横滚角绝对值超过 60 度且飞行员当前没有主动干预遥控器摇杆回到中立位时自动进入救机状态。姿态异常自动触发要加一个短时确认延迟比如持续 200ms避免飞机做大机动时瞬间超过角度阈值就误触发。4.2 救机策略从姿态改出到恢复手动控制救机启动后执行的控制策略有三个阶段。第一阶段是“先回平”。目标横滚角设为 0目标俯仰角设为安全爬升角通常取 10 到 15 度让飞控用双环 PID 全力控制舵面使飞机回到安全姿态。回平过程中如果飞机仍在高速旋转角速度环会先尽全力刹转角速度内环在救机阶段的作用比角度环更大——必须先把滚转速率压下来角度才能有效回调。所以救机模式下我会临时把内环 Kp 的系数放大 1.5 倍并启用积分快速清零功能防止残留积分导致纠偏反向。第二阶段是“维持安全姿态”。飞机达到目标姿态后维持横滚 0 度、俯仰 10 到 15 度持续飞行。此阶段持续 2 秒给飞行员留出重新判断和接管的时间。第三阶段是“交还控制”。2 秒后系统自动退出救机模式控制权交还遥控器杆量。如果救机期间飞行员拨回手动档位则立即退出救机防止飞控与人为操作“打架”。4.3 救机状态机实现与安全保护细节救机逻辑我用一个状态机来实现防止逻辑混叠。typedef enum { MODE_MANUAL 0, MODE_STABLE, // 增稳模式 MODE_RESQUE_WAIT, // 检测到异常确认中 MODE_RESQUE_ACTIVE // 救机执行中 } FlightMode; FlightMode flight_mode MODE_MANUAL; void Check_Rescue_Trigger(void) { if (rc_rescue_switch 1500) { // 遥控器主动触发立即进入救机 rescue_timer 0; flight_mode MODE_RESQUE_ACTIVE; return; } if (flight_mode MODE_STABLE fabs(roll_angle) 60.0f rescue_timer 200) { rescue_timer dt_ms; if (rescue_timer 200) { flight_mode MODE_RESQUE_ACTIVE; } } }救机阶段还有一个强制中断保护的细节如果姿态改出失败比如角速度过大致使回平超时5 秒后强制退出并回到手动模式避免飞控死死扶着飞机、飞行员完全失去控制力。这个超时时间需要结合真实飞机的机动能力来设定太短救不过来太长则有可能把飞机压出新的危险状态。另外救机触发时如果油门低于 20%飞控会输出提示让飞行员开大油门但不会直接接管油门通道。固定翼的救机本质上是角运动控制油门直接接管存在很大的安全隐患——如果自动推油时机不对反而可能加剧失速。这一点和个人飞行经验相关但我的结论是不要碰油门舵面改出足够平稳安全。5. 调试过程中的常见问题与避坑经验5.1 姿态解算的常见坑MPU6050 数据读取中最大的坑是 I2C 通信的不稳定。飞控的抗振动环境还好但在动力电机启动后干扰明显增加。我的解决办法是I2C 总线速度降到 200kHz加上拉电阻配置为内部上拉读取时增加超时判断。如果 I2C 读失败就跳过本次采样返回上一次的有效值绝不能阻塞等死否则飞控会瞬间失控。姿态解算另一个常见问题是陀螺仪零偏校准时的环境不干净。如果在校准过程中机身被风吹动或手碰到校准出来的零偏就是错的静态还好飞行中角度会缓慢漂移。我的做法是在校准期间连续检查角速度均值如果发现角速度绝对值大于 0.2dps 就判为校准无效重新校准。5.2 PID 调节的常见坑PID 调试中最常见的就是“内环 P 过大导致高频啸叫”和“外环 P 过大导致低频低幅度摆动”。区分这两种振荡的方法是看机身抖动的频率和声音高频抖动多半伴随舵机吱吱声是内环问题低频摆动持续好几秒一个周期是外环问题。另一个容易被忽略的是舵机中立点不在 1500 微秒。如果副翼中立时舵面本身就是偏的那飞控所有的修正都会叠加在一个错误的基准上飞机会一直向一侧偏。上电前务必手动检查每个舵面的中立位置这一点直接用摇控器和飞控都行但要在飞控初始化时读取默认输出值做软件微调。微分项在飞控里是把双刃剑。固定翼上的 Kd 效用不如多旋翼那么明显因为舵面对角速度的响应本身就有延迟微分增益过大的话在高速转动中会输出很大的反向力矩反而加剧舵面抖动。建议 Kd 从 0 开始只在遇到阻尼不足时才慢慢加。5.3 一键救机误触发与失效问题救机误触发是我测试中遇到最让人头疼的问题。有一次测试中飞机做大坡度盘旋时误入救机姿态瞬间被拉平整个飞行轨迹打断了。排查发现是姿态阈值设置得太激进45 度加上延迟确认时间太短正常机动中瞬间就能触发。经过反复试飞我最终把横滚自动触发阈值定在 60 度、确认时间 200ms。前提是增稳模式下正常机动不会超过这个幅度。如果后续要做特技飞行要么关闭自动救机要么把阈值提高到 80 度以上——救机功能本质上是“安全底线”不是“自动导航”不该干预正常飞行的包线。救机失效的原因多半出在角速度内环的积分项。救机动作初始阶段角速度误差极大积分快速饱和输出直接冲到舵面限幅。如果此时飞机速度低、舵面效率不足改出动作就会显得疲软无力。受到这个启发我在救机启动后第一拍把积分项清零并重新累积这样既保住了积分修正能力又避免了历史误差的“惯性冲击”。5.4 硬件层面的问题供电稳定性是飞控的“底线”。舵机在大电流动作时会造成电压跌落严重时 MCU 直接复位。我的飞控板加了 1000uF 电解电容和若干 0.1uF 陶瓷电容做电源滤波电机和舵机独立供电飞控通过 BEC 单独供电。调试中如果发现“一推油门 MCU 就复位”或“舵机一动屏幕闪黑”十有八九是电源问题。飞控安装位置也影响巨大。尽量靠近飞机重心同时远离电机、电调和舵机线缆防止磁场和电流干扰。MPU6050 振动剧烈时会输出大量噪声虽然数字滤波能压住一部分但物理减震泡棉双面胶效果远好于算法硬扛。5.5 快速参考问题排查速查表现象可能原因排查方向串口角度漂移陀螺仪零偏未校准或校准不当重新校准、检查静止状态角度跳动大振动干扰、滤波参数不当调大 DLPF 带宽、增加滑动平均舵机高频抖动内环 P 过大降低内环 P飞机低频摆动外环 P 过大降低外环 P救机误触发角度阈值过小调整阈值和确认时间救机反应迟钝积分饱和或内环 P 不足救机启动时清积分加大内环 P推油门 MCU 复位电源跌落增加电容、独立供电6. 写在最后的一点经验这个项目从零开始到完整飞行中间大概过了三周真正写代码的时间其实只有三四天剩下的时间全在调参、炸机、发现问题、重新改动中循环。固定翼飞控比多旋翼飞控“玄学”的地方就在于它和空气动力耦合太深同一个参数在静态测试时完美上了天就可能完全不同。如果让我重新做一次我会提前准备地面测试台把飞机固定在一个可以从外部改变姿态的支架上先在静态模拟各种飞行姿态来验证控制效果而不是直接上真机。现在很多人做飞控都急着上天测试一次炸机损失的不仅是硬件更是大量的调试时间。这个小项目做完之后我还想过把救机功能扩展成“自动改出螺旋”——通过检测持续的高角速度和角度发散来判断失速状态然后用副翼和方向舵配合做螺旋改出。这个功能需要更完整的传感器融合和更深入固定翼动力学建模目前正在研究中。如果你也在做类似的 STM32 飞控项目欢迎交流实际调试中遇到的问题一起把这些经验补全。