手把手推导APM的 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\ast pos\_err---（1）$$
即控制器的输出与输入误差成正比，对于本文的高度控制器来讲，位置环的输入为高度误差，输出为期望速度，为速度环的输入，输入与输出之间的关系如下图所示。

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

加速度的变化趋势？

凭空想象是不行，必须要用数学公式来解释。
$$vel=vel\_ref=Kp\ast pos\_err---（2）$$
两边求导：
$$acc=\dot{vel}=Kp\ast \dot{pos\_err}=-Kp\ast vel=-K{p}^2\ast pos\_err---（3）$$
可以看出，误差收敛到0的过程中，加速并不是恒定不变的，与$K{p}^2$成正比。

实际系统中，作动器输出的加速度是有上限的，比如四旋翼无人机，能达到的最大上升加速度也是有限的，并不是给定多大的参考加速度，系统都能跟踪得上。
假设系统能提供的最大加速度为acc_max，我们期望在整个运行过程中，给定的参考加速度不超过acc_max，即误差超过一定值时，加速度为一个恒定值，加速度不能再随误差呈线性增长，如下图所示。
当误差较小时，为变加速运动，当误差较大时，为匀加速运动。

为了让速度控制器给定一个合理的控制量，就需要针对系统的特性进行限制，于是sqrt_controller就登场了。

sqrt_controller公式推导

如上所述，我们希望误差超过一定大小时，速度变化率（加速度）为恒定acc_max，由于位置控制器输出的是目标速度vel_ref，要想限制加速度，就需要限制vel_ref的变化率。
根据上图acc与pos_err的曲线，再补充一个完整vel与pos_err的曲线（pos_err>0时）。
下图中，横轴为位置误差pos_err，纵轴为目标速度vel_ref。
根据公式（3），位置误差小于$acc\_max/K{p}^2$时，加速度是变化的，当位置误差大于它时，限定为恒定的加速度acc_max。

当位置误差$x>acc\_max/K{p}^2$时，此时的速度vel_ref为多少呢？根据牛顿第二定律，物体做匀加速运动时：
$$v^2-v_0^2=2as---（4）$$
s为距离，a为加速度。
$s=x-\frac{acc\_max}{K{p}^2}$, $a=acc\_max$, $v_0=\frac{acc\_max}{Kp}$，带入到式（4）中：
$$v^2-(\frac{acc\_max}{Kp}{)}^2=2\ast acc\_max\ast (x-\frac{acc\_max}{K{p}^2})$$
得到误差$x$与速度的关系如下：
$$v=\sqrt{2\ast acc\_max\ast x-(\frac{acc\_max}{Kp}}{)}^2---（5）$$
所以有
$$vel\_ref=\{\begin{array}{ll}Kp\ast pos\_err& \text{, pos\_err<=Kp2acc\_max}\\ \sqrt{2\ast acc\_max\ast x-(\frac{acc\_max}{Kp}}{)}^2& \text{, pos\_err>Kp2acc\_max}\end{array}$$

sqrt_controller源码

下面为sqrt_controller的圆满，删除了一些不相关的代码。

// Proportional controller with piecewise sqrt sections to constrain second derivative
float sqrt_controller(float error, float p, float second_ord_lim, float dt)
{
    float correction_rate;
   
    // Both the P and second order limit have been defined.
    float linear_dist = second_ord_lim / sq(p);
    if (error > linear_dist) {
        correction_rate = safe_sqrt(2.0f * second_ord_lim * (error - (linear_dist / 2.0f)));
    } else if (error < -linear_dist) {
        correction_rate = -safe_sqrt(2.0f * second_ord_lim * (-error - (linear_dist / 2.0f)));
    } else {
        correction_rate = error * p;
    }
       
    return correction_rate;
}

代码中，linear_dist与$\frac{acc\_max}{K{p}^2}$对应，second_ord_lim就是加速度限制acc_max，再对照看代码就不会迷糊了。