先问一个所有玩过APM飞控的人都会遇到的问题你在调横滚或俯仰通道的时候有没有见过这种奇怪现象——误差明明只有几度期望角速度却直接顶到上限误差比较大的时候反而觉得飞机响应“肉”得不行像有什么东西在背后拽着它。我以前排查这类问题总是习惯性去动内环PID结果越调越毛最后发现罪魁祸首根本不在PID而在姿态环和位置环最前面的那个“误差转化器”也就是ArduPilot代码里反复出现的sqrt_controller。这个函数在APM源码里出现频率极高姿态控制用它、位置控制用它、定高控制也用它的变体。但很多人第一次读源码的时候都会懵这不就是个比例控制器吗为什么还要切开一段外面再套一个平方根网上讲这个函数的资料不少但大多只贴公式不说推导或者只讲姿态环不讲位置环。今天这篇我就从数学推导开始一步一步把sqrt_controller的来龙去脉拆干净包括它为什么这么设计、参数到底怎么理解、在ArduPilot的代码里是怎么被调用的、实际调参时最容易踩哪些坑。适合谁看正在调APM/CubePilot飞控姿态和定高的人想搞懂ArduPilot控制链路源码的开发者以及所有对“分段控制器设计”感兴趣的飞控算法爱好者。看完之后你不仅能看懂源码里那几行公式还能自己判断参数该怎么调。1. 认识sqrt_controller一段被太多人忽略的“外环”1.1 先搞清楚它在APM控制链路里的位置APM飞控的姿态控制不是一层而是串级结构。外环把用户期望的角度和目标姿态做差得到姿态角误差然后这个误差被转换成期望角速度交给内环内环再通过PID把实际角速度拉向期望角速度最后输出电机指令。这个结构里外环最核心的“误差到期望速率”转换用的就是sqrt_controller。以横滚通道为例在ArduPilot的姿态控制代码里伪代码大致长这样// 姿态角误差目标横滚角 - 当前横滚角 float roll_error roll_target - roll_current; // 把角度误差转换成期望角速度 float rate_roll_target sqrt_controller(roll_error, rate_max, ATC_ANG_RLL_P);也就是说ATC_ANG_RLL_P这个角度增益并不是像很多教程说的那样“越大气越硬”它实际上是作为sqrt_controller里的斜率参数参与运算。同理位置控制里位置误差到期望速度的转换也调用了同一个函数// 水平位置误差米转换成期望水平速度米/秒 float vel_target sqrt_controller(pos_error, pos_max_speed, PSC_POSXY_P);所以如果你把sqrt_controller当成“一个会改变输出的比例控制器”很多控制链路中的疑难杂症都会迎刃而解。它决定了外环对大误差和小误差的响应方式直接影响飞控的“手感”和定高/定点模式的稳定表现。1.2 没有它之前纯比例控制到底出了什么问题为什么APM不直接写rate kp * error纯比例控制看起来简单但实际工程里有非常明显的毛病。小误差时如果kp不够大输出太小飞机收敛很慢表现为“回中无力”如果为了小误差响应快把kp调大大误差时输出会非常吓人。比如横滚角误差30度kp若取20期望角速度就是600度每秒这个数值远超飞机物理上能做到的角速度结果就是内环PID饱和、电机瞬间拉满、姿态剧烈震荡。那加一个硬限幅行不行比如期望角速度最大只给300度每秒。硬限幅在角度误差从30度慢慢减小到限幅区域的边界时输出会突然从300跌落到线性区对应的值这种输出跳变会让内环和电机产生不自然的突加突减飞起来的感觉就是一顿一顿的随时要抽风。sqrt_controller的思路很直接小误差时保持比例控制的线性响应确保手感细腻大误差时不让输出继续线性暴涨而是按平方根曲线增长。平方根的增长速度远慢于线性所以能有效避免输出顶穿内环能力同时曲线本身连续不会出现硬限幅那种“撞墙”式的跳变。2. 手把手推导误差到期望速率的数学过程2.1 设计目标与分段函数框架在推导之前先把设计目标列清楚。这个控制器要满足下面几个约束函数必须单调误差越大输出越大不能出现“误差增加输出反而减小”的情况。函数必须连续至少在分段点不能有跳变否则内环和电机就会感受到阶跃。小误差范围里保持线性斜率为kp这样比例控制的工程经验可以继续用。大误差范围里输出增长要慢下来比线性更“柔和”让内环来得及跟踪。参数不能太多最好两个参数就能表达清楚一个控制小误差灵敏度一个控制大误差响应尺度。带着这些约束自然得出一个分段函数的框架当 |error| 比较小时 rate_des kp * error 当 |error| 比较大时 rate_des f(error)其中 f 是增长慢于线性的函数关键问题就是两个分段点取在哪里以及大误差段的f(error)怎么选。2.2 线性段与边界条件先看线性段。在线性区控制器输出rate_des kp * error现在定义一个参数rate_max它代表线性段结束位置的输出值。也就是说在线性段的末端输出刚好等于rate_max。于是边界处的误差自然满足rate_max kp * error_boundary error_boundary rate_max / kp这是一个非常巧妙的参数设计rate_max的单位和输出一致是速率单位kp的单位是1/s两者相除正好得到误差单位。如果你设置kp10rate_max2那么线性段边界就是0.2个单位的误差。在姿态环里就是0.2弧度约11.5度。在这个范围内控制器就相当于一个普通P控制器输出最大到2。2.3 大误差段平方根曲线从哪来当误差超过rate_max / kpAPM的sqrt_controller采用下面的公式rate_des sign(error) * ( sqrt(2 * rate_max * (|error| - rate_max / kp)) rate_max )这个公式怎么理解我们可以从运动学里找一个直观类比。想象你开车目标是停在某个位置当前离目标还有一段距离。如果你用一个恒定的减速度a去刹车汽车允许的最大速度和剩余位移之间满足关系v^2 2 * a * s也就是v sqrt(2 * a * s)。这个关系非常像平方根曲线。在APM的公式里rate_max起到了“等效加速度”的作用。误差超过线性区的那一段也就是(|error| - rate_max/kp)相当于“距离线性区边界还有多远”。在这个额外距离上允许的提升速度按恒定“加速度”rate_max来计算得到sqrt(2 * rate_max * 额外误差)。然后还要叠加上线性段边界处的输出rate_max所以公式末尾是 rate_max。验证连续性。当|error| rate_max / kp时sqrt(2 * rate_max * 0) rate_max rate_max而线性段在边界上的输出也是kp * (rate_max / kp) rate_max。两个分段在交界处输出完全一致函数没有跳变。误差继续增大时平方根项随误差增大而缓慢增大整体输出远低于纯比例控制器同误差下的输出。举个例子感受一下。设kp5rate_max2线性边界是0.4。误差为1时纯比例输出5sqrt_controller的输出是sqrt(2 * 2 * (1 - 0.4)) 2 sqrt(2.4) 2 ≈ 3.55输出从5被压到了3.55。误差为10时纯比例输出50sqrt_controller输出是sqrt(2 * 2 * (10 - 0.4)) 2 sqrt(38.4) 2 ≈ 8.2从这里能清楚看到线性增长和平方根增长的区别。误差放大10倍比例输出放大10倍而sqrt_controller输出只增长了不到3倍。这就是它防止大误差下输出失控的关键。我来严谨说明一下上面这个APM实际使用的公式在分段点处导数并不连续。线性段的导数是kp而平方根段在边界处的右侧导数趋近于无穷大因为根号里的变量从0开始增长导数在0点附近发散。这看起来好像不够“平滑”为什么还要这样设计工程上是这样考虑的外环真正关心的不是数学上的光滑性而是输出连续、单调且有界增长。导数在边界处瞬变反映到实际效果里就是误差刚越过线性区时输出仍以较快速度提升之后迅速进入“低速增长”的平方根段。这种设计方案简单、计算量小、参数语义明确而且实际飞行验证下来表现够好。如果你追求分段点处导数也连续可以改造成另外一版公式rate_des sign(error) * ( 2 * sqrt(rate_max * kp * |error|) - rate_max )这个版本在边界处同样连续导数也等于kp。ArduPilot不同历史版本和不同分支里出现过类似变体。理解两种公式的差别能帮你看源码的时候不迷糊。2.4 量纲和参数单位再看参数量纲。姿态环里error单位是弧度rate_des单位是弧度/秒所以kp的单位是1/s。位置环里error单位是米rate_des单位是米/秒kp单位同样是1/s。Rate_max在姿态环里单位是弧度/秒或度/秒位置环里是米/秒它总与输出的单位保持一致。从线性段公式还能得到一个重要直觉线性段的时间常数约等于1/kp。kp越大时间常数越小系统小误差收敛越快。这个直觉对调参非常有用因为你可以根据期望的“回中速度”反推kp的大致范围。比如你觉得误差0.1弧度时希望期望速率达到1弧度/秒那么kp至少在10附近想更快kp可以往上加。3. 在APM里的实际用法与调参3.1 姿态控制里的调用方式与相关参数在ArduPilot的姿态控制里sqrt_controller被用于将横滚、俯仰、偏航的角度误差转换成期望角速度。这里最直接的参数是角度增益比如横滚通道的ATC_ANG_RLL_P俯仰通道的ATC_ANG_PIT_P偏航通道的ATC_ANG_YAW_P。这几个参数就是公式里的kp。而公式里的rate_max在姿态控制里来自飞控对最大姿态角速度的限制通常与AC_ATTITUDE_RATE_MAX或不同控制模式下的角速度限幅相关。注意rate_max本身不一定是固定的某些模式或某些轴会动态调整。打个比方如果飞控认为当前允许的最大横滚角速度是200度/秒那么这个200度/秒就是rate_maxkp除以它就得到线性区边界误差。调参时最常犯的错是只盯kp不看rate_max。如果你把ATC_ANG_RLL_P调得很大但rate_max比较小线性区会变得很窄飞控会很容易进入平方根段大误差响应反而变“肉”。这时候很多人以为是kp还不够大继续加kp结果线性区进一步压缩越调越怪。正确做法是同时检查kp与rate_max的比值确保正常飞行中常见的误差落在线性区内。3.2 位置和定高控制里的同款控制器sqrt_controller不仅用在姿态环位置控制和定高控制也在大量使用。水平位置控制中飞控将位置误差转换为期望速度再用期望速度与当前速度做差经过速度控制器输出期望加速度。这里的“位置误差到期望速度”就是sqrt_controllerkp对应位置增益PSC_POSXY_Prate_max对应最大水平速度限制PSC_VELXY_MAX等定高控制里也一样高度误差经过sqrt_controller转成期望垂直速度kp对应PSC_POSZ_Prate_max对应最大垂直速度限制。这里有个很实用的理解角度位置环的sqrt_controller本质上是给“刹车距离”留余量。当飞行器离目标点还有100米时期望速度可以很高当接近目标点只有1米时期望速度会被平方根曲线压得很低避免冲到目标点再刹车导致超调。所以很多人抱怨定点模式“最后一点点总是飘”大概率就是位置环的sqrt_controller参数没配合好误差到速度的映射在最后阶段衰减太快或太慢。3.3 调参建议与仿真验证我自己的调参习惯是先仿真后真机APM自带的SITL软件在环仿真非常适合做这件事。在SITL里改参数试飞可以快速观察误差和期望速率的曲线关系。第一步确认线性区边界。计算rate_max / kp看正常飞行和打杆时的典型误差是否落在边界内。如果大部分时间误差都大于边界说明控制器长期工作在平方根段响应偏软通常需要提高kp或提高rate_max。第二步从小误差响应看kp。给一个小的阶跃姿态指令观察姿态误差进入线性区后期望角速度是否足够大、是否平滑。若回中太慢增大kp。第三步从大误差响应看rate_max。打一个较大的姿态指令观察实际角速度是否经常触顶、是否出现内环饱和。如果饱和明显适当降低rate_max或调高内环限制。仿真验证的好处是随便改参数不用担心炸机。但要注意SITL里的电机模型和真机有差异仿真里调好的参数上真机后还要再微调尤其是内环PID这个经验不要省。4. 常见问题与踩坑记录4.1 关于rate_max的最大误解这是我在各个飞控群里看到最多的问题很多人把rate_max当成输出限幅认为期望速率永远不会超过这个值。这实际上是不对的。从公式就能看出来当误差进入平方根段后输出会超过rate_max。rate_max真正的作用是控制线性段范围和平方根段的增长速度而不是硬性封顶。只有在误差刚好等于线性区边界时输出才等于rate_max误差继续增大时输出是超过rate_max的。硬限幅是另一种控制器形态比如直接写rate_des constrain(rate_des, -rate_max, rate_max)。APM的sqrt_controller和硬限幅是两回事。如果心里总是默认“rate_max就是上限”你在分析日志时会非常困惑明明速度限制设了200度/秒为什么日志里期望角速度跑到250答案就是平方根段允许超过rate_max这是设计特性不是bug。4.2 内环跟不上外环指令怎么办外环sqrt_controller生成了期望角速度内环PID负责跟踪。如果内环能力不足期望角速度再合理也白搭。常见表现是飞行日志里Rate.Roll远低于Rate.RollTarget姿态误差迟迟无法收敛飞机感觉“软绵绵”。这时候有人会继续加大外环kp期望外环把目标打得更猛。结果内环更追不上还可能因为期望角速度突然增大导致电机饱和反而加剧振荡。正确做法是先检查内环。内环的比例项、前馈项是否够大角速度反馈是否正常电机响应是否在线性范围内。内环跟踪质量上去了再回头调外环的sqrt_controller参数。一个我踩过很多次的坑装了比较重的载荷后飞机转动惯量变大内环响应变慢我没有去动内环直接把外环kp降低了结果感觉飞机“更肉了”。后来看日志发现内环实际角速度一直够不着目标真正的问题是内环P太小。内环P提上来之后外环参数还原飞行手感马上正常。4.3 用工具和日志确认控制器工作状态APM生态里调试工具其实相当丰富很多人只用了Mission Planner最基本的刷固件和调参功能太浪费了。这里分享一套我日常用的工具链和检查日志的方法。Mission Planner地面站刷固件、调参、看实时状态、下载数据闪存日志。MAVProxy命令行地面站适合在无图形界面的服务器或树莓派上调试脚本化控制非常方便。FlightPlot / Mission Planner的日志曲线功能查看DataFlash日志曲线重点观察误差、期望速率、实际速率三个量。SITL软件在环仿真改参数、跑场景、验证算法几乎可以做到和真机一致的数据曲线。拿到日志后我一般会拉一条简单的曲线姿态角度误差、期望角速度、实际角速度外加一个由rate_max / kp算出来的线性区边界。然后看几个问题误差主要落在线性区还是平方根区期望角速度有没有频繁超过实际角速度能跟上的范围在误差跨过边界时期望角速度有没有抖动排查步骤可以这样走确认控制器版本和参数来源有人会把旧版本参数直接搬到新固件含义变了都不知道。单独看外环把目标姿态设成阶跃观察期望角速度曲线是否平滑正常情况下不应出现跳变。单独看内环看实际角速度跟踪期望角速度的效果正常情况滞后很小。对比不同通道横滚、俯仰、偏航的响应特性不一样参数往往不能互相照抄。APM的工具链总体来说是很成熟的关键是养成“看曲线再调参数”的习惯而不是凭手感乱拧。很多莫名其妙的飞行问题数据一拉出来原因立刻清楚。5. 从源码到工程这段函数带来的启发sqrt_controller只有几行代码但它体现了嵌入式飞控设计里一个很重要的思想用简单的分段函数把控制器输出限制在物理系统能够响应的范围内同时保持小信号的精细度和手感。如果你去读ArduPilot新版本的控制代码会发现姿态控制器结构经过多次重构但“误差大时用平方根或类似曲线限制速率增长”这个思想一直被保留。因为它简单、计算开销低、参数直观非常适合在单片机实时控制里长期运行。与之对比工业上也有更复杂的时间最优控制、S曲线规划等方案它们能给出更精确的轨迹但需要更多状态信息和计算资源。飞控本身有加速度计、陀螺仪、气压计等传感器噪声和延迟都不小复杂的轨迹规划不一定能带来多少实际手感提升。APM选择在控制器前段加一个轻量级的sqrt变换用最低成本解决大误差饱和问题这在工程上是很务实的选择。我在实际使用中的体会是调飞控不要一上来就啃PID三个字母先把你控制链路每一段输入输出搞清楚尤其是这种“误差转化器”。很多时候你以为自己在调PID其实一直在和sqrt_controller的参数较劲。把分段边界、平方根特性、内环能力这几个概念理清楚再回来看那些默认参数你会突然明白官方为什么把数字设成那样。最后再分享一个小技巧如果你怀疑外环响应有问题先不要急着改参数在Mission Planner里把日志的err、rateTarget、rate三条曲线画出来对比。如果误差基本都在线性区那就是kp主导你的手感如果误差经常跑到平方根区那多半是rate_max或kp的比值不合理。先把这条边界画出来再调参数效率至少翻倍。