1. 从一次炸机说起飞控算法到底在管什么很多人第一次接触无人机注意力都在电机、桨叶、机架上觉得这些看得见摸得着的东西才是核心。我当年也这么想直到第一次试飞自组的四轴起飞不到三秒就侧翻炸机桨叶断了一地。那次之后我才真正意识到让一架多旋翼能稳稳悬停在空中的不是那几个转得飞快的电机而是藏在飞控板里、每秒运算几百上千次的那套算法。飞控算法说白了就是无人机的大脑决策逻辑。它要干的事情可以拆成一条闭环链路传感器告诉飞控“现在姿态是什么样”飞控算法算出“离我想要的目标姿态差多少”然后解算出每个电机该给多少油门最后通过电调驱动电机。这条链路每秒要跑几百到几千轮任何一环慢了或者算错了飞机就会抖、会漂、会翻。这篇文章我想把飞控算法这条链路从头到尾拆开讲清楚。适合两类人看一类是刚上手 Pixhawk 这类开源飞控、想搞明白参数背后含义的飞友另一类是自己写飞控、做硬件在环仿真或者路径规划需要理解底层控制逻辑的开发者。我不会只丢公式而是把每个环节“为什么这么设计”“实际调的时候会踩什么坑”讲透。看完你至少能明白为什么你的飞机悬停会画圈、为什么定高会掉高、为什么自动航线会切弯切得离谱。2. 姿态解算飞控算法的第一道地基2.1 传感器给的数据为什么不能直接用飞控板上通常有加速度计和陀螺仪高端点的还有磁力计和气压计。加速度计测的是比力静止时能反映重力方向从而知道“哪边是下”陀螺仪测的是角速度积分之后能得到角度变化。听起来两者都能算姿态但单独用都不行。加速度计的问题是对振动和线性加速度极其敏感。电机一转机架高频振动直接混进加速度信号里算出来的姿态全是噪声。而且飞机一旦加速往前飞加速度计会把“向前加速”误判成“机身倾斜”姿态就错了。陀螺仪的问题正好相反它短期响应快、不受振动影响但积分会累积漂移时间一长角度就慢慢偏了飞着飞着飞机就以为自己是斜的。所以工程上的标准做法是互补滤波或者卡尔曼滤波把两者的优点捏在一起。互补滤波的思路很直白高频段信陀螺仪低频段信加速度计。用一个简单的公式表达就是// 互补滤波伪代码 angle 0.98 * (angle gyro_rate * dt) 0.02 * accel_angle;那个 0.98 和 0.02 就是权重系数意思是“我更相信陀螺仪的短期变化但长期慢慢往加速度计修正”。这个系数不是随便拍的它对应一个时间常数决定了修正的快慢。系数太偏向陀螺仪漂移修正不过来太偏向加速度计振动噪声全进来了。2.2 卡尔曼滤波到底比互补滤波强在哪互补滤波够用但它是“手工调权重”没有严格的数学最优性。卡尔曼滤波则是在统计意义上做最优估计它同时建模了过程噪声陀螺仪漂移、模型误差和观测噪声加速度计噪声然后根据两者的置信度动态分配权重。实际飞控里用得最多的是扩展卡尔曼滤波EKF因为姿态动力学是非线性的。Pixhawk 的 EKF2 就是典型代表它不只估姿态还把位置、速度、陀螺仪零偏、加速度计零偏、磁力计偏置、甚至风速都塞进状态向量里一起估。状态维度能到二十多维运算量不小所以早期飞控跑不动得靠专门的浮点运算单元。这里有个很多人忽略的点EKF 的调参比互补滤波难得多。互补滤波你调一个系数就行EKF 要调过程噪声协方差矩阵 Q 和观测噪声协方差矩阵 R。Q 调大了滤波器更信观测响应快但噪声大Q 调小了更信模型平滑但滞后。我见过不少人直接抄别人的 Q/R 参数结果换一套机架、换一批桨叶就完全不对了因为振动特性变了观测噪声的统计特性也变了。实操心得如果你只是飞着玩互补滤波完全够调参直观。如果你要做自动航线、视觉定位、载重飞行老老实实上 EKF并且一定要用飞控自带的传感器校准流程把零偏和尺度因子标定准否则 EKF 再强也救不回来。2.3 磁力计和气压计偏航与高度的定海神针加速度计和陀螺仪能定俯仰和横滚但偏航角机头朝向它们定不了因为重力方向对偏航不敏感。这时候需要磁力计提供地磁北向参考。磁力计最怕的是电机电流产生的磁场干扰所以外置 GPS/罗盘模块通常要架高远离动力线。校准的时候要远离铁磁物体做完整的椭球拟合校准把硬铁和软铁偏差都消掉。气压计负责定高。它测的是大气压换算成高度。问题是气压随天气变化而且桨叶旋转会在机身周围产生低压区导致气压读数偏低。所以定高算法里通常要做气压基线动态估计把缓慢变化的天气因素和快速的桨叶扰动分开。Pixhawk 里有个EKF2_HGT_MODE参数可以选择用气压计、GPS、测距仪还是视觉来定高多传感器融合的时候优先级和权重都要配好。3. 控制律从“想要什么姿态”到“电机转多快”3.1 串级 PID 为什么是标配姿态解算出来的是“当前姿态”而你要的是“目标姿态”。两者之间的误差怎么变成电机指令最经典也最实用的方案是串级 PID。外环是角度环输入是目标角度和当前角度的差输出是期望角速度。内环是角速度环输入是期望角速度和陀螺仪测到的实际角速度的差输出是力矩指令。为什么非要串两级因为角度环响应慢、抗扰差角速度环响应快、能抑制振动和突发扰动。外环负责“往哪转”内环负责“转得多快多稳”。用生活类比你开车想保持车道居中眼睛看的是“车离车道线多远”角度环但手上打方向盘的动作其实是在控制“车头转动的角速度”角速度环。如果只看距离直接打方向车会画龙加上角速度内环方向就稳了。PID 三个项各有分工。P 是比例误差越大纠正越猛I 是积分消除稳态误差比如飞机总往一边偏I 项会慢慢累积把它拉回来D 是微分预测误差变化趋势提前刹车防止超调。多旋翼里 D 项特别重要因为机架有惯性没有 D 就会来回振荡。3.2 参数整定的顺序和踩坑记录调 PID 有个铁律先内环后外环先 P 后 D 再 I。我见过新手一上来三个参数一起调结果飞机抖得像筛糠根本分不清是哪个参数的问题。具体步骤我一般这么走把外环角度环的 P 设小I 和 D 归零内环角速度环也只留小 P。慢慢加内环 P直到飞机对扰动有快速响应但还没开始高频抖动。这时候用手轻推机臂能感觉到飞控在“顶”你。加内环 D抑制 P 带来的超调和振荡。D 太大会引入高频噪声表现为电机发热、声音刺耳。内环稳了之后加外环 P直到飞机能快速回到水平但不过冲。最后加一点点 I消除悬停时的缓慢漂移。I 太大会导致“积分饱和”飞机在大误差后恢复特别慢甚至振荡。这里有个隐藏坑桨叶不平衡和机架共振会伪装成 PID 问题。如果你的飞机在某个特定油门区间抖得厉害其他油门正常那大概率是共振不是 PID。这时候调 PID 是治标不治本得先做动平衡、加减震垫。3.3 从 PID 到更高级的控制LQR、MPC 和自适应PID 够用但它的局限在于“一套参数打天下”。载重变化、电池电压下降、风速变化都会让最优参数漂移。所以进阶方案有LQR线性二次型调节器基于状态空间模型能同时优化多个状态量的控制效果。需要比较准的动力学模型适合机型固定的场景。MPC模型预测控制在每一步预测未来若干步的状态滚动优化控制量。能显式处理约束比如电机转速上限、姿态角限制但算力要求高一般跑在机载计算机上而不是飞控 MCU 上。自适应控制在线估计模型参数并调整控制律能应对载重变化和部分故障。实现复杂调不好容易发散。我的建议是先把串级 PID 吃透再考虑高级算法。很多所谓“高级控制”在实际飞行里的表现未必比调好的 PID 强多少但调试成本和炸机风险高得多。4. 混控与电机分配算法落地的最后一公里4.1 四轴、六轴、八轴的混控矩阵差异控制律算出来的是三个力矩指令俯仰、横滚、偏航加一个总推力指令。这四个量要分配到每个电机上靠的是混控矩阵。以最常见的 X 型四轴为例四个电机两两对角同向。俯仰力矩靠前后电机差速横滚靠左右差速偏航靠对角电机差速利用桨叶反扭矩。写成矩阵大概是这样电机1(右前) 油门 俯仰 横滚 - 偏航 电机2(左后) 油门 俯仰 - 横滚 偏航 电机3(左前) 油门 - 俯仰 横滚 偏航 电机4(右后) 油门 - 俯仰 - 横滚 - 偏航六轴和八轴的矩阵更复杂但原理一样。关键点在于混控矩阵的符号和电机编号必须和实际机架完全对应。我见过有人把电机顺序接错结果一解锁飞机就疯狂自旋因为偏航修正方向反了。4.2 电机饱和与推力分配优化混控有个现实约束电机转速有上限也有下限一般不能停转否则失去控制。当总推力需求很高、同时又要大姿态修正时某些电机可能被要求输出超过 100% 的推力这就是饱和。饱和一旦发生姿态控制就失效了飞机可能直接翻。解决办法是推力分配优化在满足总推力和力矩需求的前提下尽量让各电机输出均衡避免个别电机顶到上限。简单做法是先算需求如果发现最大输出超过上限就整体降低总推力指令优先保姿态。更高级的做法是解一个带约束的优化问题但飞控 MCU 上一般用简化的优先级策略。注意电池电压下降时同样的油门指令对应的实际推力会变小。所以好的飞控会做电压补偿根据实时电压调整混控输出保证操控手感一致。这个补偿系数没调好低电量时飞机就会变得“软绵绵”。5. 位置与高度控制从“稳得住”到“飞得准”5.1 高度控制的串级结构高度控制也是串级外环是高度环输入目标高度和当前高度差输出期望爬升率内环是速度环垂直方向输入期望爬升率和实际垂直速度差输出总推力修正。这里的气压计噪声和 GPS 高度噪声要处理好。气压计短期准但会漂GPS 高度长期准但噪声大、更新慢。EKF 会把两者融合输出一个平滑的高度估计。定高模式下如果发现飞机像坐电梯一样上下晃通常是高度环的 P 太大或者 D 太小也可能是气压计受桨叶气流干扰太严重需要加海绵罩或者改安装位置。5.2 水平位置控制与路径规划的关系水平位置控制依赖 GPS、光流、视觉等定位源。控制结构和高度类似位置环输出期望速度速度环输出期望姿态角然后交给姿态环执行。这里和路径规划直接相关。路径规划算法比如 A*、RRT、人工势场给出的是一个个航点或一条期望轨迹飞控的位置环负责跟踪这条轨迹。如果位置环响应太慢飞机切弯就会切得很大甚至飞出安全边界如果响应太快又会在航点附近振荡。所以做自动航线时航点间距、飞行速度、转弯半径这些参数要和位置环的带宽匹配。我个人的经验是先把定点悬停调稳再调航线跟踪。定点都飘的飞机航线一定飞得稀烂。定点悬停时观察飞机在 GPS 坐标里的漂移范围如果超过一两米先查罗盘干扰和 GPS 星况别急着调位置环 PID。5.3 视觉感知与视觉定位的介入点现在很多无人机用视觉做定位和避障。视觉里程计输出的位置和速度可以替代或补充 GPS。视觉的好处是室内也能用缺点是受光照和纹理影响大算力消耗高。视觉定位接入飞控时关键是坐标系对齐和时间同步。视觉算法算出来的位置是在相机坐标系下的要转到机体坐标系再转到导航坐标系。时间戳对不齐高速飞行时位置估计就会滞后表现为飞机“反应慢半拍”。所以视觉和飞控之间的时间同步精度最好做到毫秒级。6. 仿真与硬件在环不上天也能调算法6.1 软件在环仿真的价值与局限调飞控算法最怕的就是炸机。软件在环仿真SITL让你在电脑里跑一个虚拟飞机飞控固件原封不动地跑只是传感器数据来自仿真模型。这样你可以随便试参数、试极端情况不用担心摔飞机。常用的仿真环境有 Gazebo、jMAVSim 等配合 MAVLink 协议和地面站通信。你可以模拟 GPS 丢失、磁力计干扰、电机失效等故障观察飞控的应对。局限在于仿真模型再准也和真机有差距尤其是振动、气流、电机响应非线性这些仿真里很难完全复现。6.2 硬件在环仿真的接线与注意事项硬件在环HIL是把真实飞控板接上传感器数据由仿真机注入执行机构信号也回传给仿真机。这样飞控跑的是真实固件、真实运算只是“身体”是虚拟的。接线时要注意电平匹配和接口协议。Pixhawk 一般通过串口或 USB 和仿真机通信注入传感器数据需要走特定的 MAVLink 消息。常见坑是仿真机的实时性不够导致数据注入有抖动飞控以为传感器在抖控制效果就失真了。所以 HIL 对仿真机的性能有一定要求最好用实时内核或者至少保证高优先级线程。6.3 从仿真到真机的参数迁移仿真里调好的参数搬到真机上通常还要再调。因为真机的振动谱、电机响应、桨叶效率都和模型不同。我的做法是仿真里确定参数的大致范围和趋势真机上从小往大微调。特别是 D 项仿真里往往可以给得比较大真机上因为噪声大D 要给得保守些。7. 那些没人告诉你但一定会遇到的坑7.1 振动是飞控算法的头号敌人再好的算法遇到烂机架也白搭。振动会让加速度计数据失真EKF 估计发散PID 的 D 项疯狂放大噪声。判断振动是否超标可以看飞控日志里的加速度计频谱如果某个频率的峰值特别高说明机架在那个频率共振。解决办法做桨叶动平衡、电机做动平衡、飞控用减震胶安装、机架加强刚度避开共振频率。这些机械层面的功夫比调任何参数都管用。7.2 罗盘干扰的排查链路飞机自旋、偏航漂移、定点画圈很多时候是罗盘被干扰。排查顺序我一般这么走先看罗盘校准是否通过再看外置罗盘是否远离动力线然后看电机电流变化时罗盘读数是否跟着跳。如果跳说明磁场干扰严重要么换罗盘位置要么加屏蔽要么在算法里做电流补偿。7.3 参数不是越“高级”越好我见过有人一上来就开 EKF、开视觉、开 MPC结果基础 PID 都没调好飞机连稳定悬停都做不到。飞控算法是层层叠加的底层不稳上层全是空中楼阁。老老实实从姿态环开始一层一层往上调才是最快的路。8. 写在最后的一点个人体会飞控算法这东西看公式觉得不难真正上手调才知道细节全是坑。我自己的习惯是每次飞行都开日志飞完回来看姿态跟踪误差、电机输出、振动频谱比凭感觉调参数靠谱得多。另外别迷信别人的参数表机架、电机、桨叶、重量任何一项变了参数都得重新过一遍。把姿态解算、串级 PID、混控分配这三块吃透大部分飞行问题你都能自己定位到根因而不是到处问“为什么我的飞机抖”。