刚接触ArduPilot位置控制的时候，我一度以为位置环就是一个简单的比例控制器：误差乘个Kp，输出期望速度。直到有一次在SITL里把WPNAV_ACCEL调大，飞行器接近目标点时直接冲过头，来回摆了好几下才定住。翻源码的时候才发现，真正决定“怎么靠近目标”的核心，其实是AC_PosControl里的sqrt_controller——一个把位置误差映射成期望速度的非线性函数。这篇文章就带你从最朴素的物理直觉开始，把这个控制器的公式一步步推完整，再对照ArduPilot源码和实际调参经验，把里面的细节讲透。

1. 为什么位置控制里需要一个非线性函数

1.1 直接乘Kp的线性控制器在大误差下的“刹不住”

很多刚开始接触飞控二次开发的人，第一次看到位置控制，会下意识地认为就是一句vel_target = kp * pos_error。简单，直观，误差越大飞得越快。但真这么干，你会发现飞行器在大误差下根本停不下来。

问题出在制动距离上。假设位置误差是5米，kp = 2.0，那期望速度就是10米/秒。如果你的最大刹车加速度只有2.5米/秒²，从10米/秒刹到0需要多长距离？

s = v² / (2a) = 100 / 5 = 20米

可现在目标就在5米外，速度却奔着10米/秒去，这怎么可能刹得住？结果就是高速冲过目标点，然后来回摆荡。

这就是线性P控制器在原点的“远视”问题：它完全不考虑你还能不能刹住，只根据瞬时误差等效地给速度，误差5米给10m/s，误差10米就给20m/s，整个控制律对应的制动距离会随着误差线性膨胀，远远超出实际可达的减速距离。

1.2 从物理常识里长出来的公式：v = √(2as)

那反过来说，如果现在距离目标还有s米，最大允许减速度是a，那么你此刻允许的最大速度是多少？

这是一个纯粹的运动学问题。匀减速运动公式把它直接给出了：

v² = 2 * a * s v = sqrt(2 * a * s)

想要在距离s内稳稳停住，当前速度就不能超过这个值。把这个公式直接转成控制律，就是：

vel_target = sign(err) * sqrt(2 * accel_limit * |err|)

这个式子非常漂亮：误差大的时候，速度指令按平方根规律上升，但不会线性地冲到无限制的水平；误差小的时候，速度指令平滑归零。所有飞行过程中的速度曲线都被限制在“最大刹车加速度”这个物理边界之内，至少在运动学层面是“有把握刹住”的。

但如果你真的把这个公式直接丢进ArduPilot，问题也随之而来——它在零点附近的斜率是无穷大。误差稍微抖一下，速度指令就可能跳变一点几米每秒，整个系统会在目标点附近撒欢儿抖动。这就引出了下一步的分段设计。

2. 一步步推导：从物理公式到分段控制律

2.1 平方根函数在零附近的问题：无穷大斜率

我们需要先看清楚直接使用平方根控制律的病根在哪里。

对f(e) = sqrt(2a|e|)求导，当e > 0时：

df/de = sqrt(2a) / (2 * sqrt(e))

当e -> 0⁺的时候，斜率趋向无穷大。这意味着误差只要有极小的变化，期望速度就会发生较大的变化。控制器在目标点附近相当于一个超高增益的比例环节。

而真实系统里传感器噪声、姿态控制延迟、机体振动都会让位置误差存在高频抖动，哪怕只有几个毫米的噪声，经这个无穷大斜率一放大，输出给内环的速度指令就是大幅度的噪声信号。飞行器会在目标点附近表现为持续的“哆嗦”，严重时直接引起震荡发散。

所以工程上必须让控制器在零点附近退化为有限增益的线性控制器，让斜率是kp而不是无穷大。于是分段控制律的雏形出现了：

|err| < d_lin : v = kp * err |err| >= d_lin : v = sign(err) * sqrt(2 * a * (|err| - d_off))

这里d_lin是线性区宽度，d_off是平方根曲线的一个横向平移量。接下来要解决的问题是：d_lin和d_off怎么取，才能让两个分段在交界处平滑相接？

2.2 线性区与平方根区的衔接：边界处的连续条件

