四轴飞行器飞控编写教程

四轴飞行器飞控编写教程写在前面这份教程专门为零基础的初学者编写。如果你刚接触四轴飞行器不知道从何下手听说过PID控制但不理解它是怎么工作的看过飞控代码但感觉像天书一样看不懂想自己动手写飞控但不知道从哪里开始——那么这份教程就是为你准备的。我们只讲飞控算法不涉及硬件底层。也就是说我们不会去讲寄存器配置、PWM波形生成、中断优先级设置等内容而是专注于飞控是怎么想的、怎么计算的、怎么控制飞机的。我们的目标是让你理解飞机为什么能稳定飞行以及如何让它稳定飞行。建议按照章节顺序阅读因为后面的内容会用到前面的知识。如果某个地方不理解可以多读几遍或者跳过去看后面的内容有时候后面的例子会帮你理解前面的概念。第一章飞控系统概述飞控是做什么的飞控全称飞行控制器是四轴飞行器的大脑。想象一下如果让你手动控制四个电机让飞机稳稳地悬停在空中你能做到吗几乎不可能因为你需要同时控制四个电机的转速需要根据飞机的姿态实时调整需要对外界扰动如风做出快速反应而且需要在毫秒级别完成所有计算和调整。这就是飞控的工作——它以每秒200次甚至更高的速度不断地读取传感器数据、计算姿态、调整电机让飞机稳定飞行。用一句话概括飞控的工作让飞机从当前姿态变成期望姿态并保持稳定。举个例子当你推动遥控器想让飞机向前飞时期望姿态就是飞机低头。飞控会读取传感器发现当前是水平的然后计算需要让飞机低头于是调整电机让前面两个转速降低、后面两个升高飞机就开始低头向前飞了。这个过程中飞控一直在工作不断调整确保飞机按照你的指令飞行。飞控系统的四大模块飞控系统可以分为四个核心模块传感器模块负责感知飞机当前状态姿态解算模块把原始数据转换成我们容易理解的角度PID控制模块根据期望和现实的差距计算修正方案电机输出模块把修正方案分配给四个电机。传感器就像是飞机的感觉器官它告诉飞控飞机当前是水平的还是倾斜的、是在上升还是下降、转了多少角度。姿态解算模块把传感器给出的原始数据加速度、角速度转换成我们容易理解的角度。PID控制器根据期望姿态和当前姿态的差距计算出需要怎么调整电机。最后电机输出模块把修正量转换成四个电机的具体转速。把这四个模块串起来就是飞控的完整工作流程首先读取传感器数据然后进行姿态解算得到当前姿态角度接着读取遥控器数据得到期望姿态计算误差后通过PID算出修正量最后通过电机混控输出给四个电机。这个循环每5毫秒执行一次也就是每秒执行200次第二章姿态与坐标系什么是姿态姿态就是飞机在空中的姿势就像人可以站、坐、躺一样飞机也有不同的姿态。描述飞机姿态需要用三个角度专业术语叫欧拉角。Pitch俯仰角描述的是飞机低头和抬头的动作。想象你站在原地低头看地面时俯仰角为负抬头看天空时俯仰角为正平视前方时俯仰角为零。对飞机来说俯仰角为负意味着机头向下飞机会向前飞俯仰角为正意味着机头向上飞机会向后飞。Roll横滚角描述的是飞机左右倾斜。想象你站着向左歪着站时横滚角为负向右歪着站时横滚角为正站得笔直时横滚角为零。对飞机来说横滚角为负意味着左低右高飞机会向左飞横滚角为正意味着左高右低飞机会向右飞。Yaw偏航角描述的是飞机左右转向。想象你原地转身向左转时偏航角为负向右转时偏航角为正面向前方时偏航角为零。对飞机来说偏航角变化意味着机头转向飞机改变朝向。用一张图来理解三个轴的关系想象你坐在飞机里Pitch就像点头机头上下Roll就像歪头左右倾斜Yaw就像转身左右转向。四轴飞行器的电机布局四轴飞行器有四个电机呈X形布局。从上往下看左前是M2右前是M1左后是M3右后是M4。其中M1和M3顺时针旋转M2和M4逆时针旋转——注意对角电机的转向相同这样可以抵消反扭矩让飞机不会乱转。电机控制原理这里是飞控的最重要的部分。四个电机转速相同飞机就水平悬停四个电机转速不同飞机就倾斜或转向。通过让某些电机转得快一些、某些慢一些就能控制飞机的姿态。向前飞需要让飞机低头原理是前面两个电机转速降低、后面两个升高。当前面升力小、后面升力大时飞机就会低头向前飞。向左飞需要让飞机左倾原理是左边两个电机转速降低、右边两个升高。当左边升力小、右边升力大时飞机就会左倾向左飞。转向需要利用反扭矩原理。因为对角电机转向相同改变对角电机的转速差就能产生转向力矩。想让飞机左转逆时针就让顺时针转的M1、M3转速升高、逆时针转的M2、M4转速降低反扭矩会使飞机逆时针转。动作M1(右前)M2(左前)M3(左后)M4(右后)效果悬停基准基准基准基准保持水平向前飞降低降低升高升高飞机低头向前向左飞升高降低降低升高飞机左倾向左左转升高降低升高降低飞机逆时针转这个表就是飞控要做的事情。飞控就是根据遥控器的指令计算出需要调整哪些电机转速、怎么调整然后输出给四个电机。第三章传感器与姿态解算认识IMU传感器IMU是飞控的眼睛和耳朵它包含两个核心传感器加速度计和陀螺仪。加速度计测量三个方向的加速度包括重力加速度。把手机平放在桌面上加速度计会测到Z轴有一个约9.8 m/s²的值重力把手机立起来这个值会转移到Y轴或X轴。在飞控中加速度计告诉我们哪里是下方通过重力方向可以推算出飞机的倾斜角度。但加速度计有个缺点它会被振动干扰电机振动、空气扰动都会影响测量值而且飞机运动时测量值会叠加运动加速度变得不准确。陀螺仪测量三个轴的角速度旋转速度单位是度/秒。你原地转身陀螺仪会测到一个旋转速度转得越快测量值越大。在飞控中陀螺仪告诉我们飞机转得有多快对角速度积分可以得到旋转角度。但陀螺仪也有缺点它有漂移积分会累积误差时间长了算出来的角度会越来越不准。为什么需要姿态解算你可能会问有了加速度计和陀螺仪不是就能算出角度了吗问题在于两者单独使用都不够可靠。加速度计长期稳定不漂移但短期噪声大、受振动干扰陀螺仪短期准确响应快但长期漂移、积分误差累积。解决方案是融合两者的优点用陀螺仪提供短期准确的数据用加速度计修正陀螺仪的漂移。这就是姿态解算算法的工作。第四章Mahony姿态解算算法重点这一章是整个姿态解算的核心内容会稍微深入一些。为什么用四元数而不用欧拉角在讲Mahony算法之前先回答一个关键问题为什么我们不直接用欧拉角而要用四元数欧拉角虽然直观pitch、roll、yaw三个角度但存在一个严重问题万向节死锁Gimbal Lock。当飞机旋转到某些特定角度时欧拉角的计算会出现奇异点导致姿态解算失效。举个通俗的例子想象你拿着一个指南针当指南针水平放置时它能正确指示方向。但如果你把指南针竖起来旋转90度它的指针就不再能正确指示方向了——这就是一种死锁。四元数用四个数q0, q1, q2, q3来表示旋转虽然不够直观但不会出现死锁问题计算也更平滑适合连续旋转。我们不需要深入理解四元数的数学原理只需要知道四元数可以表示任意姿态四元数可以转换成欧拉角我们最终使用的还是欧拉角pitch, roll, yawMahony算法的核心思想Mahony算法的核心思想可以概括为一句话用陀螺仪积分得到姿态用加速度计修正漂移。具体怎么实现呢分几个步骤来讲第一步理解两种重力方向陀螺仪积分后我们得到了一个姿态用四元数表示根据这个姿态可以估计出重力应该指向哪个方向。同时加速度计直接测量了重力方向因为静止时加速度计测的就是重力。如果陀螺仪没有漂移这两个方向应该完全一致。但如果陀螺仪有漂移这两个方向就会有差异。这个差异就是误差第二步用误差修正陀螺仪计算出误差后我们用这个误差来修正陀螺仪的读数。这就像一个PI控制器P项比例误差越大修正越强I项积分累积的误差也要修正修正后的陀螺仪数据更加准确积分得到的姿态也更加准确。第三步更新四元数用修正后的陀螺仪数据更新四元数。这一步涉及四元数的微分方程四元数微分方程q̇ 0.5 × q ⊗ ω其中q是四元数ω是角速度⊗是四元数乘法。这个公式告诉我们四元数的变化率等于当前四元数与角速度的乘积。第四步归一化四元数四元数必须保持单位长度|q| 1否则会引入误差。所以每一步都要归一化。第五步四元数转欧拉角最后把四元数转换成我们熟悉的欧拉角。Mahony算法的数学推导可选如果想更深入理解这里有一些关键公式。如果觉得太难可以跳过不影响理解算法的整体流程。四元数与旋转矩阵的关系四元数q (q0, q1, q2, q3)对应的旋转矩阵R是四元数对应旋转矩阵R | 1-2(q2²q3²) 2(q1q2-q0q3) 2(q1q3q0q2) | | 2(q1q2q0q3) 1-2(q1²q3²) 2(q2q3-q0q1) | | 2(q1q3-q0q2) 2(q2q3q0q1) 1-2(q1²q2²) |旋转矩阵的第三列就是陀螺仪估计的重力方向估计重力方向公式估计重力 (2(q1q3-q0q2), 2(q2q3q0q1), 1-2(q1²q2²))误差计算误差等于测量重力方向叉乘估计重力方向误差计算公式error accel_measured × gravity_estimated叉乘的结果是一个向量垂直于两个输入向量大小等于两个向量的夹角的正弦值。当两个方向一致时叉乘结果为零当有偏差时叉乘结果就是修正方向。误差修正误差修正公式gyro_corrected gyro_raw Kp × error Ki × ∫error其中Kp是比例增益Ki是积分增益。这两个参数决定了修正的强度。Mahony算法的代码实现思路理解了原理后我们来看看代码是怎么实现的。这里给大家一个伪代码帮大家来理解流程。Mahony算法伪代码// 全局变量四元数初始值代表水平姿态 float q0 1.0f, q1 0.0f, q2 0.0f, q3 0.0f; // 全局变量积分误差 float integralFBx 0.0f, integralFBy 0.0f, integralFBz 0.0f; // 算法参数 float twoKp 2.0f; // 2倍比例增益 float twoKi 0.0f; // 2倍积分增益通常设为0也能工作 void MahonyAHRSupdateIMU(float gx, float gy, float gz, // 陀螺仪弧度/秒 float ax, float ay, float az) // 加速度计任意单位 { // 步骤1归一化加速度计数据 // 先计算加速度的模长 float norm sqrt(ax*ax ay*ay az*az); // 防止除零 if (norm 0.0f) return; // 归一化 ax / norm; ay / norm; az / norm; // 步骤2计算估计的重力方向 // 根据当前四元数估计重力方向旋转矩阵第三列 float halfvx q1*q3 - q0*q2; float halfvy q0*q1 q2*q3; float halfvz q0*q0 - 0.5f q3*q3; // 步骤3计算误差叉乘 // 误差 测量重力 × 估计重力 float halfex ay*halfvz - az*halfvy; float halfey az*halfvx - ax*halfvz; float halfez ax*halfvy - ay*halfvx; // 步骤4积分误差如果启用了Ki if (twoKi 0.0f) { integralFBx twoKi * halfex * (1.0f / sampleFreq); integralFBy twoKi * halfey * (1.0f / sampleFreq); integralFBz twoKi * halfez * (1.0f / sampleFreq); // 把积分误差加到陀螺仪上 gx integralFBx; gy integralFBy; gz integralFBz; } // 步骤5比例修正加到陀螺仪上 gx twoKp * halfex; gy twoKp * halfey; gz twoKp * halfez; // 步骤6积分四元数微分方程 // 预乘时间因子 gx * 0.5f * (1.0f / sampleFreq); gy * 0.5f * (1.0f / sampleFreq); gz * 0.5f * (1.0f / sampleFreq); // 保存旧的四元数值 float qa q0, qb q1, qc q2; // 更新四元数 q0 -qb*gx - qc*gy - q3*gz; q1 qa*gx qc*gz - q3*gy; q2 qa*gy - qb*gz q3*gx; q3 qa*gz qb*gy - qc*gx; // 步骤7归一化四元数 norm sqrt(q0*q0 q1*q1 q2*q2 q3*q3); q0 / norm; q1 / norm; q2 / norm; q3 / norm; }代码要点解释归一化加速度计因为加速度计的单位和量程可能不同归一化后只关心方向不关心大小。估计重力方向用四元数计算出按照当前姿态重力应该指向哪里。公式看起来复杂其实就是旋转矩阵的第三列。叉乘计算误差叉乘的结果告诉你怎么旋转才能让估计方向和测量方向一致。积分误差如果误差一直存在陀螺仪有固定偏移积分项会累积起来最终消除这个偏移。四元数更新这是最核心的一步用陀螺仪数据更新四元数。公式来自四元数微分方程。归一化四元数由于浮点数计算误差四元数长度会逐渐偏离1必须每步归一化。四元数转欧拉角最后我们需要把四元数转换成欧拉角方便后续使用四元数转欧拉角代码void quaternionToEuler(float q0, float q1, float q2, float q3, Euler_angles* angles) { // Pitch俯仰角 float sinp 2.0f * (q0*q2 - q3*q1); if (fabs(sinp) 1) angles-pitch asin(1.0或-1.0) * 180/π; // 处理90度边界 else angles-pitch asin(sinp) * 180/π; // Roll横滚角 angles-roll atan2(2.0f*(q0*q1 q2*q3), 1.0f - 2.0f*(q1*q1 q2*q2)) * 180/π; // Yaw偏航角 angles-yaw atan2(2.0f*(q0*q3 q1*q2), 1.0f - 2.0f*(q2*q2 q3*q3)) * 180/π; }注意这里乘以180/π是把弧度转换成角度方便阅读和调试。Mahony算法的参数调节Mahony算法有两个关键参数Kp比例增益和Ki积分增益。Kp的作用决定了加速度计修正陀螺仪的强度。Kp越大收敛越快但对加速度计噪声也更敏感。典型值在0.5到5之间。Ki的作用消除陀螺仪的固定偏移零偏。如果陀螺仪零偏很小可以设为0。典型值在0到0.1之间。调节建议首先把Ki设为0只调KpKp太小收敛慢姿态响应迟钝Kp太大姿态震荡对加速度计噪声敏感找到合适的Kp后如果发现姿态有固定偏差再逐渐增加Ki完整的姿态解算流程把所有内容串起来完整的姿态解算流程如下完整姿态解算流程代码void AttitudeUpdate(void) { // 1. 读取IMU原始数据 float ax, ay, az; // 加速度计原始数据 float gx, gy, gz; // 陀螺仪原始数据 ReadIMU(ax, ay, az, gx, gy, gz); // 2. 转换单位 // 加速度计归一化或转换为g // 陀螺仪从原始值转换为弧度/秒 gx gx_raw * π / 180.0f / 灵敏度; gy gy_raw * π / 180.0f / 灵敏度; gz gz_raw * π / 180.0f / 灵敏度; // 3. 可选数据滤波 LowPassFilter(ax, ay, az); // 注意陀螺仪通常不滤波因为会引入延迟 // 4. Mahony算法更新四元数 MahonyAHRSupdateIMU(gx, gy, gz, ax, ay, az); // 5. 四元数转欧拉角 quaternionToEuler(q0, q1, q2, q3, angles); // 现在 angles.pitch, angles.roll, angles.yaw 就是当前姿态 }这个函数应该以固定频率调用如200Hz通常放在定时中断里。第五章PID控制算法什么是PID控制PID控制是工业中最常用的控制算法从空调温控到汽车巡航到处都有它的身影。它的核心思想很简单根据误差的大小计算出修正量使系统趋向目标值。举个生活中的例子想象你在开车想让车速保持在100km/h。你看仪表盘发现当前速度是80km/h误差就是20km/h。你踩油门踩多深取决于误差大小。车速上升后误差减小油门逐渐松开最终车速稳定在100km/h。这个过程就是一个P比例控制。PID三个部分详解**P比例控制**的公式是输出 Kp × 误差意思是误差越大修正越强。比如目标角度是0°、当前角度是10°误差就是10°如果Kp2那么输出就是20。P控制的特点是响应快、立即产生修正但可能导致超调修正过头而且如果只有P控制可能无法完全消除误差。它就像开车时误差大就猛踩油门。**I积分控制**的公式是输出 Ki × 误差积分意思是把误差累加起来持续修正。为什么需要I有时候P控制的输出不够大无法完全消除误差。比如飞机一直偏向左边2°P控制输出一个小修正但不够I控制每周期累加这个误差累积到一定程度就能产生足够大的修正把它消除。I控制的特点是能消除稳态误差但可能导致超调和震荡而且需要限制积分值防止积分饱和。它就像持续的小错误也要纠正不能忽视。**D微分控制**的公式是输出 Kd × 误差变化率意思是预测误差的变化趋势提前修正。为什么需要DP控制只看当前误差不知道误差在变大还是变小D控制能预判趋势提前刹车。比如当前误差是5°、上一周期误差是10°误差变化是-5°说明误差在减小修正够了不需要再加D输出为负可以抵消一部分P输出。D控制的特点是提供阻尼抑制震荡但对噪声敏感需要配合滤波太大会导致响应迟钝。它就像预判快到目标了就开始刹车。积分饱和是什么积分饱和是PID控制中的一个常见问题。假设飞机在地上你突然把目标高度设为10米误差非常大积分项开始快速累积积分值变得很大很大。当飞机接近目标时积分项仍然很大会导致飞机严重超调甚至冲过头。解决方法是积分限幅限制积分值的范围不让它无限增长。完整的PID公式完整的PID公式是输出 Kp × e(t) Ki × ∫e(t)dt Kd × de(t)/dt其中e(t)是误差期望值减测量值。用代码表示更直观void Com_PID_Calculation(PID_Struct *pid, float dt) { // 计算误差 float error pid-desire - pid-measure; // 积分项累加误差 pid-integral error; // 积分限幅防止积分饱和 if (pid-integral pid-Limiting) pid-integral pid-Limiting; if (pid-integral -pid-Limiting) pid-integral -pid-Limiting; // 微分项误差变化率 float der error - pid-last_error; pid-last_error error; // PID输出 P项 I项 D项 pid-output pid-kp * error pid-ki * pid-integral pid-kd * der; }级联PID双环控制单环PID存在一个问题响应不够快抗干扰能力弱。想象你在控制飞机角度单环PID是角度误差直接输出给电机但如果有风吹来飞机先倾斜PID才开始修正反应就慢了。解决方案是级联PID也就是外环加内环。外环是角度环输入期望角度和测量角度输出期望角速度作用是确保最终角度正确。内环是角速度环输入期望角速度和测量角速度输出电机修正量作用是快速响应扰动。为什么这样设计因为角速度来自陀螺仪响应非常快毫秒级角度需要积分计算响应较慢。内环可以快速抑制扰动不等它影响角度外环负责最终精度。void Com_PID_Cascade(PID_Struct *outter, PID_Struct *inner, float dt) { // 第一步计算外环PID Com_PID_Calculation(outter, dt); // 第二步外环输出作为内环的期望值 inner-desire outter-output; // 第三步计算内环PID Com_PID_Calculation(inner, dt); }第六章飞控核心流程飞控的心脏——定时中断飞控的核心代码运行在一个定时中断里。什么是中断想象你在看书定时中断就像闹钟每隔固定时间响一次闹钟响了你就放下书本去处理其他事情处理完回来继续看书。为什么用定时中断因为这样可以保证飞控以固定的频率运行如每5ms一次不受其他代码影响确保实时性。主控制循环飞控的主控制循环每5ms执行一次流程如下void MainControlLoop(void) // 5ms定时器中断 { // 步骤1姿态解算——读取IMU数据计算当前姿态 AttitudeUpdate(); // 得到 angles.pitch, angles.roll, angles.yaw // 步骤2遥控器数据解析——得到期望值 ParseRemoteController(); // 得到期望角度、油门、飞行模式等 // 步骤3高度控制——如果是定高模式执行高度PID if (altitude_hold_mode) { AltitudePID(current_height, target_height, 0.005f); } // 步骤4姿态PID控制——根据期望角度和当前角度计算电机修正量 AttitudePID(); // 步骤5电机输出——把PID输出转换成四个电机的转速 MotorMixer(); }姿态PID控制详解姿态PID控制是飞控的核心算法。三个方向Pitch、Roll、Yaw各有一套级联PID每套PID包含外环角度环和内环角速度环。角度环外环与角速度环内环的级联PID控制逻辑及输出流程可通过以下程序框图清晰理解贴合飞控实际代码执行逻辑框图说明遥控器输入的期望角度进入角度环外环与姿态解算得到的当前角度计算误差输出期望角速度该期望角速度作为角速度环内环的输入与陀螺仪测量的当前角速度计算误差输出电机修正量修正量与基础油门经电机混控后输出四个电机的具体转速驱动飞机调整姿态形成闭环控制。以Pitch控制为例外环的期望值来自遥控器用户想让飞机低头多少测量值来自姿态解算当前实际俯仰角。内环的测量值来自陀螺仪Y轴角速度俯仰角速度。通过级联PID计算最终输出俯仰方向的电机修正量。void AttitudePID(void) { // Pitch俯仰控制 pid_pitch.desire 遥控器映射到角度; pid_pitch.measure angles.pitch; // 当前俯仰角 pid_gyro_y.measure gyroy / 灵敏度; // 角速度 Com_PID_Cascade(pid_pitch, pid_gyro_y, dt); // Roll横滚控制——原理与Pitch相同 pid_roll.desire 遥控器映射到角度; pid_roll.measure angles.roll; pid_gyro_x.measure gyrox / 灵敏度; Com_PID_Cascade(pid_roll, pid_gyro_x, dt); // Yaw偏航控制 pid_yaw.desire 遥控器映射到角度; pid_yaw.measure angles.yaw; pid_gyro_z.measure gyroz / 灵敏度; Com_PID_Cascade(pid_yaw, pid_gyro_z, dt); }电机混控PID控制输出的是三个修正量Pitch修正量、Roll修正量、Yaw修正量。这些修正量需要转换成四个电机的转速这个过程叫电机混控。混控公式怎么理解以Pitch修正为例向前飞要低头前电机M1、M2要减油门后电机M3、M4要加油门所以M1、M2加Pitch而M3、M4减Pitch当Pitch为负时。Roll修正和Yaw修正的道理类似。M1(右前) 基础油门 Pitch修正 - Roll修正 - Yaw修正 M2(左前) 基础油门 Pitch修正 Roll修正 Yaw修正 M3(左后) 基础油门 - Pitch修正 - Roll修正 Yaw修正 M4(右后) 基础油门 - Pitch修正 Roll修正 - Yaw修正第七章飞行模式与状态机飞控的几种模式一个完整的飞控通常支持多种飞行模式。手动模式完全由你控制飞控只保持姿态稳定适合有经验的飞手。自稳模式松开摇杆飞机会自动保持水平适合初学者。定高模式自动保持高度你只控制方向也适合初学者。定点模式自动保持位置悬停在原地对新手最友好。自动模式按预设航线飞行适合航拍等应用。什么是状态机状态机是一种管理模式切换的方法。想象电梯当前状态是停在一楼按3楼按钮状态就变为上升到达3楼状态又变为停止。电梯只能处于一种状态停止、上升、下降状态之间有明确的切换条件。飞控的状态机也是一样的道理。飞控状态机设计典型的飞控有四种状态IDLE停机状态电机停转、HAND_FLYING手动模式完全由遥控器控制、FIXED_HEIGHT定高模式自动保持高度、FIXED_POINT定点模式自动保持位置。状态之间的切换逻辑是从IDLE状态如果满足解锁、遥控器连接、急停关闭、起飞指令等条件就可以切换到HAND_FLYING或FIXED_HEIGHT状态。在HAND_FLYING状态下如果触发急停或上锁就回到IDLE。在FIXED_HEIGHT状态下可以切换到FIXED_POINT也可以回到IDLE。在FIXED_POINT状态下可以切换回FIXED_HEIGHT也可以回到IDLE。第八章定高与定点控制为什么需要定高在手动模式下你需要不断调整油门来保持高度这很累人。定高模式下飞控会自动调整油门让飞机保持在你想要的高度。高度测量要控制高度首先要测量高度。常用的测量方式有三种。TOF飞行时间传感器发射红外光测量光线往返时间从而计算距离精度高毫米级但量程有限通常只能测几米内。超声波传感器原理类似但使用声波代替光波成本低量程较大但精度较低且受环境影响大。气压计测量大气压变化推算高度变化适合大高度变化但精度低且受天气影响。高度控制高度控制也使用级联PID。外环是高度环输入期望高度和测量高度输出期望垂直速度。内环是速度环输入期望垂直速度和测量垂直速度输出高度修正量。测量垂直速度有两种方法一种是对TOF高度微分短期准但噪声大另一种是对加速度积分长期准但有漂移。实际应用中会融合两种方法。定点控制定点控制让飞机保持在水平面上的某个位置不漂移。它使用光流传感器通过摄像头检测地面纹理的移动来计算飞机的水平位移。定点控制是更复杂的级联控制位置环输出期望速度速度环输出期望角度然后才进入姿态控制。也就是说定点PID的输出会作为姿态PID的期望值。光流传感器有个问题飞机旋转时传感器也会检测到位移但这是虚假位移。解决方法是用陀螺仪测量旋转角速度计算旋转引起的虚假位移然后从光流数据中减去。第九章PID调参指南调参的重要性PID参数决定了飞控的性能参数合适时飞机稳定、响应快、操控好参数不合适时飞机震荡、漂移甚至失控。调参是飞控开发中最关键也最考验经验的环节。调参前的准备首先要确保硬件正常电机安装正确转向正确传感器校准完成遥控器连接正常。其次要确保安全卸下螺旋桨防止意外伤人准备急停开关在空旷场地测试。最后要准备调试工具能实时查看传感器数据和PID输出能快速修改参数。调参顺序记住口诀先内环后外环先P后D再I。为什么要先调内环因为内环角速度环直接控制电机内环不稳定外环根本无法工作内环调好后外环才能发挥效果。角速度环调参调P比例系数初始值建议P0.3。测试方法是把飞机固定在测试架上只能绕一个轴转动给一个干扰观察飞机反应。如果P太小反应迟钝修正缓慢晃动几下才稳住如果P刚好反应迅速一两次晃动后稳定如果P太大高频震荡嗡嗡响甚至发散。调参方法是从小到大逐渐增加P出现轻微震荡时往回退一点找到刚好不震荡的值。调D微分系数初始值建议D0。D的作用是增加阻尼抑制震荡。如果D太小容易超调震荡如果D刚好响应平滑无震荡如果D太大响应迟钝对噪声敏感。调参方法是P引起轻微震荡时逐渐增加D震荡消除即可不要过大D过大反而会引入噪声震荡。调I积分系数初始值建议I0.01很小的值。I的作用是消除稳态误差。如果I太小有偏差时无法自动纠正一直歪着如果I刚好能自动回到水平无低频震荡如果I太大低频震荡超调。调参方法是在P和D调好后给飞机一个固定干扰如挂个重物观察能否自动回正不能回正则增加I出现低频震荡则减小I。角度环调参角速度环调好后才能调角度环。角度环通常只需要P初始值建议P5.0。测试方法是推动遥控器摇杆松开后飞机应自动回正。如果P太小回正慢需要等很久如果P刚好快速回正无超调如果P太大回正时超调震荡。调参方法是从小到大增加P直到飞机能快速响应遥控器指令但不要出现震荡。参考参数以下是一组典型的PID参数仅供参考。注意参数正负号取决于传感器安装方向和电机转向每架飞机的参数都可能不同需要根据实际飞行效果微调。// Pitch俯仰控制 pid_pitch.kp 5.0f; // 外环角度环 pid_gyro_y.kp -0.3f; // 内环角速度环 pid_gyro_y.ki -0.025f; pid_gyro_y.kd -4.0f; // Roll横滚控制 pid_roll.kp 5.0f; // 外环 pid_gyro_x.kp 0.3f; // 内环 pid_gyro_x.ki 0.025f; pid_gyro_x.kd 4.0f; // Yaw偏航控制 pid_yaw.kp 2.0f; // 外环 pid_gyro_z.kp -1.0f; // 内环 // 高度控制 pid_height.kp 1.0f; // 外环 pid_z_speed.kp 0.2f; // 内环 pid_z_speed.ki 0.02f; pid_z_speed.kd 0.4f;调参常见问题飞机剧烈震荡通常是PID参数过大解决方法是减小角速度环P值或增加D值同时检查控制周期是否稳定。飞机反应迟钝通常是PID参数过小解决方法是增加P值或减小D值。飞机有固定偏差通常是I参数太小或传感器零偏解决方法是增加I值或校准传感器零偏。飞机漂移严重通常是姿态解算不准解决方法是检查Mahony算法参数、调整Kp增益、上电后静置校准。第十章常见问题与解决方法起飞问题飞机起飞后立即翻转是最常见的问题之一。可能的原因有电机顺序接错、电机转向错误、PID正负号错误、传感器方向错误。排查方法是逐个测试电机确认顺序和转向用手倾斜飞机观察PID输出方向是否正确检查传感器安装方向。飞机无法起飞的可能原因有油门不足、电机不转、电池电量不足、飞行模式错误。排查方法是检查电机是否能正常转动、检查电池电压、确认处于正确的飞行模式。稳定性问题飞机悬停时震荡可能的原因有PID参数过大、传感器噪声大、机械振动。解决方法是减小角速度环P值、增加传感器滤波、检查电机平衡和机架刚性。飞机缓慢漂移可能的原因有陀螺仪零偏、加速度计校准不准、定点传感器失效。解决方法是上电后静置校准陀螺仪、重新校准加速度计、检查光流/TOF传感器。控制问题遥控器无响应可能的原因有SBUS信号丢失、解析错误、中断配置问题。解决方法是检查接收机连接、检查SBUS波特率通常100000、检查串口中断是否正常。飞机不听指挥可能的原因有遥控器通道映射错误、PID输出方向错误、电机混控公式错误。解决方法是检查遥控器通道定义、验证PID输出与期望动作的关系、检查电机混控公式。传感器问题姿态角不准可能的原因有传感器校准问题、Mahony算法参数不对、滤波参数不当。解决方法是重新校准传感器、调整Mahony的Kp和Ki参数、调整滤波器截止频率。高度跳变可能的原因有TOF传感器数据不稳定、测量范围超限、地面反射问题。解决方法是增加数据滤波、检查测量范围、更换测量表面。附录关键公式速查欧拉角定义Pitch俯仰角绕Y轴旋转机头抬起为正低头为负范围-90°到90°。Roll横滚角绕X轴旋转右侧下沉为正左侧下沉为负范围-180°到180°。Yaw偏航角绕Z轴旋转机头右偏为正左偏为负范围-180°到180°。PID公式输出 Kp × e(t) Ki × ∫e(t)dt Kd × de(t)/dt其中e(t) 期望值 - 测量值。离散形式error desire - measureintegral errorderivative error - last_erroroutput Kp×error Ki×integral Kd×derivative。电机混控公式M1(右前) Throttle Pitch_out Roll_out - Yaw_outM2(左前) Throttle Pitch_out - Roll_out Yaw_outM3(左后) Throttle - Pitch_out - Roll_out - Yaw_outM4(右后) Throttle - Pitch_out Roll_out - Yaw_out。注意正负号取决于具体布局和转向。Mahony算法核心用加速度计修正陀螺仪漂移修正公式gyro_corrected gyro_raw Kp × error Ki × ∫error其中error accel_normalized × gravity_estimated叉乘。四元数转欧拉角pitch asin(2×(q0×q2 - q3×q1))roll atan2(2×(q0×q1 q2×q3), 1 - 2×(q1² q2²))yaw atan2(2×(q0×q3 q1×q2), 1 - 2×(q2² q3²))结语恭喜你读完了这份教程飞控开发是一个需要理论与实践相结合的领域这份教程帮助你建立了理论基础但要真正掌握还需要大量的实践。建议的学习路径是先通过这份教程理解原理然后对照教程逐个函数理解代码并在代码中添加注释接着动手调试先确保硬件正常再逐步调通每个模块最后进行整合测试最后进行飞行测试注意安全从低空开始逐步增加功能。记住遇到问题不要急逐个排查多看代码多动手多思考飞控调参是一门艺术需要耐心和经验。祝你飞行愉快早日成为飞控高手