直接进入正题。这一篇继续源码连载讲mc_att_control。熟悉PX4的朋友应该都知道多旋翼飞控里最核心的闭环就是姿态控制而这个模块管的就是“怎么让飞机转到你想要的角度”后续的角速度环、混控、电机都在它下游。我先把丑话说在前面这篇源码分析基于我长期在用的一个旧分支对应v1.11到v1.13之间的代码结构新版固件里姿态控制和角速率控制已经拆成了两个模块函数名也有变化但核心算法和控制思路没有变读懂了这篇换到新版只是换个入口的问题。1. 先搞清楚mc_att_control在整个飞控里的位置1.1 从位置环到电机的全链路很多初学者一上来就抓起mc_att_control.cpp猛看结果越看越晕。原因很简单单看一个模块很难建立全局观。我习惯先讲链路再进代码。整条控制链路大概是这样的导航层拿到目标点位置控制器mc_pos_control算出期望加速度再换算成期望姿态四元数和期望推力通过vehicle_attitude_setpoint话题发出来。mc_att_control订阅这个期望姿态同时订阅当前姿态来自姿态估计器两者一比较得出“我现在需要转多快”也就是期望角速率。在旧版固件里这个模块还会继续做角速率控制把期望角速率和当前角速率比较算出三个轴的力矩期望写成actuator_controls_0话题。下游就是混控器根据机架类型把力矩和推力分配到各个电机。所以mc_att_control不是一个独立闭环它是级联控制里的中间层。你调参数时如果只调姿态环P位置环不动飞起来可能就没反应只调内环外环又不匹配结果就是振荡。前面这些链路关系先弄清楚后面看代码才有方向。1.2 旧版一锅端和新版拆分的原因网上很多教程对应的源码版本不一样经常有人问我“为什么我的源码里没有control_rate()函数”原因就是版本变迁。早期mc_att_control是姿态环和角速率环都在一个模块里姿态环输出期望角速度函数内部接着做角速度PID直接输出力矩。这个设计实现简单但有个问题固定翼和多旋翼的角速率控制特性不一样有人飞固定翼想单独调试角速度环结果发现和多旋翼姿态环绑在一起折腾起来很费劲。后面PX4把角速率控制单独拆出去变成了mc_rate_control模块。mc_att_control只负责姿态环输出的是vehicle_rates_setpointmc_rate_control订阅这个角速率期望再输出力矩到混控。从代码维护角度讲拆开之后职责更清晰也方便不同类型的飞行器复用同一个角速度控制器。我这篇主要是讲旧版整合式的代码结构因为算法部分更完整也更容易理解整条控制链新版只是把后半段搬到另一个文件里PID参数名字基本不变。1.3 源码入口和运行周期模块入口在src/modules/mc_att_control/mc_att_control_main.cpp。它和PX4大多数模块一样启动后开一个任务以固定周期运行主循环。周期就是4ms也就是250Hz。你可能听说过有的飞控跑到500Hz甚至1kHz那是角速率环的频率姿态环本身并不需要那么高250Hz足够覆盖绝大多数多旋翼的动力学响应。看一下task_spawn和run这段代码模块初始化时会创建uORB订阅和发布。订阅的主要话题包括vehicle_attitude_setpoint、vehicle_attitude、vehicle_control_mode、vehicle_rates_setpoint新版发布的是actuator_controls_0旧版或vehicle_rates_setpoint新版和对应的状态调试话题。主循环里第一件事就是检查vehicle_control_mode.flag_control_attitude_enabled如果姿态控制没有被使能输出清零飞机就不会乱动。这个标志位在多旋翼解锁和模式切换时非常关键不少人飞控“没反应”就是卡在这个标志位前面。2. 代码核心第一步四元数误差怎么算成期望角速率2.1 为什么姿态误差用四元数而不是欧拉角看代码之前先聊一个很多新手纠结的问题姿态误差为什么要用四元数直接拿期望roll、pitch、yaw减去当前roll、pitch、yaw不是更直观吗直观是直观但不好用。欧拉角在大角度时有万向锁问题而且角速度方程是非线性的。你看着好像10度误差就是10但如果姿态同时变化roll、pitch、yaw三个通道之间会产生交叉耦合。四元数没有奇异性计算也是纯代数运算适合嵌入式实时计算。PX4内部姿态估计、姿态控制基本都统一用四元数只有到最后显示和调试才转成欧拉角。你在日志里看到的vehicle_attitude里的q就是四元数四个数分别是实部和三个虚部一般写作w, x, y, z。理解四元数有个简单的类比它描述的是一个旋转轴和一个旋转角度比欧拉角多了一个“轴”的信息。正因为这样它才能表示任意姿态不会出现“转了几十圈之后角度绕晕了”的情况。2.2 误差四元数qe的计算和符号处理进入控制函数后首先从vehicle_attitude_setpoint里取出期望四元数qd从vehicle_attitude里取出当前四元数q。接下来核心代码就是求误差四元数Quatf qe qd * q.inversed();这里要特别说一下q.inversed()。四元数的逆对应的是“反向旋转”所以期望姿态乘以当前姿态的逆结果就是从当前姿态转到期望姿态的旋转量。这就像你出门前先知道自己脸朝哪边当前朝向再查地图上应该朝哪边期望朝向中间差的那一下转向就是误差四元数。紧接着会看到一段非常容易被忽略的代码if (qe(0) 0.0f) { qe -qe; }这个负号处理是干什么的四元数有个性质q和-q表示同一个姿态。如果qe(0)也就是实部是负数说明误差旋转大于180度控制上就出现了“绕远路”的情况。把它取反相当于保证误差旋转路径小于180度让飞机走最短路径。这句话很重要有次我在调试某型机时发现飞机总爱“绕一下”再回中排除了半天最后就是这里被改坏了。2.3 从四元数误差到期望角速率误差四元数拿到手接下来怎么转成角速率期望在经典代码里是直接取四元数的向量部分乘系数_rates_sp(0) 2.0f * _params.roll_p * qe(1); _rates_sp(1) 2.0f * _params.pitch_p * qe(2); _rates_sp(2) 2.0f * _params.yaw_p * qe(3);这个式子背后有个小角度近似。当误差角比较小时四元数的虚部近似等于sin(角度/2) * 转轴分量乘以2就约等于角度误差本身。姿态环的P增益_params.roll_p、_params.pitch_p、_params.yaw_p再对应乘上去得到的就是期望角速率。这里就解释了一个常见疑问为什么PX4姿态环参数默认是6点多、2点多而不是位置环那种0点几因为姿态环输入是误差旋转向量无单位输出是角速率rad/sP的单位就是1/s数值大小取决于系统响应需求。把P调得很大角度误差稍微有一点角速率期望就很大飞机看起来“很贼”P太小飞机则迟钝。有些新版本引入旋转矩阵误差向量e_R来替代直接取四元数虚部公式复杂一点但控制思想一样都是把姿态误差映射到机体三轴期望角速率上。往后看数据时不要因为变量名不同就发怵。2.4 角速率期望的限幅和保护期望角速率算出来后不能直接丢给角速度环必须限幅。原因很简单如果某个大机动导致误差四元数很大rates_sp可能瞬间变成几十rad/s电机根本没有能力跟反而可能触发保护或者让积分饱和。代码里常见的做法是对三个轴分别限幅_rates_sp(0) math::constrain(_rates_sp(0), -roll_rate_max, roll_rate_max); _rates_sp(1) math::constrain(_rates_sp(1), -pitch_rate_max, pitch_rate_max); _rates_sp(2) math::constrain(_rates_sp(2), -yaw_rate_max, yaw_rate_max);对应的可调参数一般叫MC_ROLLRATE_MAX、MC_PITCHRATE_MAX、MC_YAWRATE_MAX单位是rad/s。我在调试某款500mm轴距的四旋翼时把roll/pitch限幅设为220度/s左右大约3.8rad/syaw设成150度/s试飞时大杆量横滚不会出现“瞬间翻过去刹不住”的感觉。这个值不是越大越好越大越考验内环和混控的能力太小的话姿态环在大偏差时输出受限位置环会抱怨“我让你转90度你半天没转到”。3. 角速率内环从期望角速度到力矩指令3.1 PID结构恢复到一条输出旧版mc_att_control在姿态环之后紧接着做角速率环。角速率环的输入是上一节算出来的_rates_sp反馈是当前角速率。当前角速率从哪里来这里不是从陀螺仪直接读原始值而是经过姿态估计器融合好的角速度来源在vehicle_attitude话题里带的那组angular_velocity字段通常对应机体系下的三轴角速度。误差计算非常简单Vector3f angular_rate_error _rates_sp - _rate;其中_rate就是当前角速度。接下来是PID分配。经典代码里可以看成这样_actuators_control(0) roll_p * error(0) roll_i * integral(0) roll_d * derivative(0); _actuators_control(1) pitch_p * error(1) pitch_i * integral(1) pitch_d * derivative(1); _actuators_control(2) yaw_p * error(2) yaw_i * integral(2) yaw_d * derivative(2);输出对应三个轴的期望力矩。注意这里的“力矩”在PX4内部并不是带单位的物理力矩而是归一化到[-1,1]的控制量。下游混控器拿到这个控制量再按照机架力臂把正负力矩映射到各个电机的油门增量上。P项负责“对当前误差立即反应”I项负责“消除静差”D项负责“对误差变化速率提前制动”。姿态控制里P和I用得最多D项通常很小增益大大会让系统变得非常敏感尤其是陀螺噪声一大D项会把噪声放大成高频抖动。3.2 积分项为什么要限幅PID控制里积分项最麻烦。只要误差长期存在积分就一直累积。角速率误差持续存在通常有两种原因一是外部持续干扰比如侧风顶着飞机二是执行机构饱和比如电机已经到最大油门还是不够。如果积分一直累积就会产生“积分饱和windup”结果是你松开摇杆后飞机还要抽一下甚至突然转向。PX4对积分项的处理不是简单加一个限幅到最后输出而是在控制器内部单独限制积分值。遇到过一个问题某次我改了大油门混控曲线后roll通道出现悬停时周期性抖动看日志发现integral已经接近限幅上限一碰限幅就反弹像是“呼吸效应”。后来把积分限幅调低抖动消失代价是存在一点轻微静态倾斜但远比抽风可接受。调试积分时也建议看日志曲线而不是只看手感。手感这东西会骗人特别是有风的天气你会以为飞机很稳其实积分项一直在慢慢爬。看actuator_controls和integral曲线能更客观。3.3 输出饱和和油门衰减逻辑角速率环输出的是三个轴的力矩期望但混控器最终输出要加到油门基准值上。大油门状态下电机已经接近满载剩余可调范围变小如果还用同样的P和I增益很容易激起振荡。PX4为此引入了油门衰减TPAThrottle Performance Attenuation逻辑用一个小公式根据当前推力减小P和I增益。简化理解就是油门越大姿态控制增益越低防止电机饱和时控制量被“截断”产生振荡。这类参数在旧版里能看到类似MC_TPA_BREAK_P、MC_TPA_BREAK_I、MC_TPA_RATE_P之类的配置。平时默认值可用但如果你飞重载无人机或者电机选型偏小、油门经常顶在80%以上建议专门试一下油门80%附近的姿态响应。我见过某型植保类无人机载重上去后横滚一打就“弹屁股”最后就是靠TPA把高油门段的P压下来解决的。3.4 新版拆分后接口怎么接新版固件里角速率环被拆到mc_rate_control模块后mc_att_control的输出不再是actuator_controls_0而是vehicle_rates_setpoint。也就是说mc_att_control只发“期望转多快”真正算力矩的是另一个模块。这个拆分很容易让人误会“姿态环被砍了”其实没有只是代码搬家。你调参时仍然要调MC_ROLL_P姿态环和MC_ROLLRATE_P角速率环只不过两个模块的XML参数定义文件分开了。排查问题时还有一个不同以前崩溃日志里一堆mc_att_control的耗时现在要看两个模块的时间戳是否连续、有没有出现“断档”。我在一次升级固件时遇到过新版角速率环偶发超时现象是飞机每隔十几秒轻微顿一下最后查了一圈发现是任务优先级配置问题。遇到这类问题优先打开watchdog和调度统计打印。4. 参数速查和一套实用的调参顺序4.1 核心参数对照表调参前先把参数和作用的对应关系理清。下面这个表是我常用的速查表适用于绝大多数多旋翼参数名所属环作用MC_ROLL_P / MC_PITCH_P姿态环角度误差到期望角速度的比例越大越“跟手”MC_YAW_P姿态环偏航回路比例一般比横滚俯仰小防止偏航振荡MC_ROLLRATE_P角速率环角速度误差到力矩的比例决定快速消除角速度偏差的能力MC_ROLLRATE_I角速率环消除角速度静差抗风MC_ROLLRATE_D角速率环对角速度误差变化率做阻尼默认值很小或0MC_ROLLRATE_MAX姿态环输出限幅限制三个轴最大角速度期望MC_TPA_*输出衰减高油门时降低增益防止电机饱和振荡注意不同固件版本里默认值相差很大。比如新版默认P值普遍比旧版低因为混控器、滤波器甚至机架默认参数都变了。网上抄别人的参数最好先确认版本和机型否则很容易出现“我用了某大神参数飞机抖成筛子”的情况。4.2 从内到外还是从外到内调参顺序是个争议话题。我的习惯是先内环、后外环也就是先调整角速率环再调姿态环。理由很简单外环依赖内环的响应内环都没稳住外环调得再好也没用。具体流程大概是这样的。第一步先把姿态环P降到一个偏低的水平比如默认值的60%-70%避免外环干扰内环判断。第二步加大角速率环P直到悬停时能听到电机有轻微但连续的“嗡嗡”变化感飞机出现微小高频抖动这时候回退20%左右。第三步加角速率环I让飞机在悬停时能扛住轻微侧风观察有没有低频“晃头”现象。第四步加姿态环P让飞机跟随摇杆更紧但不要追求“完全没有松弛感”。最后才是微调D如果还有轻微振荡且确认机械装配没问题再一点点加D。试飞时不要一上来就大机动。我一般先在约1.2米高度悬停观察三个轴的rates_sp和实际角速度之间的跟随再小幅打杆。确认没有振荡后再做前后左右平移和刹车测试。刹车那一瞬间最容易暴露内环D不足或外环P过大的毛病。4.3 日志里重点看几条曲线会用日志比会调参更重要。很多人说“不知道看什么”我这里给一个每次试飞必看的日志清单vehicle_attitude_setpoint.q和vehicle_attitude.q的差值这是姿态环的“输入误差”。如果误差持续很大说明姿态环P太低或角速率上限不够。vehicle_rates_setpoint和vehicle_attitude.angular_velocity这是内环的“输入误差”。如果这个误差大且长时间不收敛内环P和I不够。actuator_controls_0的roll/pitch/yaw看控制器输出的“用力程度”。如果频繁触顶到-1或1说明下限幅在起作用控制量已经饱和。电机转速或PWM/DSHOT占空比看是否存在单轴方向电机持续拉满。有一次我调一架尾桨偏弱的机子悬停时yaw通道的输出一直偏向一侧40%以上刚开始以为混控有问题后来发现是尾电机效率下降电流一大转不动。这就是只看控制量不看执行机构的坑。5. 实飞中容易踩的坑和排查思路5.1 高频振荡先怀疑刚度和滤波再怪PID高频振荡最常见的现象是飞机悬停时“嗯嗯嗯”地响像在快速颤振。新手往往第一时间想着调低P其实很多时候不是P高了而是机架结构太软或者陀螺仪滤波器参数不合适。某次我在一台折叠机上遇到高频振荡把MC_ROLLRATE_P从默认降到一半仍然抖。后来检查发现折叠机臂的锁定结构有间隙电机高转速时整个机臂在共振。换了一套锁紧结构后P回到默认值也没问题。所以高频振荡第一步检查机械第二步看陀螺仪原始振动数据第三步再动PID。很多人跳过前两步结果P越调越低飞机越飞越“肉”。5.2 低频晃动多半是I和重心低频晃动和电机“呼吸式”的周期摆动常见原因是角速率环I过强或者重心明显偏移。I过强时控制器把持续偏差全部积分掉当外部条件变化比如风停时积分项还保留着旧值就会造成“过冲—反向积分—再过冲”的循环表现出来就是几秒一次的晃动。重心偏移也会造成类似现象。飞机重心不在几何中心悬停时需要持续输出一个偏置力矩。这个偏置靠积分补偿没问题但重心偏得太狠积分值一直很大动态响应就会变脏。排查办法是看roll和pitch通道的积分项是否持续大值如果是先调整电池位置或飞控安装方向不要硬调PID。5.3 悬停缓慢漂移先查零偏和估计器还有一种情况各通道都很稳但飞机就是缓慢朝一个方向漂。很多人以为是姿态控制问题其实姿态控制器没问题它把姿态稳得住。漂移来源于位置环或者估计器输出有偏差常见原因包括陀螺零偏没有完全校准、加速度计振动校准不佳甚至气压计受到气流干扰。落到日志上就是姿态误差很小、角速率误差也很小但水平速度慢慢变大。这时应该查vehicle_local_position的速度估计而不是改MC_ROLL_P。遇到过一位朋友悬停总往左后漂折腾了两周控制参数最后发现是飞控安装减震棉老化导致加速度计受到机架共振影响估计器速度估计有缓慢偏差。5.4 消息超时和模式切换抽风mc_att_control是一个uORB消费者依赖上游持续发布数据。如果vehicle_attitude_setpoint或者vehicle_attitude的消息发布中断模块会进入超时保护输出被清零或保持最后值飞行器就会突然“松劲”。典型表现是悬停时突然下垂一下然后又恢复。排查这类问题看消息时间戳timestamp有没有跳变。我遇到过一次奇怪现象解锁后第一次推油门飞机突然左右晃一下查日志发现vehicle_attitude_setpoint在解锁后延迟了几百毫秒才开始更新位置环和姿态环之间出现数据空隙。最后通过调整导航模式的同步逻辑解决。如果你在本地复现类似问题怀疑模块调度问题可以把mc_att_control的开始日志打印打开看任务启动后是否有持续循环。6. 这块后续还能怎么深入最后聊点继续进阶的方向。mc_att_control表面上是“算姿态误差再做PID”但真正深入下去会接触到非线性姿态控制、执行机构饱和补偿、线性二次调节器LQR姿态控制、甚至基于模型的鲁棒控制。PX4代码里那一堆四元数运算和旋转矩阵就是为这些高级算法打基础的。我个人建议下一步做两件事。第一把姿态环的四元数误差推导自己在纸上推一遍从旋转矩阵到小角度近似搞清楚2 * qe(1)那个系数为什么是2而不是别的东西。这一关过了很多代码就不再是黑盒。第二自己搭仿真环境或者半实物平台把mc_att_control的输入改成正弦摆动姿态信号观察输出和实际角速度的跟踪情况。这比闷头调机更容易建立“控制器到底在干什么”的直觉。这一篇先讲到这儿。mc_att_control里还有一些细节例如大机动的混控优先级、故障保护时的控制权切换、以及姿态估计短暂失效时的降级逻辑这些我都还没展开后续有机会继续写。源码分析这种事情看十遍不如自己改一遍再跑一遍祝你早日调出又稳又跟手的姿态环。
