四旋翼飞控系统设计:姿态解算与串级PID整定实战
飞行器控制系统设计,说白了就是把传感器读到的姿态、位置信息变成控制信号,让飞机按要求飞起来。这个领域看着门槛高,其实拆开来看就四大块活:采集数据、算姿态、出控制量、驱动电机。我前后做过两套多旋翼飞控,一套基于开源方案改,一套从MCU底层自己写,踩了不少坑,也总结出了一些能直接复用的经验。这篇文章就以我最近完成的一次小型四旋翼控制系统设计为背景,把从需求拆解、器件选型、姿态融合、串级PID整定到实飞排查的完整过程串起来,给想入门或正在做飞控设计的同学一个参考。文章里的方法和结论不绑定某个特定飞控平台,换成固定翼或者垂起,很多思路一样能迁移。
1. 控制系统架构设计:先拆需求,再定硬件方案
1.1 开始设计前,先回答三个问题
不管你是想改一套开源飞控,还是从零画板子写代码,飞行器控制系统设计的起点都不是芯片选型或控制算法,而是先把需求锁死。我这次做的是“1.5公斤级X型四旋翼,室内外都能飞,悬停姿态误差小于1度,抗3级风”,目标一确定,传感器的冗余级别、控制回环频率、电机选型全部跟着定了。如果上来就选一块顶配飞控板,结果发现重量超标、续航不够,整个方案就得返工。
需求定量有个简单的推导方法:先估算起飞重量,再算出电机能提供的最大拉力和推重比。常规多旋翼的推重比建议在2到3之间,也就是最大拉力至少是起飞重量的两倍。这个参数直接决定了油门余量和抗风能力。接着根据任务场景定姿态精度:航拍要求角度稳定在±1°以内,灯光表演可能要求±0.5°,普通娱乐飞行±3°就够。控制精度需求不一样,传感器和算法成本会差很多。
注意:需求阶段最容易犯的错是把参数指标堆得很高,却不考虑测试环境。标称“姿态精度0.1°”在实验室静置台上很容易做到,但实飞振动一来,传感器噪声就会让姿态抖动。我一般会留出50%的余量,例如目标悬停误差±1°,设计时就按±0.5°的标准做,这样才能给滤波和振动留出消化空间。另一个容易忽略的是电池和续航的耦合关系:为了达到更高控制精度加了第二片IMU、更强的主控芯片,但重量增加后续航下降,控制余量反而变小。所以需求阶段要画一张能涵盖重量、功耗、成本、控制性能的权衡表,避免单项指标一路加码。
1.2 方案选型:自研还是套开源,我选“半开源”
飞控开发有个绕不开的路线问题:完全基于PX4/ArduPilot改,还是从单片机裸机自己写?前者生态成熟、调试工具完整,但代码量巨大,一个固件几十万行,改一行都要重新看半天;后者从底层做起,能真正理解每个环节,但开发周期长,稳定性验证需要大量时间。
我这次采用的是“硬件参考开源、算法自己写”的折中路线:主控板引用了类似Pixhawk的硬件架构,但固件不是直接用PX4,而是基于STM32跑一个精简的实时操作系统任务集,姿态解算和控制律全部自己实现。这样既能利用成熟的传感器布局和供电设计,又能把控制链路的每一行代码都吃透。实际做下来,开发周期比我预想的短很多,因为底层驱动和传感器驱动可以从开源框架借力,我需要集中精力写的是姿态、控制、混控这几个核心模块。
这种折中方案还有一个附带好处:对比测试很方便。自己写的算法跑不通的时候,我可以把同一块硬件刷成开源固件作参照,用遥控器打同样的舵量看响应区别,快速判断问题在传感器、控制算法还是执行机构。很多团队也采用类似策略:先在一个成熟框架上做垂直领域的算法验证,验证完再把必要部分移植回自研固件,这样风险最可控。
1.3 硬件信号链路与控制任务划分
飞行器控制系统可以抽象成一条单向数据链:IMU等传感器进入MCU,MCU读数据后做姿态解算,接着控制律计算,再混控输出到电调和电机,机体运动再次被IMU感知,形成闭环。我在架构设计阶段就把这条链上的更新率明确下来,因为这决定了后续所有代码的时间预算。
| 环节 | 更新率/采样率 | 说明 |
|---|---|---|
| IMU采样 | 1000 Hz | 陀螺仪加速度计共享SPI总线,DMA读取 |
| 姿态解算 | 500 Hz | 四元数更新加传感器矫正 |
| 控制律计算 | 500 Hz | 内环角速率、外环姿态串级PID |
| 混控输出 | 500 Hz | 输出DSHOT信号或PWM |
| 任务日志 | 100 Hz | 记录关键状态和调试变量,方便事后分析 |
这个频率分配不是随便拍的。控制回环低于200Hz时,周期延迟会明显降低相位裕度,飞机容易在高速响应时出现振荡;而IMU采样率如果只有500Hz,等于把时间分辨率降下来,陀螺仪在高角速度时的噪声会变大。既然STM32F407的主频足够,我直接把IMU采样拉到1000Hz,姿态解算和控制律都跑500Hz。
在任务调度上,我用了一个简单的RTOS:IMU驱动任务以1ms周期唤醒,通过共享内存把最新数据交给姿态任务;姿态任务完成四元数和角速度更新后,再交给控制任务。这样任务之间耦合度低,后续加航行灯控制之类的小功能也不会干扰主回路。任务划分时还要注意优先级反转问题,比如日志任务优先级不能高于IMU任务,否则IMU数据可能被延迟读取,导致控制周期抖动。
2. 核心模块设计:传感器、姿态解算与校准
2.1 传感器选型:不能只看参数表,抗振和温漂才是关键
飞行器控制系统对传感器的要求可以归纳成三个词:稳定、抗振、可冗余。常见IMU有MPU6000、BMI088、ICM20689这些,但我选型时最关注的不是最高量程,而是温漂和振动环境下的噪声密度。BMI088的陀螺仪噪声相对低,加速度计抗振性比MPU系列好,所以很多飞控都拿它当主IMU。磁力计我用了IST8310,气压计用MS5611,这套组合在主流开源方案里验证得很充分。
除了选型号,PCB布局和减振结构更重要。我的飞控把IMU放在靠近几何中心的位置,并粘贴一层0.5mm的泡沫减振垫,目的是隔离电机振动。注意减振垫不是越软越好:太软会在机动时产生共振,太硬又滤不掉高频振动。我实测下来,用中等硬度的硅胶垫,在300到500Hz范围内的振动衰减效果最好。
传感器冗余方面,我最初设计了双IMU方案,但实际只启用了一个主IMU,第二片作为故障备份和离线对比。开发过程中这个备份IMU帮助很大:我可以把两片IMU的数据同时记录下来,比较振动环境下的输出差异,快速定位是哪一路信号被电机干扰了。如果预算紧张,至少留出一个IMU的焊盘和接口,别把冗余彻底取消。因为实际飞行中,一个IMU被虚拟故障或单点干扰的概率并不低,冗余既是安全策略,也是调试工具。
2.2 姿态解算:互补滤波和卡尔曼的工程取舍
姿态解算就是把陀螺仪、加速度计、磁力计的原始数据融合成一个相对真实的姿态角。直接用陀螺仪积分短时间内很准,但零点漂移会让角度慢慢跑偏;加速度计在没有加速的时候能测重力方向,但飞行器一旦做加减速,它给出的“重力方向”就被扰动了。所以要把两者的优点合并。
最容易被新手理解的是互补滤波:
angle(t) = alpha × [angle(t-1) + gyro_rate × dt] + (1-alpha) × acc_angle(t)
alpha越大越相信陀螺仪,越小越相信加速度计。alpha一般按截止频率取,比如截止频率为1Hz、采样周期0.004秒,则alpha约等于0.98。这个算法实现简单,但控制器在机体运动剧烈时会有滞后,所以我最终选用了Mahony姿态解算,本质上是带非线性PI补偿的互补滤波,用加速度计和磁力计的误差修正陀螺仪零漂。工程上它比完整扩展卡尔曼更容易调试,性能差距也没有传言那么大。
如果你追求更高精度,可以上扩展卡尔曼滤波,把四元数、陀螺仪零偏都放进状态向量。但代价是需要调协方差矩阵,参数没调好,滤波发散起来比互补滤波还难救。我的建议是:第一版飞控先用Mahony把整个链路跑通,等试飞稳定了再决定要不要切卡尔曼。算法复杂度应该跟着需求和风险走,不是越高越好。实际测试中,用好的传感器加上校准到位的Mahony,静态角度精度能优于0.3°,动态飞行时也能保持在1°以内,很多应用是足够的。
2.3 校准环节:最容易偷懒但绝对不能省的步骤
再好的解算算法也救不了没校准干净的传感器。我每次装机都强制自己按下面的顺序走一遍:
- 陀螺仪零偏:水平静止放置1分钟,采样1000帧取平均值,开机前把该值扣除。
- 加速度计六面校准:把飞行器分别放在六个平面上,让每个轴正负方向分别指向重力,采集后标定零偏和标度因数。
- 磁力计校准:在无大金属干扰的环境下旋转机体,做椭球拟合,把硬磁和软磁误差消掉。
- 气压计高度校准:记录起飞点气压和温度,起飞后气压计高度会随气象缓慢漂移,需要结合加速度或GPS高度做融合。
实际试飞时,传感器“装歪”的问题比很多人想象严重。PCB上的IMU芯片方向本来和机体轴一致,但手工焊接或者结构装配稍微偏1到2度,姿态角就会存在固定偏差。我的处理方法是增加一个“机架安装校准”步骤:把飞行器调平放在地面上,记录此时飞控输出的横滚和俯仰角,作为安装偏角补偿。否则你会发现明明机体水平,地面站却显示倾斜2度,后面的控制误差会一直存在。
校准的另一个细节是顺序:先校传感器,再装机,装机后最好做一次机架校准。如果先装机再校传感器,拿到的坐标系其实是“飞控坐标系”,和机体轴的偏差会被标定算法错误吸收。我在一次项目中跳过机架校准,结果横滚角始终偏1.5度,外环I一直增大才把误差压住,但一遇风就明显晃,最后找到根因还是安装偏角的问题。
3. 控制律设计与整定:串级PID背后的逻辑
3.1 为什么飞行器必须用串级PID
很多刚接触飞控的朋友第一反应是给姿态角直接做一个PID,角速度信息只是辅助。但实际飞行器姿态控制几乎都是串级结构:外环是角度环,内环是角速度环。原因在于,单纯比例控制角度只能让飞机“缓慢回正”,缺少对转动角速度的阻尼。
用一个通俗的类比:外环是“规划者”,看到飞机偏了,告诉内环“该以多快的角速度转弯”;内环是“执行者”,负责把这个角速度精确跟踪住。如果只有规划者直接踩油门,系统会像一个只有位置反馈的弹簧,很容易振荡。串级PID的目标是让角度环输出期望角速度,角速度环再把期望角速度跟踪好。
链路是这样的:
期望姿态角 → 角度环PID → 期望角速度 → 角速度环PID → 期望力矩 → 混控 → 电机。
为什么角速度环必须比角度环快?因为内环需要先稳定住机体的转动,外环才能把被控对象看成是一个良好的角速度跟踪器,而不是一个容易振荡的积分器。外环如果设计得比内环还快,整个环路就会不平衡,产生明显的超调和抖动。这也是为什么很多飞控代码里角度环PID频率是100到200Hz,而角速度环可以跑到500Hz,两者物理含义完全不同。
3.2 参数计算过程:从零开始整定内环和外环
参数整定是飞行器控制系统设计里最花时间的一环,但其实是有一套可操作的流程的。我以X型四旋翼为例,说明我常用的工程整定顺序。
第一步只调内环P。把外环角度环P设为零,内环给定一个固定期望角速度,用遥控器拨杆或地面站发送指令,逐渐增大内环P,直到听到电机出现明显的“声音抖动”,或者从日志看到角速度以5到10Hz的频率持续振荡,然后取振荡临界值的一半作为内环P初值。第二步在角速度阶跃响应中加大内环D,用来减小超调。D太大会引入高频噪声,应在试飞中逐步加,不要一次到位。
第三步调外环P。设置一个角度阶跃,比如从0°目标给到30°,观察角度响应。外环P过大会让飞机很快回正但随后来回晃,过小又显得肉。一般外环P从4起步,逐步加到8左右,角度响应没有明显超调就够了。第四步再加外环I,用来消除重力、质心偏移造成的静态误差。I不要给太猛,否则风一吹,角度惩罚过头,飞机会出现低频“点头”。
我常碰到的现象是新手把内环P直接当成“灵敏度”旋钮,一上来就加到0.3以上,结果飞机解锁后高频震动。实际上角速度环P的量级和传感器噪声、机身转动惯量强相关,我做的1.5公斤级机身,内环P大概在0.08附近,内环I在0.05附近,内环D在0.002附近;外环P到6.5,外环I到0.05。这个值放到另一架飞机上不一定合适,所以真正的技巧是掌握整定流程,而不是背参数。另外要注意单位一致,如果你把角速度环的误差单位定义为弧度每秒,那PID系数和用度每秒会完全不同,混用代码最容易出问题。
3.3 混控输出与电机模型:把控制量换成油门命令
控制律计算出来的是期望力矩(横滚、俯仰、偏航)和总油门,但电机执行需要把四路指令变成转速差异才算完成。以X型四旋翼为例,一种常见的混控公式如下(符号取决于电机转向和机头方向,我只展示结构示意):
混控公式要注意两点:一是电机顺序和旋向必须一致,否则正负号全反;二是必须做指令饱和处理。四个电机命令在加法之后经常超过[0,1]范围,简单的钳位会破坏力矩比例,导致急推油门时机体瞬间翻转。我在工程上用的是等比缩放:先把四项中超出边界的部分算出来,再按比例缩到合法范围,保证横滚、俯仰、偏航的相对关系不变。
电机执行部分,现在主流是DSHOT协议,它把控制信号打包成数字帧,省去了传统PWM的脉冲宽度量化误差。DSHOT600的位周期是1.67微秒,更新率可以轻松跑到500Hz以上,而且还能把遥测信号一起传回来。如果还在用普通PWM,至少把PWM频率设在400Hz以上,避免低刷新率引发电机响应滞后。实际项目中我还测试过PWM 50Hz和500Hz的区别:50Hz时电机几乎每20ms才收到一次新指令,悬停时会有明显“呼吸感”,换成500Hz后立马稳定。
3.4 控制频率与延时的工程账
这部分要说一下为什么“更快”并不总是无代价,但飞控主回环确实是越快越好。以500Hz控制回环为例,每个周期的计算时间只有2ms,任务里姿态解算、PID、混控都要在1到1.5ms内完成,否则系统时间预算失控。如果STM32F407超频到168MHz,执行一个四元数更新加三个串级PID也就几十微秒,压力不大;但如果你用了一个很慢的软件I2C读IMU,光一次采样就可能耗时1ms以上,控制回环就拖到250Hz。
我在开发中做过一次降频对比:把控制频率从500Hz降到250Hz,悬停姿态误差从0.8°左右恶化到1.5°以上,飞机在中等风里出现可感知的低频晃动。原因不是控制算法变差了,而是每个周期多出的2ms延迟让整个系统等效相位裕度下降,阻尼变弱。所以,控制规划时给主回环留出充足时间余量,是飞行器控制系统设计里容易被忽略的硬约束。现在很多MCU都带浮点运算单元,推荐直接开启FPU并用单精度浮点,能显著缩短控制律计算时间。
4. 从仿真到实飞:调参流程与实测排查
4.1 在半实物仿真里先跑通逻辑
直接上电试飞的风险太高,我的流程是先做两层验证。第一层是纯软件仿真,把姿态动力学模型和控制律代码在同一台机器上跑,观察在给定扰动下的响应。第二层是硬件在环,把单片机控制板接上仿真器,IMU数据由仿真模型模拟生成,控制输出直接执行仿真里的电机,这样能看到真实PID代码在目标硬件上的运行效果。
不过说实话,对于个人项目,硬件在环的搭建成本不低,我更多用的是“地面台架测试”:把飞控固定在可以自由转动的单轴平台上,分别测试滚转、俯仰、偏航三个通道。台架测试能暴露出PID接线、电机换向等低级错误,但对整机耦合的动态特性覆盖不全面,所以最终还是要小心地进行低速试飞。
仿真和台架不能替代实飞的一个原因是,螺旋桨滑流、机架弹性模态、电池电压跌落和执行机构的真实响应延迟,很难在模型里精确建模。我通常把仿真当作“排除法”:如果代码逻辑不通,仿真会先崩;如果仿真过了实飞还是晃,那问题多半出在物理环节,比如桨平衡、电调校准或减振。在我做过的几个机架方案里,通过仿真能提前优化控制参数的范围,实飞时只需要做局部修正,整体效率提升很大。
4.2 试飞前的安全检查和首次解锁
飞行器控制系统设计做得再好,安全意识不到位也不行。我每次试飞前按清单过一遍,不打折:
- 电池电压:单片电芯不低于3.8V,避免大油门时电压跌穿。
- 螺旋桨方向:看桨叶上的标识和电机转向是否一致,不一致会在解锁瞬间拍地。
- 安全开关:设置好飞控解锁逻辑,遥控器油门最低且方向确认,避免误触发。
- 解锁倾角:飞控检测到倾角超过设定值(如30°)时禁止解锁。
- 日志记录:试飞全程开日志,哪怕只飞1分钟,也足够事后分析。
首次解锁我会把飞机放在平整地面,遥控器油门缓慢从最低推到10%,听电机声音是否平稳,然后立即回到最低。如果电机出现断断续续或转速不稳,先别急着起飞;此时最可能是电调校准不一致、桨叶松动或电机轴承有异物。
确认电机响应正常后,我上电缓缓推油门至25%左右,这时飞机应该还贴地但能感觉到升力。如果飞机明显朝某个方向飘,不要急着修正遥控器,先检查该方向的混控符号和传感器安装方向。我在第一次测试时就犯过“把前右和前左电机接反”的低级错误,结果油门一推,飞机直接逆时针打转,只有通过仔细检查线序才找到原因。这个阶段的排查一定要耐心,把低级错误全部排除后再考虑飞行。
4.3 一次典型的高频振荡排除实录
实飞等待你的往往是各种振荡问题。我记录一次比较典型的:解锁后低油门悬停,飞机从外观到日志都呈现出约7Hz的连续高频抖动,电机声音发紧。
我第一步看日志,发现IMU角速度中混入了明显的7Hz周期性分量,而且四个电机输出都在以同样频率波动。这说明控制回路在“过度补偿”一个外部激励。我先把内环P降低30%,抖动明显减轻但机动力度不够;随后把内环D增加一倍,高频分量被阻尼压住,但仍存在毛刺。再检查振动频谱,确认是电机转速的基频和机架的某个模态耦合所致。最后解决方案是:更换减振垫材料,并让姿态解算增加一个中心频率在7Hz的陷波器,把机架共振分量滤掉。这之后高频抖动基本消失。
这个案例给我的教训是:参数整定不能盲目追求“快速响应”,很多时候噪声和共振会伪装成控制参数问题。正确顺序是先查机械、再从滤波角度处理,最后才动PID。反过来换参数,可能当时好了,换个油门点问题又冒出来。记录每次试飞的频谱图也很关键,有了频谱才能判断振动是机械问题还是控制参数问题,否则只能靠猜。
5. 常见问题排查与经验速查
5.1 解锁失败或电机不转怎么办
我把这个放第一位,因为九成新手都遇到过。可以按顺序排查:
- 遥控器通道映射:确保油门通道、解锁通道和模式通道在飞控里正确映射,很多解锁失败只是“解锁开关没设到对应通道”。
- 飞行器状态报错:地面站会提示磁力计未校准、陀螺仪偏航角异常、安全开关未打开等,逐条消灭。
- 电调校准:如果电调从来没有执行过油门行程校准,解锁后哪怕给油也可能不转。我的做法是第一次上电时统一校准一遍,以后换电调时要重新做。
- 电机线序:无刷电机三根引线其中两根对调会导致电机反转,再叠加混控符号,就会出现各种诡异动作。
排查时建议用地面站实时观察电机输出指令和遥控器输入。如果输出指令有值但电机不动,问题在电调和电机;如果输出指令本来就是0,说明问题在解锁逻辑或通道映射。很多飞控固件在解锁前还要完成“陀螺仪自检”,如果飞机放在移动平台上,自检可能失败,把飞机放到固定桌面再重新上电就好。
5.2 姿态漂移和高度漂移怎么区分
姿态角度缓慢漂移通常是传感器零偏没扣干净或数据融合权重不对。我遇到过的特例是飞控板温度变化导致陀螺仪零偏漂移,开机前在校准点状态,飞了五分钟后机温上来,角度就缓缓偏了。对策是让飞控上电预热2分钟再校准,或者加一个缓慢的零偏在线估计,把陀螺仪零偏作为状态量放到滤波器里。
高度漂移则更复杂。气压计的原始噪声很大,而且会受螺旋桨气流影响。如果气压计位置离螺旋桨太近,悬停时会受到乱流冲击,高度控制会持续波动。正确的做法是把气压计放在避风的泡棉腔里,并在结构上留一个小孔和环境连通。另外,气压计高度要和加速度计做融合,不能拿原始气压值直接当高度反馈,否则飞机会像坐电梯一样忽上忽下。
区分姿态漂移和高度漂移有一个简单方法:看日志里的姿态角误差变化和高度估计变化。如果角度误差随时间单调增大,说明姿态解算在漂;如果角度正常但油门指令一直变化,说明高度通道在漂。把问题归类后,再决定是校准传感器还是检查气压计安装,能省不少时间。
5.3 滤波越深越好吗?陷波器怎么用
很多人在处理振动时会堆低通滤波器,结果姿态解算的相位延迟越来越大,飞控越调越飘。低通滤波器的截止频率每降低一倍,相位滞后会增加不少;控制回环里最怕的就是滞后。我一般只在陀螺仪数据上加很小的低通(截止频率80到120Hz),主要靠机械减振解决宽频振动;对于单一的窄带共振,用陷波器更合适。
陷波器的作用是“精准挖掉”某个频率,例如机架共振在7Hz,就在姿态数据通路上加一个中心频率7Hz的陷波器,衰减该频率的信号,同时对其他频率影响很小。工程上需要考虑陷波器的Q值:Q太高会窄到不稳定,Q太低会误伤有用信号。用频谱分析工具看到明确的尖峰后再加陷波器,不要盲目堆。我建议在飞控代码里做一个可在线修改的滤波参数表,试飞时通过串口或遥测调整,能省掉反复刷固件的麻烦。
5.4 飞行日志怎么看,重点看哪几类曲线
试飞完一定不要只看“飞得稳不稳”,要靠日志判断控制余量和系统健康状态。我最常看的曲线有这么几组:IMU的加速度和陀螺仪原始数据,看是否出现周期性尖峰;姿态角的误差和角速度输出,看是否有持续振荡;电机指令,看是否经常顶到饱和边界;电池电压和电流,看动力系统是否过载。如果某个方向电机指令经常到0或1,说明混控分配有问题或者油门余量不足。
另一个容易忽略的是“指令余量”。如果四个电机指令总有某一个接近上限,说明机身重心偏差较大,或某个旋向的螺旋桨效率差异明显。这时即使姿态控制还能维持,姿态角也会持续偏向一侧,需要先解决配平问题,而不是继续调PID。日志分析时,建议把时间轴对齐到遥控器打杆的瞬间,观察打杆后50到200ms内角速度响应是否平滑,这样可以快速评估控制环路的快速性和阻尼感。
5.5 我的两点体会
飞行器控制系统设计做了几个项目之后,我最深的感受是:别把算法想得太神秘,也别把硬件想得太简单。一个飞控能不能稳飞,往往是机械振动、传感器校准、功率裕度和PID四者是否匹配。很多所谓“玄学振荡”,拆开来看都是某一环没做到位。
另外一个小技巧:每次试飞后,把调参前后的日志和参数文件按日期命名归档。等积累了几十次试飞,你就能做出属于自己的“参数与现象对照表”,以后再换机架,整定效率会快很多。控制系统的坑很多,但只要你一步一个脚印地推,它终究是一个可复现的工程问题。