详解APM的开方控制器sqrt_controller

前言

前面说过，sqrt_controller是对P项进行整定用途的，目的就是让P项的控制响应“软”下来，实际上就是一个经过改进的P控制器。读懂了sqrt_controller，那么你对APM的P控制器就了解得差不多了。

仅讨论P控制器，sqrt_controller的作用就是设定一个量linear_dist，以这个量的正负值为分界线划分出一个线性区域范围，在这个范围内表示误差量还很小，遵循Kp*error计算方式输出控制量。

超过linear_dist后，表明误差较大，此时不能由着误差单纯按照线性方式乘以Kp输出控制量，需要按照开方形式，使用另外的方式计算出实际控制量，此处后面补充。

在开始讲解源码之前，先来补充个前提：

了解飞控的应该都清楚，一般是使用串级PID来进行姿态等的控制。在APM中，外环角度环就是接收期望与实际的角度误差，然后输入到P控制器中得到期望的角速率。也就是说，这个控制过程可以表示为：
$$w=K_p\ast erro{r}_{\theta }$$

而我们同样知道，假定小角度内速率恒定，那么可知$w=erro{r}_{\theta }/dt$，而由此可直接定义
$$Kp=1/dt$$

而实际上APM中广泛采用了这个方式，因此，后续过程中我们谈论到Kp表明的都是时间的倒数。

源码阅读

数学公式的原理放在后面讲解，这边先放源码

// Proportional controller with piecewise sqrt sections to constrain second derivative
// 翻译：具有分段sqrt截面以约束二阶导数的比例控制器
// error：输入参数，误差量
// p：输入参数，Kp值
// second_ord_lim：输入参数，二阶限制，对Kp进行二次限制，通常是加速度最大值输入
// dt：输入参数，时间
float AC_AttitudeControl::sqrt_controller(float error, float p, float second_ord_lim, float dt)
{
   
   
    float correction_rate;	// 最后需要返回的期望速率
    if (is_negative(second_ord_lim) || is_zero(second_ord_lim)) {
   
   
        // 二阶限制是否为负或者零
		// 是的话表明开方控制器不起作用，按照正常方式计算Kp*error
        correction_rate = error * p;
    } else if (is_zero(p)) {
   
   
        // Kp=0但是我们开启了二阶限制则采用如下计算方式
        if (is_positive(error)) {
   
   
            correction_rate = safe_sqrt(2.0f * second_ord_lim * (error));
        } else if (is_negative(error)) {
   
   
            correction_rate = -safe_sqrt(2.0f * second_ord_lim * (-error));
        } else {
   
   
            correction_rate = 0.0f;
        }
    } else {
   
   
        // 如果同时定义了Kp和二阶限制
		// 一般我们用了sqrt_controller都是进的这个函数
		// 计算获取分界点linear_dist
		// 根据error与linear_dist的相对大小采用不同公式
        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)))