ardupilot均方根控制器的代码解析

先上官网图：

A P controller converts the angle error (the difference between the target angle and actual angle) into a desired rotation rate followed by a PID controller to convert the rotate rate error into a high level motor command. The “square root controller” portion of the diagram shows the curve used with the angle control’s P controller.

由此可见，均方根误差控制器目的是将角度误差转换为角速率。

再贴代码:

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)) {//角加速度限制值<=0
        // second order limit is zero or negative.
        correction_rate = error * p;
    } else if (is_zero(p)) {//p为0
        // P term is zero but we have a second order limit.
        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 {
        // 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;
        }
    }
    if (!is_zero(dt)) {
        // this ensures we do not get small oscillations by over shooting the error correction in the last time step.
        return constrain_float(correction_rate, -fabsf(error) / dt, fabsf(error) / dt);
    } else {
        return correction_rate;
    }
}

      其中传入参数： second_ord_lim： 角加速度限制，记住这里是角加速度，rad/s2;

p比例因子，dt为周期或者是上一次与本次更新的间隔。

      均方根控制器可以理解为两个环，比例环与角加速度环（前馈）的并联，根据代码分析，不会两个环同时生效。

     角度向角速率转换分为三种情况，优先级顺序为：角加速度<=0 优先于比例因子为0，比例因子为0优先于比例因子不为0且角加速度>0。

1.角加速度<=0，说明载体旋转的速率一直减小，而均方根控制器的目的是使欧拉角以有限的减速度平稳地停止在输入角上，所以直接运用了比例控制，角加速度环不起作用，这里个人认为：从数学上讲，如果误差为负，safe_sqrt(2.0f * second_ord_lim * (error))可以计算出正值的角速率，这时会增大误差，减弱比例环作用，不利于系统快速收敛；如果safe_sqrt(2.0f * second_ord_lim * (error))加负号，和比例环叠加，会使系统超调也不利于系统稳定；

2.比例因子为0，此时比例环失去作用，只能依靠角加速度环，这里很好理解。

3.比例因子不为0且角加速度>0：这种情况下，需要分段处理。在second_ord_lim / sq(p)的正负范围内，均方根处理不够平滑，采用比例控制，也就是图中的红线，超过这个范围均方根处理，比例控制不够平缓，这里目的就是使载体控制平稳。为什么是second_ord_lim / sq(p)这个分界点？

     均方根函数：;比例函数;此时这两个函数必然至少会有一个交点，即分界点，根据求根公式，可以得出才会有交点，同时也可以算出此时,即代码中的float linear_dist = second_ord_lim / sq(p);

最后在dt不为0的情况下对角速率进行了限制，不能超过-fabsf(error) / dt, fabsf(error) / dt);

至此代码里的思想方法全部解析完毕。