有段时间我一直在折腾一台中型四轴拿到手之后最明显的问题是飞控切到位置模式往前推杆让它飞出去到达目标点之后机头会轻轻点一下机身前后晃两三次才稳住。这个现象不致命却很影响手感也让我下定决心把PX4里位置环、速度环这两个模块彻底啃了一遍。说实话网上讲PX4姿态控制、角速率控制的文章很多但真正把外环这两个环讲透、讲到能指导调参和排查问题的很少。这篇就当作我实战系列第十二期的内容把位置环和速度环的模块结构、信号链路、参数逻辑和排错思路一次说清楚。这篇内容适合正在用PX4做二次开发或者调试多旋翼的朋友尤其是当你发现飞机“到点刹不住”、悬停慢漂、offboard跟踪指令发飘的时候大概率问题就出在位置环或速度环而不是姿态环。读完你应该能回答三个问题这两个环分别在哪段代码里干活、它们各自输出什么、出故障时怎么从日志里反推根因。1. 先看清全貌位置环和速度环在整条控制链路中的位置1.1 一次起飞中各个控制器到底按什么顺序干活先从“一次正常飞行”说起这样能直观理解控制链路的顺序。你在地面站点击起飞或者手动推油门让飞机离地飞控内部每个控制周期里大致会经历这样一个流水线先看“我要去哪”再看“我在哪”然后算“差多少”再把差值换算成指令一级一级往下传。具体到PX4多旋翼完整链路是这样的期望轨迹可以是航线点、offboard下发的坐标或者摇杆映射出的期望速度先进入位置环位置环比较期望位置和当前估计位置输出一个期望速度这个期望速度再送给速度环速度环比较期望速度和当前估计速度输出期望加速度期望加速度会转化为期望倾斜角砸给姿态环姿态环比较期望姿态和当前姿态输出期望角速率角速率环比较期望角速率和当前角速率输出期望力矩力矩最后进混控器变成四个电机的转速指令。也就是说位置环和速度环通常被合在一起称为“外环”而姿态环加角速率环是“内环”。外环解决的是“飞机在空间里怎么移动”的问题内环解决的是“飞机朝哪边倾斜、转多快”的问题。两者各自独立又级联在一起位置环的输出恰好是速度环的输入速度环的输出又恰好是姿态环的期望值一环扣一环。1.2 为什么不直接把位置误差换算成倾斜角非要三级嵌套这是大家刚接触时最容易困惑的点。直觉上位置偏了1米我把飞机往目标方向倾斜个5度不就行了吗为什么还要先算速度、再算加速度、再转成姿态角绕一大圈关键在于干扰和惯性。飞机在空中受到风、重心偏移、螺旋桨振动各种扰动姿态角的变化并不能直接且精确地换算成位置变化。中间的倾斜角只是一个手段最终目标永远是把位置和速度同时拉回期望值。如果把位置误差直接映射到姿态角遇到一阵横风姿态为了修正位置会做剧烈摆动位置却因为机体惯性跟不上两者就会打架飞机会“点头”或者“甩尾”。所以PX4采用级联控制每个环只负责一个物理量的调节内环跑得快负责抵抗快速扰动和稳定姿态外环跑得慢负责修正位置误差给内环提供合理的期望信号。这种带宽分离的做法也让每一级都能独立调参——速度环不稳了不用去翻姿态环参数姿态环抖了也不用动外环。理解这一点后面所有调参和排查都能顺下来。2. mc_pos_control内部拆解位置环速度环到底写在哪些代码里2.1 模块入口与主循环250Hz的控制周期是怎么攒出来的在常见PX4版本中位置环和速度环实现在src/modules/mc_pos_control这个模块里模块主体是一个独立的任务通过uORB消息和姿态环、位置估计模块通信。它的主循环会先等一批“输入”到齐才启动计算当前本地位置估计、当前速度估计、当前姿态四元数、期望位置/速度设定值、飞行模式开关量等。主循环按照固定的控制周期执行常见配置下是250Hz也就是4毫秒跑一次完整的外环计算。之所以选择这个频率是因为外环面对的物理量位置和速度变化相对慢4毫秒的周期已经足够平滑同时又比多数桨叶的振动频率低很多能天然避开一部分高频噪声。如果你去改这个循环周期会发现控制效果不是线性变好——跑太快反而容易把传感器的微小抖动当成速度误差放大跑太慢则位置跟踪会显得黏滞。整个模块跑完后会发布两类关键输出一类是期望加速度和期望推力方向用于给姿态环做设定另一类是垂直方向的油门期望值。水平位置控制和垂直高度控制在同一个模块里并行处理只是它们对应的控制律和输出接口不一样这也是为什么下文要分开讲。2.2 位置环外环为什么一个P控制就够用先看水平位置环它的代码逻辑很朴素取当前位置与期望位置的误差乘以一个位置增益得到期望速度再把外部给定的速度前馈叠加进去。写出来就是期望速度 MPC_XY_P * 位置误差 期望速度前馈这里MPC_XY_P的单位是1/s物理含义很直白每米位置误差产生每秒多少米的修正速度。如果位置误差是2米MPC_XY_P为1.0那期望速度就是2米/秒飞机以2米/秒的速度往回飞。为什么位置环不用PID全套我最早也怀疑过后来想明白了在这个级联结构里位置环的“积分”功能实际上被速度环的积分替代了位置误差持续存在最终会反映到速度环积分项上不需要位置环自己再攒一个积分项。而微分项在位置环里很容易放大传感器噪声并且会让飞机在接近目标点时过早减速出现“到不了点”的黏滞感。所以单P加前馈是工程上最稳的组合不是开发人员偷懒。需要注意的是位置环算出来的期望速度会经过限幅这个限幅值通常由MPC_XY_CRUISE最大巡航速度决定。如果飞机离目标点很远位置误差很大期望速度也会被钳在巡航速度上飞机用恒定速度飞过去。这就是为什么航线飞行时即使误差再大飞机也不会越来越快直至失控。2.3 速度环内环PID、加速度前馈、限幅与倾斜角转换位置环算出期望速度之后代码接着进入速度环。速度环的标准形式是PID加前馈期望加速度 MPC_XY_VEL_P * 速度误差 MPC_XY_VEL_I * 速度误差积分 MPC_XY_VEL_D * 速度误差微分 期望加速度前馈其中速度误差就是期望速度减去当前估计速度。用力学的语言讲加速度误差本质上是“为了改变当前速度需要产生多大的加速度”。有了期望加速度再把重力向量考虑进去就能算出机体为了产生这个水平加速度需要倾斜多大的角度然后把这个倾斜角作为期望姿态发送给内环。这里的排序有讲究。位置环的期望速度是“慢变量”速度环的期望加速度是“快变量”但无论如何加速度不能无限大否则飞机倾角会过大甚至翻过去。所以速度环输出有过几层限幅最大水平加速度限幅控制飞机加减速的猛烈程度、最大倾斜角限幅MPC_TILTMAX_AIR控制飞机最大倾斜角度。形象点说位置环告诉你“该往哪走”速度环告诉你“用力多大”限幅则保证这一脚油门踩下去不会把车开翻。垂直方向的控制逻辑类似只是多了一个高度外环高度环把高度误差转换成期望垂直速度垂直速度环再把垂直速度误差换算成期望油门也就是推力。这两个小块也在mc_pos_control模块内只不过垂直速度环使用的是油门输出而非倾斜角所以调参参数是另一组。除非你做的是农业无人机或者载重平台否则默认参数一般不用动垂直方向。3. 模式切换时位置环速度环各自的路由与旁路逻辑3.1 Position / Altitude / Stabilized / Offboard哪些环在工作PX4支持多种飞行模式不同模式下外环并不是一直都在跑。这里用一张表把常见模式下的“环路接线”情况列出来理解这张表基本上就能看懂为什么某些模式飞机很“跟手”某些模式却需要你自己稳住姿态。飞行模式位置环速度环姿态环角速率环摇杆/指令映射Stabilized自稳不参与不参与不参与参与摇杆直接映射机体角速率Altitude定高水平不参与水平不参与参与参与水平摇杆映射期望姿态角Position位置参与参与参与参与水平摇杆映射期望速度Offboard外部控制参与按设定值类型参与参与参与外部下发位置/速度/姿态指令定高模式是一个特别典型的“部分旁路”场景。它只锁定高度水平方向完全由操作者通过姿态角来控制。所以定高模式下位置环和水平速度环都不计算高度环和垂直速度环正常工作。如果你在定高模式下发现飞机“稳不住高度却不漂”那问题大概率出在垂直速度环而不是水平方向。Position模式则会把外环全链路跑起来摇杆给出的实际上是期望速度位置环负责在摇杆回中时把飞机稳在当前点上。Offboard更灵活外部可以通过MAVLink直接下发速度设定值或者位置设定值下发速度设定值时位置环被旁路但速度环仍然参与下发位置设定值时两个外环都在工作。3.2 起飞、降落与过渡阶段位置环速度环的特殊照顾除了模式切换飞行阶段也会影响外环行为。以起飞为例飞控不会让位置环在飞机还没离地时就把期望速度输出成一大坨因为机轮/脚架还撑着飞机位置误差根本没法通过倾斜角来修正。在起飞阶段期望位置、期望速度都会被限制在一个可信范围内等检测到机体已经离地、垂直速度超过阈值位置环才会逐步放开限制。降落阶段同理。接近地面时速度环的输出会被收紧水平期望速度被压低垂直速度也会按照预设的降落速率下滑避免飞机在触地瞬间还带着较大的水平速度往前搓。最容易踩的坑是着陆后忘记切模式飞机还停留在位置模式它检测到位置误差后仍然试图修正位置结果在地面上“蹭”着走。这种情况不是位置环坏了而是模式没有随状态切换。我见过不少开发者在这块翻过车用offboard做自主起降起飞和降落阶段的外环设定值没有做阶段处理导致飞机在离地瞬间有个猛冲的动作。实际上只要在起飞阶段只给垂直速度设定、不给水平位置设定等离地后再切完整的期望位置控制这个问题就消失了。简单说外环跑得再漂亮也要看是谁在给外环喂设定值。4. 调参不要上来就抄从增益单位看懂每个参数4.1 从物理意义理解MPC_XY_P、MPC_XY_VEL_P和MPC_XY_VEL_IPX4位置环、速度环最常用的几个调参参数网上都能搜到默认值但很少有人讲它们背后的物理含义。我建议在动参数之前先把单位和数据流理清楚否则你只是在试运气。MPC_XY_P位置环P增益单位1/s。频率越高位置误差转化为期望速度的比例越大。调大它飞机回中速度更快但过大时会在目标点附近来回冲。MPC_XY_VEL_P速度环P增益单位1/s。它决定每米/秒的速度误差产生多大加速度。增大它速度修正更快但过大时飞机会出现高频振颤电机声音会变得尖锐。MPC_XY_VEL_I速度环积分增益单位1/s²。它负责消除持续的稳态误差比如受风或者重心偏移导致的持续漂移。积分太大会引发缓慢的周期摆动。MPC_XY_VEL_D速度环微分增益。理论上它能提前抑制速度变化实际上一旦机体振动稍大微分项就会放大噪声绝大多数场景下我建议把D项设为0靠前后馈和P项就够了。举个例子MPC_XY_P从默认的1.8调到2.5很多人会觉得“位置环更强了飞机应该更跟手”实际上只改了位置误差到期望速度的比例。如果你的速度环跟不上这个期望速度飞机反而会过冲。这就像你给一个本来跑不快的引擎换了更大的油门踏板行程结果不是车更快而是轮胎狂打滑。4.2 标准调参顺序为什么必须从内往外调外环依赖内环执行指令所以调参铁律是先角速率环再姿态环再速度环最后位置环。内环不稳定外环调得再完美也没有意义甚至会出现“位置看着还行但姿态在疯狂抖动”的假象。我实际调试时的顺序是这样的先在自稳模式下调角速率环观察打杆响应和回中情况然后切定高模式调姿态环接着切Position模式把速度环P和I一点点加上去在空旷场地做加减速往复测试最后才动位置环P。每一个环节都要用日志或者飞控状态页面观察实际曲线而不是只看手感。下表是高频现象和参数的对应关系可以直接存下来当参考现象优先怀疑的参数调整方向到达目标点后来回冲停不住MPC_XY_P减小飞行过程中高频抖、电机发烫MPC_XY_VEL_P / MPC_XY_VEL_D减小D优先清零受风后回中慢有固定偏移MPC_XY_VEL_I增加悬停时慢周期前后漂MPC_XY_VEL_I / MPC_XY_P减小积分或者位置环P松杆瞬间机身点头MPC_ACC_DEC减速限幅减小减速加速度限幅4.3 前馈与限幅的配合为什么有时候调增益不如调限幅很多人忽略了限幅参数对控制效果的影响。我遇到过飞机到达目标点时严重过冲一开始怀疑速度环P太小加了P之后过冲确实变小但飞行中高频抖动又来了。后来查日志发现期望加速度已经经常顶到限幅值上速度环P再大也白搭反而是把减速加速度限幅调低一点过冲立刻改善整机还更安静。这说明一个道理外环的“性格”不只看增益还要看限幅。增益决定小误差时的细腻程度限幅决定大误差时的猛烈程度。比如希望飞机快速跟随航线但到点不冲可以把MPC_XY_CRUISE设高一点同时把水平加速度限幅设得略低这样飞过去的途中速度快接近目标时刹车就柔和。限幅参数是有物理边界的调之前先问自己一个问题这台飞机在某个速度下用最大倾角减速刹车距离是否足够不够的话再猛的位置环也不可避免会过冲。5. 信号链路与期望值平滑摇杆、航线与offboard的融合方式5.1 期望值从哪里来又怎么进的位置环位置环和速度环只负责“算”它们不关心期望值到底来自哪里。PX4有一套完整的状态机来生成期望位置/速度设定值手动位置模式下摇杆的偏移量会经过一个速度映射曲线变成期望速度设定值如果摇杆回中就会自动启用位置保持期望位置锁定在当前位置上。航线飞行时航线点管理器会输出下一段航线的期望位置和期望速度。重点在于这些期望值进入位置环/速度环之前一般会经过平滑和限速处理。因为摇杆瞬间打满或者航线切换产生跳变如果不做平滑位置环会瞬间看到一个很大的位置误差速度环也会瞬间收到一个巨大的速度误差机身就会出现明显的猛冲或者点头。PX4里这种平滑有时通过期望速度的速率限制完成有些版本还会做加速度加加速度限制。如果你做offboard时自己下发期望速度务必记得在外部也做同样的平滑否则即使位置环参数完全正确飞机也会看起来很“毛糙”。5.2 速度前馈的真正价值以及Offboard场景为什么特别敏感前馈在位置环和速度环里都以“期望速度前馈/期望加速度前馈”的形式出现。有没有前馈区别在于纯反馈是看到误差才动作前馈是提前知道目标在移动先给出对应的输出误差自然而然就小了。拿offboard跟踪一个移动目标来举例。如果目标轨迹以5米/秒的速度移动你只给位置环期望位置位置环必须等真实位置落后目标一段距离之后才能靠位置误差产生出每秒5米的期望速度。这一段滞后会始终存在表现为“追着跑但永远差一截”。如果offboard同时下发期望速度5米/秒作为位置环前馈位置环立刻把期望速度设定为5米/秒就省掉了等待误差累积的过程跟踪精度显著提高。所以我的建议是只要做offboard优先采用位置速度同时下发的模式如果只下发位置设定值一定要在外部自己算一个期望速度前馈。很多开发者觉得offboard模式“飘”追不上目标实际上不是PX4控制算法烂而是设定值链路本身少了前馈那一环。速度环同理如果上位机能够算出期望加速度也把它作为前馈发给速度环这样飞机在转弯和加减速时会更平顺内环压力也小很多。5.3 滤波与估计值对速度环的影响速度环的输入之一是当前速度估计值而这个估计值来自位置估计模块融合加速度计、GPS或者光流后的结果。如果传感器振动大或者滤波不恰当估计出的速度就会带噪声速度环的微分项和前馈项就会把这些噪声放大最终表现为电机高频颤动。排查这类问题我通常看一眼控制量输出曲线和速度估计曲线如果速度估计上有一堆毛刺而控制量上也有一模一样的毛刺说明噪声是从传感器侧灌进来的。这时不用急着调速度环增益先把机架减振和滤波配置整理好再说。有一次我调某台四轴速度环D项已经降成0了飞机还是高频抖最后发现是飞控减振棉装反了加速度计在剧烈振动源上。硬件问题比软件问题更难找但它往往藏在滤波和噪声的细节里。6. 一次实际位置震荡的排查复盘从现象到根因的完整链路6.1 现象飞机到点后“刹车点头”但悬停尚可回到文章开头说的那台中型四轴它的症状很清晰水平飞行速度不低切换目标点或者快速往前飞然后停稳的时候机头明显向下点一下伴随一两下纵向摆动但悬停状态非常稳没有慢漂也没有高频抖。姿态环在这个平台上一直表现正常自稳模式下手感也很好。这类现象其实很有代表性它说明问题多半不在内环而在外环的速度规划和减速逻辑。悬停稳说明位置环速度环在小误差区域工作正常而“刹车点头”通常发生在大误差、大速度减速的过程中。通俗地讲飞机从高速往前飞的状态突然收到“停在这里”的指令速度环拼命减速期望加速度顶到限幅值机身为了产生这个减速度需要抬头但因为机身前倾过大水平减速的同时也在损失升力于是机头先下沉一下整体高度也跟着掉一点点。6.2 日志定位在Flight Review里找三个信号排查这类问题我最依赖的工具是飞行日志回放页面重点看三个信号线期望位置与实际位置的偏差、期望速度与实际速度的偏差、期望加速度是否经常“撞墙”。实测日志显示这个平台在刹车阶段期望加速度确实频繁触及水平加速度限幅值。与此同时期望速度从巡航速度快速降到0速度误差一大速度环P项输出立刻饱和。这里的关键判断点就是当速度误差大且加速度输出已经饱和时再增加速度环P也不会让刹车更快只会让饱和期间的微分项带来新的抖动。因此初始的调参方向应该放在“减速目标”和“减速限幅”上而不是盲目加大速度环增益。6.3 根因、修改与验证参数可以改但更值得改的是限幅随后我把减速加速度限幅往下调了一些同时也微降了最大巡航速度让刹车阶段的加速度需求落在速度环的有效输出范围内。重新试飞后刹车点头幅度明显减轻机身只会有一声轻微的“嗯”就稳定下来不再来回荡。为了确保不是靠降性能换稳定我又把位置环P微调回去一点确认小偏差区域的响应速度没有退化。结果很有意思整体飞行体验反而更好了原因很简单——以前的“跟手”其实是大增益加饱和限幅带来的错觉飞机动作猛但不精细现在限幅合理速度环始终工作在非饱和区间控制量干净得多姿态环接收到的期望倾斜角也更平滑。6.4 这一类问题通用的排查顺序这次排查之后我整理了一个通用思路送给同样被外环问题困扰的开发者先看日志里速度环输出是否常驻限幅是则先调限幅再看速度环期望值与实际值之间的跟随情况若跟不上调速度环P和I两者都正常却仍然过冲再动位置环P。最后才是去怀疑滤波、振动和硬件减振。我个人的做法是先让飞机在offboard模式下用指令速度画大矩形来回飞同时记录巡航速度变化时的刹车行为。这一步能快速暴露减速限幅和速度环增益是否匹配。等你把这一段跑顺了再回到位置保持和航线模式验证位置环的问题通常会少很多。外环调试是一个逐级收敛的过程跳级调参只会让飞机看起来“哪里都不对”实际上每一层都还说得过去。这轮折腾完之后我对PX4外环的理解比之前深了一截。它本质上不是什么玄学就是一层层带宽分离的物理量换算位置环管方向速度环管力度限幅管边界前馈管预测。下次再遇到飞机“不听话”先别急着怀疑固件打开日志看一眼你的位置环和速度环各自在哪个环节掉了链子往往答案就写在信号曲线里。