1. 飞控算法到底在飞控里扮演什么角色很多人第一次接触无人机注意力都在电机、桨叶、机架这些看得见摸得着的东西上觉得飞控不过是一块电路板。但真正飞过一段时间、炸过几次机之后你就会发现同样一套硬件换一套飞控算法飞行手感能差出十万八千里。飞控算法就是无人机的大脑它决定了这台机器是稳如老狗还是飘得像喝醉了酒。先把概念理清楚。飞控全称飞行控制器硬件上通常是一块集成了主控芯片、陀螺仪、加速度计、气压计、磁力计的板子比如大家熟悉的Pixhawk系列。而飞控算法是跑在这块板子上的软件逻辑它要完成的事情说白了就三件知道自己现在什么姿态知道你想让它变成什么姿态然后算出电机该怎么转才能从当前姿态过渡到目标姿态。这三件事分别对应状态估计、控制律计算和混控输出。为什么飞控算法这么关键因为无人机本质上是一个欠驱动系统。四轴无人机只有四个电机却要在三维空间里控制六个自由度——三个位置加三个姿态。你没法直接控制每一个自由度只能通过调整四个电机的转速差间接地影响姿态再通过姿态倾斜产生的水平分力来影响位置。这就好比你在一个光滑的冰面上脚下只有四个可以独立调速的轮子想让身体往某个方向移动得先倾斜身体让轮子的推力产生水平分量。这个过程中算法要实时解算、实时补偿稍有延迟或者算错机器就翻了。飞控算法的核心任务可以拆成一条完整的链路来看。传感器采集原始数据经过滤波和融合得到姿态估计姿态估计进入位置环和姿态环的串级控制器控制器输出期望的力矩和推力混控器把力矩和推力分配到四个电机上电调驱动电机转动电机转速变化产生新的姿态传感器再采集新的数据。这是一个闭环循环频率通常在几百赫兹到几千赫兹。任何一个环节的算法出了问题整个闭环就会失稳。适合谁来深入了解这部分内容如果你只是买个成品机拍拍风景那确实不需要懂太多默认参数飞就行。但如果你要自己组装调参、要做行业应用比如航道巡查或者果实采摘、要搞硬件在环仿真、甚至要自己写飞控固件那飞控算法就是绕不过去的坎。下面我就按实际工程中的思路把飞控算法从整体设计到具体实现拆开来讲。2. 飞控算法的整体架构与设计思路2.1 为什么采用串级PID而不是单环控制刚入门的朋友经常会问为什么不能直接用一个PID把姿态控制住就完了答案在于无人机的动力学特性。姿态角和角速度是两个不同时间尺度的量。角速度变化极快姿态角变化相对慢。如果你只用一个姿态环PID等姿态角偏差大了再去修正电机响应已经滞后了很容易振荡。所以工程上普遍采用串级结构外环是角度环输入是期望角度和估计角度输出是期望角速度内环是角速度环输入是期望角速度和陀螺仪测量的角速度输出是期望力矩。这样设计的好处是内环响应快能及时抑制扰动外环负责跟踪目标角度。类比一下你开车的时候眼睛看的是前方道路的偏差这是外环脚踩油门和刹车的力度是根据车速变化来调整的这是内环。如果只看道路偏差直接猛踩油门车肯定一顿一顿的。串级PID的参数整定也有讲究。一般先调内环角速度环把P和D调好让角速度跟踪快速且不振荡然后再调外环角度环通常只需要P就够了偶尔加一点I消除静差。内环的带宽要明显高于外环一般至少三到五倍否则两个环会互相干扰。2.2 状态估计从原始传感器到可信姿态飞控算法里最容易被低估的就是状态估计。很多人调参调不好以为是PID的问题实际上是姿态估计不准。陀螺仪测量角速度短期准但会漂移加速度计测量重力方向长期准但振动噪声大磁力计测量地磁方向能修正偏航但容易被干扰。这三者必须融合。最经典的融合算法是互补滤波和卡尔曼滤波。互补滤波简单来说就是高通滤波陀螺仪、低通滤波加速度计然后加权相加。它的优点是计算量小适合资源有限的飞控。卡尔曼滤波则是在状态空间里做最优估计理论上更优但计算量大而且需要建立准确的噪声模型。实际工程中很多开源飞控用的是扩展卡尔曼滤波或者误差状态卡尔曼滤波。这里有个实操心得加速度计的数据一定要先做低通滤波再进融合否则电机振动会直接污染姿态估计导致无人机在空中高频抖动。我见过太多人飞机抖得厉害以为是PID的D太大其实是振动没滤干净。正确的做法是先做机械减振比如用减振海绵安装飞控然后在软件里把加速度计的低通截止频率设到30到50赫兹左右具体看机架和桨叶的振动特性。2.3 控制分配混控矩阵的物理意义姿态控制器算出的是三个轴的期望力矩和总推力但电机只能接受四个转速指令。从力矩到转速的映射就是混控。对于常见的X型四轴混控矩阵可以写成电机1 油门 滚转 俯仰 - 偏航 电机2 油门 - 滚转 俯仰 偏航 电机3 油门 - 滚转 - 俯仰 - 偏航 电机4 油门 滚转 - 俯仰 偏航这个矩阵的每一列对应一个控制通道每一行对应一个电机。正负号取决于电机的旋转方向和安装位置。混控矩阵的设计要保证四个电机在产生期望力矩的同时总推力不变。如果矩阵设计错了你会发现打杆的时候无人机不是倾斜而是直接掉高度。对于六轴、八轴或者异型机架混控矩阵的维度会变化但原理一样。工程上通常用伪逆法来求解控制分配尤其是当电机数量多于控制通道时可以实现容错控制。比如八轴坏了一个电机剩下的七个电机通过重新分配仍然能维持姿态这就是控制分配算法的价值。3. 核心算法细节与实操要点3.1 姿态解算中的坐标系变换飞控算法里所有的计算都离不开坐标系。机体坐标系固定在无人机上原点在重心X轴指向机头Y轴指向右侧Z轴指向下方。地理坐标系通常用东北天或者北东地。姿态解算的本质就是求出机体坐标系相对于地理坐标系的旋转关系通常用四元数或者旋转矩阵表示。为什么用四元数而不是欧拉角因为欧拉角在俯仰角接近正负90度时会出现万向节死锁而且三角函数计算量大。四元数只有四个参数没有奇点插值和归一化都方便。但四元数不直观调试的时候可以把四元数转成欧拉角显示出来看。坐标系变换的实操要点加速度计测量的是机体坐标系下的重力分量要用来修正姿态必须先根据当前姿态估计把重力向量转到机体坐标系然后和测量值做叉积得到姿态误差。这个误差再通过PI控制器补偿到陀螺仪的角速度上。这就是Mahony滤波或者Madgwick滤波的核心思想。代码实现的时候注意归一化加速度计和四元数都要归一化否则数值会漂。3.2 PID控制器的离散化实现飞控算法跑在数字芯片上PID必须离散化。位置式PID和增量式PID各有适用场景。位置式PID输出的是控制量的绝对值适合执行机构需要绝对位置的场合增量式PID输出的是控制量的增量适合积分饱和风险高的场合。飞控里姿态环通常用位置式PID但积分项要做限幅和抗饱和处理。离散化的时候积分项用累加代替连续积分微分项用差分代替连续微分。这里有个坑微分项对噪声极其敏感。如果直接用当前误差减上次误差除以周期噪声会被放大。所以工程上要么用不完全微分要么对微分项做低通滤波要么直接用陀螺仪的角速度作为微分项的替代。实际上角速度环的输入本身就是陀螺仪测量的角速度所以角速度环的P项就相当于角度环的D项这也是串级PID的一个巧妙之处。参数整定的经验值对于常见的250到450轴距的四轴角速度环P一般在0.1到0.3之间I在0.01到0.05之间D在0.001到0.01之间。角度环P一般在3到8之间。当然这只是一个起点具体要看电机KV、桨叶尺寸、机架重量和重心分布。整定的时候先用默认参数悬停然后逐步增加P直到出现高频振荡再回退百分之三十然后加D抑制振荡最后加I消除静差。3.3 位置环与高度环的控制逻辑姿态控制稳了之后才能谈位置控制。位置环的输入是期望位置和估计位置输出是期望姿态角。也就是说位置控制器通过倾斜机身来产生水平加速度从而移动到目标位置。这里有个关键约束位置环的输出必须限幅否则期望姿态角过大无人机可能翻掉。一般水平位置环输出的最大倾角限制在30到45度之间。高度环通常用气压计或者激光测距仪作为反馈。气压计受气流和温度影响大悬停时高度会漂。实操中要么用激光定高要么用光流加超声波要么用视觉定位。高度环的PID参数比姿态环温和得多因为高度变化的惯性大。P一般在1到3之间I在0.1到0.5之间D很小或者不用。位置环还有一个常见问题GPS定位漂移。在GPS模式下悬停无人机会在几米范围内缓慢漂移这是正常的因为民用GPS精度就是米级。要更稳的悬停需要RTK或者视觉定位。如果做航道巡查这类应用路径跟踪算法要在位置环外面再套一层制导律比如L1制导或者纯追踪算法把航线偏差转化为期望位置。4. 从零实现一个简化版飞控算法的完整流程4.1 硬件选型与开发环境搭建要动手实现飞控算法不一定非得上真机。我建议先用硬件在环仿真或者纯软件仿真验证算法。硬件在环仿真的做法是飞控板通过串口或者USB连接电脑电脑上跑一个飞行动力学模型飞控板输出电机指令给模型模型计算出新的姿态和位置再反馈给飞控板。这样可以在不炸机的情况下测试算法。硬件选型上Pixhawk系列是绕不开的因为它生态成熟有大量的开源固件和文档。但如果你想自己写算法我建议用STM32F4或者F7系列的开发板搭配MPU6000或者ICM20602惯性测量单元。开发环境用STM32CubeIDE或者Keil配合FreeRTOS做任务调度。传感器读取用SPI或者I2C控制循环放在一个高优先级任务里频率设到1kHz。4.2 传感器驱动与数据预处理第一步是把传感器数据读出来。陀螺仪和加速度计通常通过SPI读取磁力计通过I2C。读取的时候要注意量程设置。陀螺仪量程一般设正负2000度每秒加速度计量程设正负8g或者16g。量程太小会饱和太大分辨率不够。数据预处理包括校准和滤波。校准分两步零偏校准和尺度因子校准。零偏校准就是静止时采集几千个样本取平均作为零偏值后续测量值减去零偏。尺度因子校准需要转台一般爱好者可以跳过用出厂校准值就行。滤波方面陀螺仪用低通滤波截止频率设到80到100赫兹加速度计截止频率设到30到50赫兹。如果振动大还可以加陷波滤波器针对电机转速对应的频率做衰减。4.3 姿态解算代码实现下面给一个基于Mahony滤波的姿态解算核心代码片段用C语言写的方便移植到STM32上// 四元数更新 void MahonyUpdate(float gx, float gy, float gz, float ax, float ay, float az, float dt) { float norm; float vx, vy, vz; float ex, ey, ez; // 归一化加速度计测量值 norm sqrt(ax*ax ay*ay az*az); ax / norm; ay / norm; az / norm; // 根据当前四元数估计重力方向 vx 2*(q1*q3 - q0*q2); vy 2*(q0*q1 q2*q3); vz q0*q0 - q1*q1 - q2*q2 q3*q3; // 计算误差 ex ay*vz - az*vy; ey az*vx - ax*vz; ez ax*vy - ay*vx; // PI补偿陀螺仪零偏 gx twoKp*ex twoKi*ex*dt; gy twoKp*ey twoKi*ey*dt; gz twoKp*ez twoKi*ez*dt; // 四元数积分 q0 (-q1*gx - q2*gy - q3*gz)*0.5f*dt; q1 (q0*gx q2*gz - q3*gy)*0.5f*dt; q2 (q0*gy - q1*gz q3*gx)*0.5f*dt; q3 (q0*gz q1*gy - q2*gx)*0.5f*dt; // 归一化四元数 norm sqrt(q0*q0 q1*q1 q2*q2 q3*q3); q0 / norm; q1 / norm; q2 / norm; q3 / norm; }这段代码里twoKp和twoKi是滤波增益一般twoKp取1.0左右twoKi取0.001左右。调的时候先调twoKp让姿态收敛快且不振荡再调twoKi消除陀螺仪零偏。注意dt是控制周期单位是秒如果控制频率是1kHzdt就是0.001。4.4 串级PID与控制分配代码实现姿态解算得到欧拉角之后进入串级PID。外环角度环计算期望角速度内环角速度环计算期望力矩。代码结构大概是这样// 外环角度环 float roll_target 0; // 期望滚转角来自遥控器或者位置环 float roll_error roll_target - roll_estimated; float roll_rate_target roll_angle_pid(roll_error); // 内环角速度环 float roll_rate_error roll_rate_target - gyro_x; float roll_torque roll_rate_pid(roll_rate_error); // 同理计算俯仰和偏航力矩 // 混控 motor1 throttle roll_torque pitch_torque - yaw_torque; motor2 throttle - roll_torque pitch_torque yaw_torque; motor3 throttle - roll_torque - pitch_torque - yaw_torque; motor4 throttle roll_torque - pitch_torque yaw_torque; // 限幅 motor1 constrain(motor1, 1000, 2000); // ... 其他电机同理这里throttle是油门基准值来自高度环或者遥控器。roll_torque、pitch_torque、yaw_torque是三个轴的力矩输出。混控之后得到四个电机的PWM值范围1000到2000微秒对应电调的最小和最大油门。实操中要注意电机怠速。如果PWM低于电调的最小值电机会停转空中停转就是炸机。所以混控之后要做下限幅一般设到1050或者1100保证电机始终在转。另外如果某个电机达到上限其他电机要同步降低否则总推力会变化这就是抗饱和混控要做的事情。5. 常见问题与排查技巧实录5.1 起飞就翻的排查思路起飞就翻是新手最常遇到的问题。原因可能有很多按概率从高到低排电机转向错误、桨叶装反、混控方向错误、陀螺仪方向错误、PID参数太激进。排查的时候先不要装桨推油门看电机转向确认对角电机转向相同相邻电机转向相反。然后检查桨叶有字的一面朝上桨叶的迎风面要产生向上的推力。再检查飞控的安装方向如果飞控旋转了90度要在固件里设置偏航偏移。如果这些都对了还是翻那就把PID的P和D都降到默认值的一半再试。还翻的话检查陀螺仪方向。在飞控软件里看姿态显示手动倾斜机身看软件里的姿态变化方向是否一致。如果滚转方向反了就在固件里改陀螺仪方向。5.2 空中高频抖动的解决空中高频抖动通常是振动引起的。先做机械检查桨叶有没有破损、电机有没有虚位、机架有没有松动。然后看日志里的陀螺仪频谱如果振动峰值在100赫兹以上说明是电机或者桨叶的振动。解决办法是换平衡好的桨叶给飞控加减振或者在软件里加陷波滤波器。如果振动峰值在50赫兹以下可能是PID的D太大。把D减小一半再试。还有一种可能是角速度环的P太大导致系统在带宽附近振荡。把P减小百分之二十同时把D也减小观察效果。5.3 悬停时缓慢漂移的处理悬停漂移分两种情况姿态漂移和位置漂移。姿态漂移是无人机自己慢慢倾斜通常是陀螺仪零偏没校准好或者加速度计校准不准。重新做六面校准静置几分钟让陀螺仪温度稳定后再校准零偏。位置漂移是无人机保持水平但水平移动这是GPS精度问题正常现象。如果漂移太快检查GPS星数和HDOP值星数少于8颗或者HDOP大于2.0就别飞了。5.4 常见问题速查表现象可能原因排查方法解决措施起飞就翻电机转向错误不装桨推油门观察交换任意两根电机线起飞就翻桨叶装反检查桨叶朝向按标记重新安装起飞就翻混控方向错误手动倾斜看姿态修改混控矩阵符号空中高频抖动振动大看陀螺仪频谱减振、换桨、加陷波空中高频抖动D太大减小D试飞逐步减小D悬停漂移陀螺仪零偏静置看陀螺仪输出重新校准零偏悬停漂移GPS精度差看星数和HDOP等待星数足够再飞打杆响应迟钝P太小增加P逐步增加P打杆过冲P太大或D太小观察响应曲线减小P或增加D电机发热严重混控不平衡检查电机PWM输出调整混控或怠速5.5 实操心得与避坑技巧第一个心得调参一定要在无风或者微风的天气进行风大了你分不清是风扰还是参数问题。而且要在开阔场地周围没人电池电量充足。每次只改一个参数改完记录效果不要一次改好几个否则出了问题不知道是哪个参数导致的。第二个心得日志是你的好朋友。Pixhawk和很多开源飞控都支持日志记录把姿态、角速度、电机输出、振动都记下来。分析日志比在空中瞎试效率高得多。我习惯用飞控自带的日志分析工具看角速度跟踪曲线如果期望角速度和实际角速度贴合得好说明内环调好了。第三个心得不要迷信默认参数。默认参数是针对典型机架和典型重量的如果你的机架特别重或者特别轻电机KV特别高或者特别低默认参数肯定不合适。比如载重20公斤的八旋翼惯量大P要小一些I要大一些否则响应太猛容易超调。而250克的小穿越机惯量小P可以大一些响应要快。第四个心得硬件在环仿真真的能省很多事。我早期调参都是直接飞炸了不知道多少次。后来搭了硬件在环仿真环境把飞控板连上电脑在仿真里调好基础参数再上真机微调炸机次数少了一大半。仿真里还可以模拟传感器故障、电机失效这些极端情况测试算法的鲁棒性。第五个心得温度对陀螺仪零偏影响很大。冬天户外飞和室内飞陀螺仪零偏能差好几度每秒。所以校准零偏的时候要让飞控先通电预热几分钟等温度稳定了再校准。有些高端飞控有温度补偿但大部分消费级飞控没有只能靠预热。6. 飞控算法的进阶方向与扩展思路6.1 从PID到现代控制算法PID虽然经典但在某些场景下力不从心。比如无人机挂载了变质量负载比如果实采摘时机械臂伸缩导致重心变化PID参数就不适应了。这时候可以考虑LQR或者模型预测控制。LQR基于线性化模型设计最优控制律对模型精度要求高但鲁棒性比PID好。模型预测控制则是在每个控制周期求解一个有限时域优化问题能处理约束但计算量大对飞控芯片要求高。实际工程中很多行业无人机还是用PID加前馈补偿。前馈补偿就是根据期望加速度提前给出一个力矩减轻反馈控制的负担。比如位置环输出期望加速度时同时根据加速度计算出需要的倾角前馈量直接加到姿态环的期望角度上。这样跟踪误差会小很多。6.2 视觉感知与飞控的融合现在越来越多的无人机搭载视觉传感器比如光流、双目、深度相机。视觉感知给飞控提供位置和速度信息实现室内无GPS定位。视觉和飞控的融合有两种方式松耦合和紧耦合。松耦合是视觉算法自己算出位置然后作为位置环的反馈紧耦合是把视觉的原始特征点直接放进状态估计器里一起优化。松耦合实现简单但延迟大紧耦合精度高但计算复杂。如果做无人机航道巡查视觉可以用来识别航道边界和障碍物。飞控算法要做的就是把视觉输出的障碍物位置转化为避障的期望轨迹。这涉及到路径规划算法比如A星、RRT或者人工势场法。路径规划给出航点飞控的位置环跟踪航点底层还是PID。6.3 硬件在环仿真的搭建要点硬件在环仿真对飞控算法开发帮助极大。搭建的时候需要一台电脑跑仿真模型一块飞控板跑实际算法两者通过串口或者UDP通信。仿真模型可以用Simulink搭建也可以用开源的JSBSim或者Gazebo。通信协议要定义好飞控发给仿真模型的是电机PWM值仿真模型发给飞控的是传感器数据包括陀螺仪、加速度计、磁力计、气压计、GPS。仿真步长要和飞控控制周期匹配一般设1毫秒。通信延迟要尽量小否则仿真结果不真实。如果延迟太大可以在仿真模型里加一个延迟补偿或者降低通信频率。硬件在环仿真最大的好处是可以反复重现同样的场景比如同样的风扰、同样的初始偏差方便对比不同算法的效果。6.4 飞控算法的安全设计飞控算法不仅要飞得稳还要飞得安全。安全设计包括失控保护、低电量保护、地理围栏、电机失效容错。失控保护是当遥控器信号丢失时飞控自动执行返航或者降落。低电量保护是电压低于阈值时自动降落。地理围栏是限制飞行区域防止飞入禁飞区。电机失效容错是通过控制分配算法在部分电机失效时仍然维持姿态并安全降落。这些安全逻辑在代码里要放在最高优先级而且要有独立的监控任务。比如失控保护检测遥控器信号如果连续几百毫秒没有收到有效信号就触发返航。返航过程中如果GPS信号也丢了就原地降落。这些逻辑要经过充分测试不能有漏洞。飞控算法这个领域入门容易精通难。PID谁都能写但要在各种极端情况下都飞得稳需要大量的实践和积累。我个人的体会是不要一开始就追求高大上的算法把PID和状态估计吃透把传感器校准和振动处理做好就已经能应对百分之九十的场景了。剩下的百分之十再根据具体需求去引入更复杂的算法。每次炸机都是学习的机会分析日志、找到根因、改进算法这个过程本身就是飞控开发的乐趣所在。