手把手推导APM的 sqrt_controller控制器

最新推荐文章于 2026-09-03 19:58:23 发布

原创

最新推荐文章于 2026-09-03 19:58:23 发布 · 3k 阅读

本文详细解析了无人机控制中sqrt_controller的作用，它实际上是一个优化版的P控制器，用于限制输出加速度不超过系统极限。在高度控制的PID串级三环结构中，sqrt_controller在外环位置控制中限制速度参考的增益，防止目标不合理导致的超速问题。通过数学公式分析了Kp控制器的特性，并推导了sqrt_controller的公式，确保在误差较大时输出加速度恒定，避免过度加速。最后展示了sqrt_controller的源码实现。

前言

玩过无人机的朋友对APM应该都不会陌生，如果是搞过多旋翼飞控且读过代码，肯定遇到过sqrt_controller控制器（开方控制器），它频繁的在位置、姿态控制中出现，所以非常有必要搞清楚这个控制原理。

本质

首先不要被sqrt_controller吓到，它的本质就是一个P控制优化版本，或许你又会问，既然它是P控制，那为什么要舍简就繁，搞一个复杂版本的P控制器呢？
在控制系统中，我们总是希望系统的输出output可以跟踪期望target，如果target是合理的，没有超过系统可以达到的极限，那么这个希望是很有可能实现的，但是如果target不切实际，超出了系统的能力范围，比如让你的拖拉机4秒内加速到100km/s，那你的希望肯定是无法实现了，油门踩到底也白搭，没准把你可爱的柴油发动机崩坏了，所以提出合理的target很重要，符合被控对象的物理极限，此时sqrt_controller就发挥作用了。

下面以高度控制器为例来分析。

高度控制

APM的高度控制采用PID串级3环控制，即外环位置控制，中间速度控制，内环速度控制，如下图所示。
高度控制器框图
今天要讨论的sqrt_controller就是应用于外环位置控制器中，其输出是速度控制器的参考reference输入，如果采用单P控制，其输出vel_ref = pos_err * kp，vel_ref与误差成正比。

下面再分析Kp控制器的一些特性。

Kp控制器

Kp控制器的公式为：
$vel\_ref = Kp * pos\_err ---（1）$
即控制器的输出与输入误差成正比，对于本文的高度控制器来讲，位置环的输入为高度误差，输出为期望速度，为速度环的输入，输入与输出之间的关系如下图所示。

在这里提出一个问题，若位置给定一个阶跃输入，且速度控制器是一个完美的控制器（可以无误差的跟踪跟踪输入），当误差衰减为0时，请问，这个过程中：

加速度的变化趋势？

凭空想象是不行，必须要用数学公式来解释。
$vel = vel\_ref = Kp * pos\_err ---（2）$
两边求导：
$acc=vel˙=Kp∗pos_err˙=−Kp∗vel=−Kp2∗pos_err−−−（3） acc = \dot{vel} = Kp * \dot{pos\_{err}} = -Kp * vel = -Kp^2 * pos\_{err}---（3）$