“sqrt_controller 这个东西我第一次在 ArduPilot 源码里看到时第一反应是‘怎么还有这种控制器’位置误差大了输出速度误差小了输出速度居然不是简单的比例关系而是带了一个平方根函数。后来真正装机、看日志、调参踩了几次坑之后才明白这个‘不寻常’的写法其实藏着很深的运动学逻辑。这篇文章我就手把手从零推导一遍 APM 的 sqrt_controller尽量让没学过现代控制理论的同学也能看懂它到底在算什么以及参数该怎么调。”在飞控里位置控制往往是最容易“看似简单、调起来却全身是病”的环节。纯 P 控制会让大偏差时响应过慢PD 控制又会让噪声放大而 ArduPilot 大量使用的 sqrt_controller 则是一种基于运动学最优思想的速度生成器。这篇文章面向正在调 APM 姿态/位置控制、想搞懂源码里sqrt_controller到底在干嘛或者想把自己调试经验系统化的朋友。我会从刹车距离这个生活常识出发一步步推导出公式再对照 ArduPilot 源码逐行解读最后聊清楚rate_p、smax、hz_dist三个参数怎么配合以及我在实际调飞机过程中遇到的典型现象和排查手法。1. 整个问题源头位置环为什么不能只靠“比例”1.1 纯比例控制的尴尬先回忆一下最朴素的位置控制思路当前飞机离目标点差了 10 米那就给一个跟 10 米成正比的速度指令差了 1 米就给 1 米对应的速度。写成式子就是v_cmd P * error。这个写法不是不行小误差、慢速场景完全够用但放大到多旋翼这种需要快速响应的系统问题就来了。如果 P 比较大那么 10 米误差时会输出很大的速度指令飞机会以很高的速度冲过去。可等它真正接近目标时误差变成 1 米速度指令又会迅速变小。问题是飞机有惯性机身不能瞬时从 8m/s 减到 1m/s所以它一定会冲出目标、出现超调。如果 P 比较小超调是缓解了但大误差时的飞行速度又太慢10 米外慢慢悠悠“飘”过去人急都急死了。这就是纯比例控制的天然矛盾大偏差想要“快”小偏差想要“稳”但一个固定斜率同时满足不了两个需求。你可能马上想到加 D 项行不行PD 控制器确实能对速度增加阻尼可 D 项对测量噪声极敏感尤其是 GPS 位置信号本身就有滞后和噪声微分出来的速度常常是抖的。而且哪怕加了 D本质上还是线性反馈面对“我有最大加速度上限”这个物理事实时并不天然匹配。1.2 换个视角这是一个运动学规划问题如果把控制目标稍微升级一下不是“让误差按比例缩小”而是“在不超过最大加速度的前提下用尽可能短的时间把飞机送到目标点并且到目标时速度刚好为零”。这是典型的运动学最优问题也就是时间最优控制。对一维运动而言答案非常直观先以最大加速度加速到了中点附近再以最大减速度减速正好停住。这个“先加速后减速”的轨迹速度随位置变化的曲线并不是直线而是一条抛物线。原因很简单匀加速运动里速度平方和位移成正比也就是v^2 2 * a * s。位置环想要生成速度指令其实就是在每一时刻回答一个问题“以我现在离目标的位置误差作为剩余可用的刹车距离我最多还能开多快”答案就是那个平方根关系。这也是sqrt_controller这个名字的来源——它本质上是一个为“双积分器系统”设计的限加速速度规划器。2. 手把手数学推导从刹车距离到 sqrt 公式2.1 最基本的情形从静止出发、到点停车先用最简单的场景推导。假设飞机当前位于误差为e的位置正数代表还没到目标点当前速度为零最大允许加速度为a_max。如果现在开始全力减速加速度取-a_max那么把它当成一个匀减速过程初速度为v_cmd末速度为零减速距离为e则有v_cmd^2 2 * a_max * e所以v_cmd sqrt(2 * a_max * e)这个式子非常符合直觉剩余距离越大允许的速度越快但增长是平方根关系不是线性的。比如a_max 4 m/s²误差 1 米时速度指令约 2.83 m/s误差 4 米时约 5.66 m/s误差 9 米时才到 8.49 m/s。误差大了速度并没有疯狂增长这正是物理限制在数学上的体现再快你就刹不住了。2.2 带正负方向符号处理实际飞行中误差有正有负目标可能在左边也可能在右边。公式需要对称处理写成v_cmd sign(e) * sqrt(2 * a_max * |e|)也就是误差为正就往正方向跑误差为负就往反方向跑。这一步没什么高深理论纯粹是工程实现时的符号约定。APM 源码里对负数分支单独写了一个-sqrtf(2.0f * smax * (-error - hz_dist))跟这个思路一致。2.3 更严格的情形如果飞机已经有速度呢上面的推导默认初速度为零。但实际飞行中飞机在跟踪一个移动目标或从上一帧状态接着跑时当前位置确实有速度。这时完整的时间最优控制应该比较当前速度和上面算出的最大允许速度。如果当前速度已经大于sqrt(2 * a_max * e)说明刹车距离不够了必须立刻产生反向加速度来减速如果当前速度还小于这个值则还可以继续加速。ArduPilot 的sqrt_controller在很多调用点上没有直接把“当前速度”作为输入传入而是把当前速度交给外层的速度控制器去闭环位置环只负责生成“建议速度指令”。这个分层设计简化了控制器但作为使用者要明白真正保证“到点停车”的是位置环 速度环的协同而不是单靠根号公式本身。如果你只看到位置环输出一个很大的速度目标却没检查速度环能不能跟上那同样会过冲。3. 近端线性段为什么代码里还有一个“hz_dist”3.1 纯 sqrt 控制在小误差时的问题纯粹的平方根公式v sqrt(2*a*|e|)在误差接近零时会有一个让人头疼的性质误差减小时速度也随之减小但速度对误差的增益在零点附近是无穷大的。什么叫增益无穷大就是误差从 0.1 变成 0.2速度指令可能翻倍增加。这会让小误差附近非常敏感只要有微小噪声速度指令就会剧烈抖动。这种抖动在飞控上尤其致命。GPS 定位误差、气压计高度噪声、姿态角的轻微震荡都会转换为位置误差接着被根号放大成明显的速度指令抖动最终表现为飞机悬停时的“呼吸感”或者小幅高频晃动。3.2 线性段和根号段的衔接工程上的解法是在小误差区域用线性 P 控制并把根号段接到线性段的外面。ArduPilot 实现的思路是当误差绝对值小于等于hz_dist时使用rate_p * error也就是一个普通的比例控制器当误差大于hz_dist时使用根号形式但是把根号里的误差先减掉hz_dist同时补上一个常数项hz_dist * rate_p保证在切换点函数值连续。写成代码逻辑就是if (error hz_dist) { correction_rate sqrtf(2.0f * smax * (error - hz_dist)) hz_dist * rate_p; } else if (error -hz_dist) { correction_rate -sqrtf(2.0f * smax * (-error - hz_dist)) - hz_dist * rate_p; } else { correction_rate rate_p * error; }这里为什么要补hz_dist * rate_p而不是简单用sqrt(2*smax*error)因为如果不补error hz_dist时根号值sqrt(2*smax*(error-hz_dist))0而线性段输出是rate_p * hz_dist两者之间存在一个突变整个控制曲线在切换点下掉一截。控制器输出跳变会让电机舵机猛地抽动一下。补上常数项之后虽然导数仍然不连续但至少函数值连续工程上已经足够平滑。3.3 三个参数分工这个函数里出现三个输入rate_p、smax、hz_dist。rate_p本质上是小误差线性段的斜率同时也是根号段在切换点处的“延续斜率基数”。在位置环里它通常对应位置 P 增益如POS_XY_P在姿态环里角度 P 增益如ATC_ANG_RLL_P、ATC_ANG_PIT_P。它的作用是在近端负责“精确到位”决定多小的误差会被当作需要积极纠正。smax是最大加速度单位跟角度/位置环的单位保持一致。姿态环里是最大角加速度水平位置环里是最大水平加速度高度环里是最大垂直加速度。它的作用决定根号段的弯曲程度smax越大控制器越“敢”在误差大时给高速度曲线平台越高。hz_dist是线性段宽度APM 源码里注释也常直接叫hz_dist意思是“horizon distance”即多大范围内走纯线性。它越小根号段越早介入它越大控制行为越接近普通 P 控制。把hz_dist设成 0整个函数就退化成普通比例控制这也是很多老版本参数兼容时用的方法。4. 走进 ArduPilot 源码实现的调用点与逐行拆解4.1 核心函数代码逐行拆解ArduPilot 里sqrt_controller定义在libraries/AC_AttitudeControl/AC_AttitudeControl.cpp中实际完整实现大致如下float AC_AttitudeControl::sqrt_controller(float error, float rate_p, float smax, float hz_dist) { float correction_rate; if (hz_dist 0.0f) { if (error hz_dist) { correction_rate sqrtf(2.0f * smax * (error - hz_dist)) hz_dist * rate_p; } else if (error -hz_dist) { correction_rate -sqrtf(2.0f * smax * (-error - hz_dist)) - hz_dist * rate_p; } else { correction_rate rate_p * error; } } else { correction_rate rate_p * error; } return correction_rate; }逐行看第一行如果hz_dist大于零走带线性段的完整逻辑否则退化为纯比例。这行注释在实际代码里就提示了“small error used for linearization”。第二到第四行处理误差大于hz_dist也就是误差大且方向为正的情况。注意这里根号内部的error - hz_dist等于说“根号段只管超过线性段的那部分误差”而hz_dist * rate_p则把线性段在该点上的输出保持不变从而让曲线连接。负方向是对称写法。最后else走线性。有个很容易被忽视的点这个函数对smax的符号没有任何检查。如果你把smax设成负数根号内部可能变成负数结果出现 NaN。所以外部参数校验非常重要APM 的 GCS 里一般会做范围检查但你自己写控制代码时要记得确保smax 0。4.2 姿态环中的调用角度误差转角速度姿态环是sqrt_controller用得最典型的地方。飞控拿到期望姿态角比如期望横滚 20 度和当前姿态角当前横滚 5 度误差 15 度。姿态控制器不会直接把 15 度乘上一个系数变成输出而是先通过sqrt_controller把角度误差转换成一个期望角速度目标。在这个场景下error期望角与当前角之差rate_p角度 P 增益典型对应ATC_ANG_RLL_P等参数smax最大角加速度典型对应ATC_ACCEL_RLL_MAX等参数hz_dist角度线性区宽度比如几度到十几度。转换出的角速度目标会继续进入角速度环由角速度 PID 最终输出扭矩。这个分层看起来多了一步但好处是姿态环天然带有了“大角度时限制角速度、接近时平滑减速”的特性飞起来不会出现猛打猛回。4.3 位置环中的调用位置误差转速度位置环的作用是把期望位置和当前位置之差转成期望速度。ArduPilot 的AC_PosControl在计算水平速度目标时会调用sqrt_controller此时error水平位置误差或垂直高度误差rate_p位置 P 增益类似POS_XY_P高度环则有对应的高度 P 参数smax最大水平加速度或最大垂直加速度由PILOT_SPEED_UP、加速度限制参数等影响hz_dist线性区距离通常是零点几米到几米。得到期望速度后再交给速度控制器往往还有一个速度 P / I 环节生成期望加速度期望加速度再映射为期望倾角和油门。所以整个链是位置误差 → sqrt 速度指令 → 速度环加速度 → 倾角/油门 → 姿态环角速度 → 扭矩。4.4 一个容易被忽略的细节调用前的限幅sqrt_controller只负责给建议速度但不负责最终限幅。APM 里调用它之后还会对输出速度做限制比如限制在PILOT_SPEED_UP或水平最大速度范围内。很多人调参时觉得自己调了smax速度还是太大其实是因为后续的限幅参数没有配好。飞控里很多位置速度上限是独立参数位置环输出的速度目标即便再高也过不了速度限幅这一步。所以调参时要想清楚你是希望它更快还是希望它在接近目标时减速更平滑如果只想限制最大飞行速度应该改速度上限而不是乱调smax否则会影响整个减速段的曲线形状。5. 调参实战让 sqrt_controller 不抖、不冲、不飘5.1 第一步先定最大加速度调参顺序很重要我的经验是先确定物理层的加速度上限。对多旋翼来说最大水平加速度往往取决于最大倾角。机身倾角越大水平加速度越大但倾角太大会导致升力损失和动力饱和。一般先用默认值或理论值估算ATC_ACCEL_RLL_MAX、ATC_ACCEL_PIT_MAX再通过日志确认实际响应是否撞到限幅。如果你发现位置控制“冲过头”先看是不是加速度限幅设得太高导致电机力矩根本约束不住。反过来如果感觉飞机“肉”怎么打杆都不走也别急着加 P先看看加速度上限设得太小。加速度上限本质上是运动学约束它决定了位置环速度指令能达到多大P 增益则决定在达到这个上限之前曲线的扭曲程度。两者配合不是单一参数能解决的。5.2 第二步从姿态环 P 开始再调位置环 P我踩过最深的坑是跳过姿态环直接调位置环结果飞机在高速翻滚边缘疯狂试探。原因是位置环输出的速度目标要由姿态环实际完成姿态环带宽不够所有外环调参都会被掩盖。所以我的顺序是先把姿态内环角速度环调稳用遥控器手动模式或半自动模式观察打杆响应确认没有高频振荡再调姿态角度环ATC_ANG_RLL_P、ATC_ANG_PIT_P让位置环给出的倾角指令能快速无超调地跟上最后才调位置环POS_XY_P和对应的hz_dist。每次只动一个参数加 20% 左右然后用日志看阶跃响应。所谓“阶跃响应”在实际调参里就是切一次 Loiter 模式让飞机从偏移状态自动回中或者给一个位置指令让它飞一小段看位置误差曲线的收敛形状。5.3 第三步hz_dist 到底取多少hz_dist这个参数很容易被忽略但它直接影响控制风格。取值太小比如 0.1 米或 1 度会让根号段过早介入小误差附近虽然还是线性但切换点之后斜率变化很剧烈表现为“快到位时突然有一下拽的感觉”。取值太大比如 5 米或 30 度则整个位置环基本退化成 P 控制大误差时响应不够激进。经验上姿态环的hz_dist可以从几度到十几度之间试位置环可以从 0.5 米到 2 米之间试。因为每架飞机的动力和噪声水平不同没有万能值。判断标准是切换点前后飞机不能有明显的“顿挫感”日志里速度指令曲线应该平滑没有尖角。5.4 经典波形与异常表现看日志时我一般直接画“期望位置误差”、“期望速度指令”、“实际速度”三条曲线。正常情况是误差大时速度指令快速上升但斜率逐渐减小误差接近零时速度指令平滑下降实际速度滞后但趋势一致。异常情况有几种速度指令出现高频锯齿多半是hz_dist太小或者 GPS/气压计噪声没有被很好滤波速度指令一直顶在限幅值上说明最大加速度设小了或者外环 P 太小控制器不够积极实际速度明显跟不上期望速度最后超调说明内环倾角限制或角速度限制不够也可能是角速度环带宽不足。6. 常见问题与排查技巧实录6.1 现象一定点悬停像在“呼吸”飞机在 GPS 定点模式下绕着目标点来回小幅移动误差曲线像呼吸一样周期性扩张收缩。我遇到这种问题时先怀疑位置环 P 过高导致飞机在零点附近反复“进出”线性区。因为sqrt_controller的线性段增益是rate_p如果这个值太大零点附近就是一个高增益比例环很小的位置波动都会产生明显速度指令。解决办法是先降POS_XY_P或姿态环的ATC_ANG_RLL_P。如果已经很低了还呼吸那再检查机架振动、GPS 定位精度。这里要特别提醒sqrt_controller只处理控制规律不负责滤波。位置源噪声大再好的控制器也白搭。6.2 现象二大角度回中时“冲过头”自稳模式或位置模式下飞机从大幅度偏移回中时出现过冲。很多人第一反应是减小 P但有时真正原因是smax太大。因为smax直接决定根号段“敢给多大速度”加速度上限标得比实际能产生的还高控制器以为能刹住实际刹不住自然过冲。做法是把ATC_ACCEL_RLL_MAX/ATC_ACCEL_PIT_MAX调小让减速距离变短同时配合位置环速度限幅一起看。记住一个生活类比sqrt 公式本质上是在问“以当前剩下的距离当刹车距离我该开多快”如果你把刹车上限标得很高但实际上刹车系统没那么强那必然冲出路口。6.3 现象三刹车时出现俯仰抬头/低头水平位置控制在刹车时出现姿态方向的突然抬头或低头往往不是位置环问题而是倾角限制和角加速度限制不匹配。位置环请求了一个减速度姿态环为了尽快响应把角速度拉得很大机身猛地低头。此时应该检查角速度环和角度环的响应是否太“猛”可以适当降低角度 P或者增加角加速度平滑。如果只在高速刹车时才出现还可以考虑是不是最大速度限幅太高导致从高速到零的速度差太大控制器不得不压榨全部角速度能力。工程上可以适当限制最大水平速度让刹车更从容。6.4 现象四有风环境下稳态位置误差始终消不掉外环位置控制本质上是把位置误差转成速度指令如果一直有恒定外力需要持续一个速度指令才能抗风。但sqrt_controller在误差为零时输出为零它本身没有积分作用。真正克服恒风的是速度环的积分项。所以发现“GPS 模式下被风越吹越偏、位置误差始终有几米”时不要拼命加位置 P那样会抖。正确方向是调速度环积分比如 ArduPilot 速度环对应的 I 增益参数。6.5 排查流程速查表现象优先检查项可能的调整方向高频抖动位置/角度 P 过大、滤波器过宽降低 P检查振动大幅回中过冲最大加速度设过大降低smax降低速度限幅响应肉、跟不上指令最大加速度过小、P 过小增大smax或 P切换点有顿挫hz_dist设置不合适调大hz_dist或检查曲线连续性风下稳态误差大速度环 I 不够增大速度环积分不要只加 P日志出现 NaNsmax或参数非法检查参数范围确保非负7. 工具箱与延伸思考7.1 用地面站和日志验证控制行为调参不能只靠“手感”我强烈建议用地面站工具看日志。Mission Planner 是最常用的 APM 地面站可以直接查看 ATT、CTUN、POS 等数据块画曲线对比期望值和实际值。MAVProxy 则在命令行下更轻量适合在无人机机载电脑里做实时分析。用这些工具时重点画几条线位置误差、速度指令、实际速度、倾角指令、实际倾角。把这五条线放在同一张图里控制链路上的每一级延迟和超调都能看得一清二楚。顺带提一句APM 生态里的周边工具还包括 DroneKit、MAVSDK 这类开发库它们可以让你在外部程序里读取飞控状态、发送位置指令做自动化测试。做控制器验证时我常常写一个简单脚本让飞机自动飞一个来回然后自动落日志、画曲线比自己手动打杆试高效很多。7.2 sqrt 思想能迁移到哪里sqrt_controller不只是飞控专用凡是“位置→速度”或“角度→角速度”的分层控制都可以借用。比如移动机器人的底盘控制给一个目标距离用限加速度规划速度就可以避免急停急转云台控制器的俯仰角度环也可以用类似思路生成角速度指令提升大角度回中的舒适度。核心思想是在物理约束面前速度指令不能和误差无限线性增长而应该遵循最大加速度下的刹车距离关系。如果还想更进一步完整的双积分器时间最优控制还包含“当前速度”作为状态比较当前速度和速度指令决定是加速还是减速。ArduPilot 的sqrt_controller部分调用点没有把所有状态全用上但整体思路已经足够稳健。真要系统学习可以从“double integrator time-optimal control”这个关键词入手。7.3 个人心得什么时候别用 sqrt_controller尽管它很好用但不是所有场景都适合。如果你的任务载体本身几乎没有惯性和加速度限制比如很小的微型无人机带动量极小P 控制可能已经够好没必要引入非线性。又或者你希望完全线性、可预测的频率响应用于某些分析和实验中那么hz_dist以外的根号段会让系统在频域上呈现明显增益随幅度变化的特性这在某些理论验证里会变得复杂。我自己在调试中的体会是sqrt_controller最宝贵的一点不是它让响应更快而是它把“物理极限”写进了控制器里。有经验的飞手用手动打杆能做出漂亮的减速进近其实脑子里做的也是同样的事——根据剩余距离判断速度、根据感觉中的最大刹车能力规划油门。控制器的意义就是把这种经验数字化用一行公式稳定复现。理解了这一点再回看那几行源码你就会有“这么简单也敢称最优”的感慨然后发现它确实够用、也确实好用。