要让两段曲线在边界上平滑，必须同时满足两个条件：速度值连续、斜率连续。

假设边界点是e = d_lin。

速度连续条件：

kp * d_lin = sqrt(2 * a * (d_lin - d_off)) （式1）

斜率连续条件：

线性区的斜率是kp。平方根区在e = d_lin处的斜率，需要对|e|求导：

d/d|e| [ sqrt(2a(|e| - d_off)) ] = a / sqrt(2a(|e| - d_off))

在e = d_lin点，这个等式变为：

a / sqrt(2a(d_lin - d_off)) = kp

分子分母稍微变换一下，会发现左边就是a / (kp * d_lin)。让它等于kp，得到：

d_lin = a / kp²

这就是线性区宽度的核心结论：它等于最大加速度除以位置增益的平方。

把d_lin = a / kp²代回式1，可以解出：

kp * (a / kp²) = sqrt(2a(a/kp² - d_off)) a / kp = sqrt(2a(a/kp² - d_off))

两边平方、化简：

a² / kp² = 2a(a/kp² - d_off) a / kp² = 2a/kp² - 2*d_off 2*d_off = a / kp² d_off = a / (2*kp²) = d_lin / 2

于是完整的分段控制律就被确定下来了，跟我平时写飞控代码时手推的结果完全一致。

2.3 完整控制律和数值例子

把上面推导结果整理成一个完整的分段表达式：

d_lin = a / kp² 若 |err| < d_lin： vel_target = kp * err 否则： vel_target = sign(err) * sqrt( 2a * (|err| - d_lin/2) )

用ArduPilot默认参数代入一个例子。假设a = 250 cm/s²，kp = 2.0：

d_lin = 250 / 4 = 62.5 cm

也就是说，位置误差小于62.5厘米时，完全走线性P控制；误差大于62.5厘米时，进入平方根限速区。

看几个实际误差对应的期望速度：

很明显，误差越大，平方根控制律对速度指令的限制越强。40厘米内的误差用线性P，系统既能保持平稳，又能保证足够的收敛速度；大误差下用平方根曲线，把速度压在一个可以刹住的物理边界内。这就是整个sqrt_controller的核心设计意图。

3. 打开ArduPilot源码：sqrt_controller真身走读

3.1 核心函数的代码逻辑与参数

ArduPilot里这个函数可以在多个地方见到，最典型的是AC_PosControl中的sqrt_controller。逻辑与上面的推导一致，我将核心逻辑提炼成如下伪代码，方便你对照理解：

