1. 飞控算法到底在飞控里扮演什么角色1.1 从一次炸机说起为什么算法比硬件更致命我玩无人机前后差不多八年从最早的KK飞控板一路用到Pixhawk、大疆的N3和A3中间炸过的机器少说也有十几架。真正让我意识到飞控算法分量的是早年一次四轴在悬停时突然翻跟头的事故。当时硬件没坏、电机没烧、桨也没断问题出在姿态解算的互补滤波参数上——陀螺仪积分漂移没被加速度计拉回来姿态角在十几秒内慢慢偏了三十多度飞控以为自己在水平状态实际上机身已经歪了PID还在拼命修正一个根本不存在的偏差最后直接失控。这件事之后我才认真去啃飞控算法的底层逻辑。很多人一提飞控就想到Pixhawk、想到大疆的飞控板觉得买个好硬件就万事大吉。实际上飞控硬件只是躯干算法才是大脑和小脑。同样一块Pixhawk 4刷不同的固件、调不同的参数飞行品质能差出天壤之别。飞控算法要解决的核心问题就三件事我现在是什么姿态状态估计、我该往哪飞制导与导航、我怎么飞过去控制律。这三件事串起来才构成一个完整的飞控算法闭环。这篇文章我打算把飞控算法从底层到上层拆开讲一遍包括姿态解算、PID控制、混控输出、导航制导这几块。适合刚入门想搞明白飞控内部逻辑的朋友也适合已经在飞但总调不好参数、想从原理层面找答案的人。我不会只讲概念会尽量把每个环节的参数怎么算、为什么这么选、踩过哪些坑都写出来。1.2 飞控算法的三层结构感知、决策、执行飞控算法从架构上可以分成三层这个分层方式在Pixhawk的PX4固件和ArduPilot里都能看到影子。最底层是感知层负责从IMU、磁力计、气压计、GPS这些传感器拿到原始数据然后做姿态解算和位置估计。姿态解算的输出是机体的滚转、俯仰、偏航三个角以及对应的角速度。位置估计输出的是三维位置和速度。这一层的核心算法是互补滤波、卡尔曼滤波以及后来的Mahony滤波和Madgwick滤波。中间层是决策层也就是制导与导航。它根据你的遥控器输入或者航线任务算出期望的姿态角和期望的推力。比如你打杆让飞机往前飞决策层会把它翻译成“期望俯仰角10度、期望推力60%”而不是直接告诉电机转多快。最上层是执行层也就是控制律加混控输出。控制律通常是串级PID外环姿态角、内环角速度。外环根据期望姿态和实际姿态的偏差算出期望角速度内环再根据期望角速度和实际角速度的偏差算出三个轴的力矩需求。最后混控器把力矩需求分配到四个或更多电机上变成具体的PWM或DShot信号。这三层的关系可以用一个生活化的类比感知层是眼睛和内耳决策层是大脑决定往哪走执行层是小脑和肌肉负责具体动作。任何一层出问题飞行的表现都会异常但异常现象往往相似所以排查时要从现象反推到具体层级这个后面会细讲。2. 姿态解算飞控算法的地基2.1 为什么加速度计和陀螺仪必须互补姿态解算的输入主要是陀螺仪和加速度计。陀螺仪测的是角速度积分之后得到角度短期精度高、响应快但积分会累积漂移时间一长角度就偏了。加速度计测的是重力方向静态下能直接算出滚转和俯仰角长期稳定不漂移但振动一大就全是噪声而且它算不出偏航角。这两者的特性正好互补陀螺仪管短期、加速度计管长期。互补滤波就是用一个高通滤波器让陀螺仪积分值通过用一个低通滤波器让加速度计值通过然后加权融合。公式大概是这样的angle alpha * (angle gyro * dt) (1 - alpha) * accel_angle其中alpha通常取0.98左右dt是采样周期。这个alpha的选取很关键越大越信任陀螺仪响应快但漂移抑制弱越小越信任加速度计稳定但振动敏感。我一般会在悬停时看姿态角的漂移速度来调如果十秒内漂了超过两度就把alpha调小一点。2.2 卡尔曼滤波和Mahony滤波怎么选互补滤波够用但遇到振动大或者机动剧烈的场景就力不从心了。这时候就得上卡尔曼滤波或者Mahony滤波。卡尔曼滤波是概率框架下的最优估计它把姿态解算建模成一个状态空间问题用过程噪声和观测噪声的协方差矩阵来动态调整信任权重。优点是理论上最优缺点是要调Q和R两个噪声矩阵调不好反而比互补滤波还差。我在Pixhawk上用过EKF2默认参数对大多数机架都够用但如果机架振动特别大需要把陀螺仪的噪声参数调大否则滤波器会过度信任陀螺仪导致漂移。Mahony滤波是互补滤波的非线性版本用四元数表示姿态通过PI控制器来修正陀螺仪的漂移。它的计算量比卡尔曼小效果又比线性互补滤波好所以在开源飞控里用得很多。PX4早期版本用的就是Mahony后来才换成EKF2。如果你是自己写飞控我建议先从Mahony入手代码量不大理解起来也直观。2.3 磁力计和气压计在姿态解算里的角色磁力计负责提供偏航角的绝对参考。没有磁力计偏航角只能靠陀螺仪积分几分钟就能漂到找不着北。但磁力计特别容易被干扰电机电流、电源线、甚至机架上的铁螺丝都会影响它。我在实际使用中发现磁力计校准一定要在远离电机和电源线的位置做而且校准完之后要把磁力计安装位置在飞控参数里填对否则融合出来的偏航角会有固定偏差。气压计负责高度估计但它对气流特别敏感。桨叶旋转产生的气流会在机身内部形成乱流导致气压读数跳动。解决办法是用海绵把气压计包起来或者把飞控装在减震座上。我试过在定高模式下气压计读数跳变导致飞机上下窜动后来把气压计用海绵裹住问题就解决了。3. PID控制飞控算法里最容易被神化也最容易被误解的部分3.1 串级PID的结构和每一环的作用飞控里的PID几乎都是串级结构外环是角度环内环是角速度环。为什么不用单环因为单环角度PID的输出直接给力矩响应慢而且容易超调。串级之后外环角度PID的输出作为内环角速度PID的期望值内环响应快能及时抑制扰动。具体来说外环角度PID的输入是期望角度和实际角度的偏差输出是期望角速度。内环角速度PID的输入是期望角速度和实际角速度的偏差输出是力矩需求。这里有个关键点外环通常只用P不用I和D。因为角度偏差的积分意义不大而微分又容易引入噪声。内环则P、I、D都用P负责响应速度I负责消除稳态误差D负责抑制超调。我调机的顺序一般是先调内环角速度再调外环角度。内环调好了外环的P值可以给得比较大响应会很快。如果内环没调好外环怎么调都别扭。3.2 参数整定的实操方法和计算过程PID参数整定没有万能公式但有一套可复现的流程。我一般用试凑法结合Ziegler-Nichols的临界比例度法。先调内环角速度的P。把I和D设为零P从一个小值开始慢慢加直到飞机在悬停时出现等幅振荡。记录下这个临界P值和振荡周期T。然后按照Ziegler-Nichols的经验公式P 0.6 * KcI 2 * P / TD P * T / 8其中Kc是临界P值T是振荡周期。这套公式给的是起点实际还要微调。我通常会把算出来的D再加大20%左右因为飞控的角速度噪声比较大需要更强的微分抑制。外环角度的P值一般从内环P值的十分之一开始试慢慢加。外环P太大飞机会快速来回摆太小则响应迟钝。我一般调到打杆后飞机能快速响应但不过冲就停。3.3 滤波和限幅PID之外必须做的两件事PID的输出不能直接给电机中间必须加滤波和限幅。滤波是为了抑制D项的噪声常用的是低通滤波或者陷波滤波。陷波滤波特别有用因为机架的共振频率通常在100到200赫兹之间用陷波滤波把这个频段滤掉能显著改善飞行品质。我在Pixhawk上会先做一次频率扫描找到机架的共振峰然后把陷波滤波的中心频率设在那里。限幅是为了防止PID输出过大导致电机饱和。电机饱和之后飞控的修正能力就没了飞机会失控。所以每个轴的力矩输出都要限幅通常限制在最大力矩的80%左右。另外积分项也要限幅防止积分饱和导致超调。4. 混控输出从力矩到电机转速的最后一公里4.1 四轴和八轴的混控矩阵怎么推导混控器的作用是把三个轴的力矩需求加上油门需求分配到每个电机上。四轴的混控矩阵是一个4x4的矩阵输入是滚转、俯仰、偏航、油门输出是四个电机的推力。以常见的X型四轴为例电机1在右前电机2在左后电机3在左前电机4在右后。滚转力矩由左右电机差速产生俯仰力矩由前后电机差速产生偏航力矩由对角电机差速产生。混控矩阵大概是这样的motor1 throttle - roll pitch yaw motor2 throttle roll - pitch yaw motor3 throttle roll pitch - yaw motor4 throttle - roll - pitch - yaw八轴的混控矩阵更复杂但原理一样。我建议自己写飞控的话先用四轴把混控逻辑跑通再扩展到八轴。混控矩阵的系数要根据电机的实际推力和力臂来标定不能随便填。4.2 电机选型和混控的关系混控的输出最终要落到电机上所以电机选型直接影响混控效果。电机的KV值、桨叶尺寸、电池电压共同决定了最大推力。如果电机推力不够混控输出饱和飞控就失去了修正能力。我一般会先估算整机重量然后按推重比2:1来选电机和桨。比如整机重2公斤那四个电机加起来最大推力要4公斤每个电机1公斤。然后查电机的推力表找到能满足这个推力的KV值和桨叶组合。这里有个经验低KV配大桨效率高但响应慢高KV配小桨响应快但效率低。航拍机选低KV大桨穿越机选高KV小桨。4.3 DShot和PWM的区别以及怎么选混控输出的信号协议有PWM、OneShot、DShot几种。PWM是模拟信号精度低、延迟大现在基本淘汰了。OneShot是PWM的改进版延迟小一些。DShot是数字信号精度高、延迟小、还支持双向遥测能回传电机转速。我现在的机器全部用DShot600延迟比PWM低了一个数量级而且双向DShot能回传转速对调参帮助很大。如果你还在用PWM建议至少升级到DShot300。升级之后你会发现飞机的响应明显变快尤其是做快速机动的时候。5. 导航与制导从手动飞行到自主航线5.1 位置估计和传感器融合导航的前提是知道自己在哪。位置估计靠GPS、光流、视觉、气压计、IMU融合。GPS提供绝对位置但更新率低、精度差光流和视觉提供相对位置但需要好的光照和纹理。融合这些传感器通常用扩展卡尔曼滤波。我在做航道巡查系统的时候发现GPS在树林附近漂移特别大后来加了光流模块在GPS信号差的时候自动切换位置估计就稳多了。光流模块的安装要注意离地高度和镜头清洁否则算出来的速度全是错的。5.2 路径规划算法的选择路径规划算法有A*、Dijkstra、RRT、蚁群算法等。A*适合静态已知地图RRT适合高维空间和动态障碍蚁群算法适合多目标优化但计算量大。我在实际项目里用得最多的是A*因为计算量小、路径最优性好。但A的缺点是路径不够平滑飞机飞起来会一顿一顿的。解决办法是用贝塞尔曲线或者B样条做后处理把折线路径平滑成曲线。另外A的启发函数要选好曼哈顿距离适合网格地图欧几里得距离适合连续空间。5.3 制导律的设计制导律负责把路径变成飞机的期望姿态和期望速度。最简单的制导律是纯追踪飞机始终朝向路径上的前视点飞。前视距离的选择很关键太短会震荡太长会切弯。我一般设前视距离为飞机速度的两倍。更高级的制导律有L1制导和非线性制导。L1制导用一个虚拟圆弧来跟踪路径对风扰动的抑制更好。我在做果实采摘执行末端的时候因为飞机要悬停在果树前对位置精度要求很高就用了L1制导效果比纯追踪好很多。6. 常见问题与排查技巧实录6.1 飞控算法相关的典型故障速查表现象可能原因排查方法解决办法悬停时缓慢漂移姿态解算漂移看姿态角是否随时间偏移调小互补滤波alpha或换Mahony打杆后快速震荡内环P太大或D太小降低P值看是否改善重新整定内环PID飞行中突然翻跟头陀螺仪饱和或滤波失效看日志里陀螺仪是否超量程换低量程陀螺仪或加陷波滤波定高模式上下窜动气压计受气流干扰看气压读数是否跳动海绵包裹气压计或换减震座航线飞行切弯前视距离太短看日志里横向偏差加大前视距离电机发热严重混控系数不对或电机选型不当看电机电流是否超额定重新标定混控矩阵或换电机6.2 日志分析飞控算法调试的必备技能飞控日志是排查算法问题的最重要工具。Pixhawk的日志里能看到姿态角、角速度、PID各环输出、电机输出、传感器原始数据。我一般先看姿态角和角速度的跟踪情况如果期望值和实际值差很多说明内环没调好。然后看PID各环的输出如果某一环经常饱和说明参数给大了。有个技巧把日志导出来用Python画图比在Mission Planner里看直观得多。我常用matplotlib画姿态跟踪曲线一眼就能看出是相位滞后还是幅值不够。6.3 硬件在环仿真不上天也能调算法硬件在环仿真就是让飞控板接上仿真器在电脑里模拟飞行。这样不用真机就能调算法安全又高效。我一般用jMAVSim或者Gazebo做仿真把Pixhawk通过USB接上飞控以为自己在飞真机实际上是在仿真环境里。仿真的好处是可以反复复现同一个场景比如阵风、GPS丢失、电机失效。我在做无人机视觉感知的时候就在仿真里跑了上百次降落测试把算法调稳了才上真机。仿真和真机的差距肯定有但趋势是一致的能省很多炸机成本。7. 我个人的调参心得和几个实用建议调飞控算法这件事我最大的体会是不要迷信默认参数也不要迷信别人的参数。每架飞机的重量分布、电机响应、桨叶特性都不一样参数必须自己调。但调参要有章法先调内环再调外环先调P再调I和D每次只改一个参数改完记录现象。另外日志一定要看。很多人调参靠感觉觉得飞机不抖了就完事。实际上有些问题在悬停时看不出来一做机动就暴露。日志能让你看到悬停时看不到的东西。最后分享一个小技巧如果飞机在某个油门区间特别抖其他区间正常大概率是机架共振。这时候不要急着改PID先做频率扫描找到共振频率加陷波滤波。我有一架八轴就是在40%油门时抖得厉害加了陷波滤波之后立马就稳了。飞控算法这东西入门容易精通难。但只要你把姿态解算、PID、混控这三块吃透大部分问题都能自己解决。剩下的就是多飞、多看日志、多总结。