float AC_PosControl::sqrt_controller(float error, float p, float second_ord_loops, float dt) { // 关闭加速度限制，退化为纯比例 if (second_ord_loops < 0.0f || is_zero(p)) { return error * p; } // 不启用线性区，直接用纯平方根 if (is_zero(second_ord_loops)) { return sqrtf(2.0f * p * fabsf(error)) * get_sign(error); } // 线性区宽度 = accel / kp² float linear_dist = second_ord_loops / (p * p); // 误差在线性区内 if (fabsf(error) < linear_dist) { return error * p; } // 误差在平方根区 float vel = sqrtf(2.0f * second_ord_loops * (fabsf(error) - linear_dist / 2.0f)); return get_sign(error) * vel; }

这个函数里有几个参数要特别注意：

• error：这里是位置误差，单位通常为厘米（Copter坐标系下常用cm）。
• p：位置环的比例增益，对应参数是WPNAV_KP，默认值约2.0。
• second_ord_loops：变量名很误导人，实际传入的是最大加速度accel_cmss，对应WPNAV_ACCEL，默认约250cm/s²。
• dt：在一些较新的版本里用于可选的低通滤波，位置控制主体通常传0或者直接忽略。

当second_ord_loops为0时，意思是不指定加速度限制，直接用纯平方根曲线。这在ArduPilot的某些辅助函数里会出现，但主位置控制通常不会走这个分支。

3.2 它在位置控制环路中的调用链

在Copter的update_pos_controller里，位置环的流程大致是这样的：

1. 读取期望位置（来自定高、悬停、航点任务等）。
2. 计算当前位置误差。
3. 用sqrt_controller把位置误差转换成期望速度。
4. 把期望速度交给速度环PID，输出期望加速度。
5. 加速度再通过姿态环，转成期望姿态和油门。

如果把位置控制看成一个“级联系统”，sqrt_controller属于最外环的“速度指令发生器”。它的输出质量直接影响整个级联系统的稳定性。

实际调试时，如果发现飞行器在大误差返航时出现明显过冲，我首先确认的不是速度环PID，而是WPNAV_ACCEL和WPNAV_KP是否匹配。举个例子：把WPNAV_ACCEL从250加到500，而WPNAV_KP不变，那么线性区宽度会从62.5cm变成125cm，平方根区的“预期减速距离”变大，速度指令整体抬升，如果内环加速度跟不上，过冲是必然的。

3.3 姿态环、定高环里的同类思路

sqrt_controller不止出现在位置环。ArduPilot的姿态控制里，把姿态误差转换为期望角速度时，也存在类似的分段逻辑。比如AC_AttitudeControl里，为了限制最大角加速度，同样使用平方根映射。定高控制里把高度误差转成垂直速度指令时，也用了同一个函数。

这说明一个很重要的工程思想：只要你面对的是一个“位置/速度”或者“角度/角速度”这种二阶积分系统，想要保证它在一个物理极限内平滑收敛，平方根映射就是那个最直接的解析解。ArduPilot把它做成通用函数放在底层，所有需要“误差转速度”的地方都可以复用。

4. 仿真、实测与调参避坑

4.1 一组数值仿真对比：线性、不连续平方根、连续平方根

纸上谈兵没意思，我直接用一小段Python仿真，模拟一个简化的二阶系统（位置->速度->加速度，内环用理想1:1跟踪），目标位置100cm，看看三种控制器从误差100cm开始收敛时的表现：

import math import matplotlib.pyplot as plt kp = 2.0 # 位置增益 accel = 250.0 # 最大加速度 cm/s² dt = 0.01 total_time = 8.0 n = int(total_time / dt) def simulate(controller_type): pos = 0.0 vel = 0.0 target = 100.0 pos_hist = [] vel_hist = [] for _ in range(n): err = target - pos if controller_type == 'linear': vel_cmd = kp * err elif controller_type == 'pure_sqrt': vel_cmd = math.copysign(math.sqrt(2.0 * accel * abs(err)), err) else: # continuous sqrt d_lin = accel / (kp * kp) if abs(err) < d_lin: vel_cmd = kp * err else: vel_cmd = math.copysign( math.sqrt(2.0 * accel * (abs(err) - d_lin / 2.0)), err) # 内环 v = vel_cmd，加速度饱和限制 accel_cmd = (vel_cmd - vel) / dt accel_cmd = max(-accel, min(accel_cmd, accel)) vel += accel_cmd * dt pos += vel * dt pos_hist.append(pos) vel_hist.append(vel) return pos_hist, vel_hist # 运行三种控制器并绘图 pos_lin, vel_lin = simulate('linear') pos_sqrt, vel_sqrt = simulate('pure_sqrt') pos_cont, vel_cont = simulate('continuous_sqrt')

三条曲线对比下来，观察点非常明确：

• 线性P：初始速度指令是200cm/s，但a=250时从200cm/s刹停需要80cm，勉强能停住；如果你把初始误差加大到200cm，线性P的速度指令变成400cm/s，刹停距离要320cm，必冲。
• 纯平方根：没有线性区的时候，误差接近0时速度指令高频抖动，噪声会被斜率无穷大放大，工程上不可用。
• 连续平方根：大误差阶段走平方根限速，线性区平滑过渡，既刹得住，也不抖。

4.2 从飞行日志里看“加速度限制是否生效”

很多人调参时只看最后“有没有定住”，很少会去打开日志看速度曲线。我强烈建议你在Mission Planner的绘图功能里同时拉这几条曲线：

• NAV_POS_XY_TARGET和POS.X（位置误差）
• VEL.X（实际速度）
• NAV_VEL_XY_TARGET（期望速度）

如果是正常的sqrt控制，你会在一次远距离移动中看到一段近似“抛物线上升—平台—线性下降”的速度曲线。那个“线性下降”正是误差进入线性区后，期望速度跟着误差被kp线性拉回的表现。

如果实际速度曲线在接近目标时还保持比较大的斜率刹不住，通常说明：

• 内环速度环跟踪太慢，速度指令没被及时执行。
• 实际机架能产生的加速度小于WPNAV_ACCEL设定值。典型情况是满载、动力不足，或者倾角限制挡着，机体根本达不到设定加速度。

这时候调低WPNAV_ACCEL，不是“变慢”，而是让控制器变得更诚实，确保速度指令一直处在“真能刹住”的区间内。

4.3 常见调参问题与解决经验

这块把我踩过的坑集中列一下，都是能直接套用的经验。

问题一：小位移定点时频繁点头、抖动。

常见原因不是震荡发散，而是线性区增益太高。注意d_lin = a/kp²，线性区宽度完全由kp和a的比值决定。如果kp太高，线性区斜率大，小误差下速度指令依然很大，飞行器就像簧上弹跳。解决办法有两种：降低WPNAV_KP，或者不降kp而是增大WPNAV_ACCEL来拉宽线性区，让相同的误差进入更“软”的控制区间。

问题二：大误差返航时过冲明显，甚至来回摆好几次。

先把WPNAV_ACCEL降一档试，再确认速度环是否调好了。我遇到过很多情况，位置环参数看着正常，实际过冲是速度环的VEL_XY_P太低导致的，位置控制器以为速度已经执行了，实际速度跟不上，等误差变小才发现“离目标还有距离”，又反复修正。这种情况先用Log分析速度跟踪误差，再动位置环。

问题三：改了WPNAV_ACCEL之后，速度指令的最大值似乎没变。

注意sqrt_controller的输出只是位置环的期望速度，它后面通常还会经过一个总速度限幅，比如WPNAV_SPEED。如果平方根算出来的速度已经超过WPNAV_SPEED，实际输出会被速度上限截断，此时再加大WPNAV_ACCEL并不会让“峰值速度”变高，只会让达到速度上限的过程更早、更快。

5. 把平方根思维带出位置环

5.1 避障、跟随、着陆中的速度指令生成

ArduPilot的避障模块里，当检测到前方障碍物时，会计算期望速度的限制；跟随模式里，移动目标追踪也需要一个“误差->速度”的转换。很多这类模块没有直接用PID，而是用同一个sqrt_controller函数。原因很简单：避障场景中最忌讳的就是速度刹不住。用平方根限速，至少从运动学上保证“在任何位置开始减速，都能在目标点或障碍物边界前停下”。

着陆、定高也是同一个套路。垂直方向上的惯性相对小，但物理逻辑没变——高度误差比较大时，允许的垂直速度不能超过 sqrt(2 * a * err)，否则系统无法在触地或者到达目标高度前完成减速。理解了这一点，再看那些奇怪的下沉率限制参数，就有一种“原来如此”的通透感。

5.2 设计自己的非线性控制器时，如何复用这个套路

这套“分段解析控制律”是一种通用的设计范式，不只在ArduPilot里能用。凡是“误差->速度/流量”的二阶积分系统，都可以套用：

1. 定义物理极限加速度 a。
2. 设定线性区的期望斜率 kp。
3. 线性区宽度d_lin = a/kp²。
4. 平方根区做半个d_lin的平移，保证边界连续。

在二次开发里，如果你不想引入沉重的PID调参流程，只想为上层导航模块生成一条安全、平滑的速度指令，这个公式完全可以直接写进自己的控制代码。它够简单、有物理意义，还带边界平滑，比单纯乘Kp高到不知道哪里去了。

甚至在机器人底盘、电动滑板车的刹车控制、自动泊车路径规划中，我也见过近乎一模一样的思路。所谓“最高级的控制，往往藏在最朴素的运动学公式里”，sqrt_controller用一个匀减速公式起手，补上线性区、边界连续性和源码级实现，就稳稳扛起了ArduPilot位置控制的大梁。

最后分享一个小技巧：在看ArduPilot代码时，不要只盯控制律本身，一定把默认参数一起看。kp和accel的比值决定线性区宽度，记住d_lin = a/kp²这条公式，你调参时会少走很多弯路。