[算法]get_stopping_point和sqrt_controller

函数名： get_stopping_point_z

位置：libraries\AC_AttitudeControl\AC_PosControl.cpp

使用我们之前分享过的思路，把KP可以看成 1/dt，这样更容易理解，我们以z轴的停止点为例进行分析。

可以看出停止点的计算方法，当前位置加上刹车距离，而刹车距离使用的是分段函数。

根据当前速度的大小采用不同的计算方法：

1	linear_velocity = _accel_z_cms/_p_pos_z.kP();

$linear_{velocity}=a_{max}t$

当前速度的绝对值小于线性速度时刹车距离为：

1 2	stopping_point.z = curr_pos_z + curr_vel_z/_p_pos_z.kP(); 刹车距离=curr_vel_z/_p_pos_z.kP()

$S=V_zt=a_zt^2,a_z<=a_{max}$

当前速度的绝对值大于线性速度时刹车距离为：

1
2
3

linear_distance = _accel_z_cms/(2.0f*_p_pos_z.kP()*_p_pos_z.kP());
stopping_point.z = curr_pos_z + (linear_distance + curr_vel_z*curr_vel_z/(2.0f*_accel_z_cms));
刹车距离=(linear_distance + curr_vel_z*curr_vel_z/(2.0f*_accel_z_cms));

$S=\frac{1}{2}a_{max}t^2+\frac{V^2_z}{2a_{max}}$

第一段比较好理解，当我的速度比较小的时候，一直使用最大加速度，把速度减到0，使用匀加速直线运动公式，可以得到这段距离为：

$S=\frac{V^2_z-0^2}{2a_{max}}=\frac{V^2_z}{2a_{max}}$

这和程序里的算法是一样的。

如果当前速度太快，超过了最大加速度能产生的最大速度。

那么应该刹车距离应该为：

$S=\frac{V^2_z-V_{max}^2}{2a_{1}}+\frac{1}{2}a_{max}t^2$

V_max是单位时间内最大加速度能得到的最快速度，即V_max=a_max*t，a1为V_max到Vz的加速度。

为什么和程序里不一样呢？程序里为什么要这样处理呢？

通常遇到问题不能只盯着问题，要继续往下看。

这个刹车停止点，会被设为期望位置，进入位置控制器，位置控制器输入的是，期望位置与当前位置的差，也就是刹车距离。

刹车距离作为err,进入开方控制器sqrt_controller。

开方控制器同样分成两段，以线性距离linear_dist为分界点.

1	linear_dist = second_ord_lim/sq(p);

$linear_{dist}=\frac{a_{max}}{K_p^2}=a_{max}t^2$ $V_{max}=\frac{a_{max}}{K_p}=a_{max}t$ $linear_{dist}=V_{max}t$

|err|<linear_dist时

1	correction_rate = error*p;

$correction_{rate} = err*K_p$

|err|>linear_dist时

1 2	linear_dist = second_ord_lim/sq(p); correction_rate = safe_sqrt(2.0fsecond_ord_lim(error-(linear_dist/2.0f)));

$correction_{rate} = \sqrt{2a_{max}\times (err-\frac{1}{2}a_{max}t^2)}$

是不是完全看不懂这些函数在干什么，没关系，把刹车距离带入开方控制器非常直观。

计算刹车距离第一部分正好可以满足开方控制器的判断

$err = a_zt^2 < a_{max}t^2$

所以对应的可以得到

$correction_{rate} = err*K_p = a_zt^2/t=a_zt=V_z$

即刹车起点的期望速度就是当前速度。

再看第二部分，刹车距离的第二部分，正好也可以带入开发控制器的第二部分

$err=\frac{1}{2}a_{max}t^2 +\frac{V_z^2}{2a_{max}}>\frac{1}{2}a_{max}t^2 +\frac{V_{max}^2}{2a_{max}}=a_{max}t^2\\ correction_{rate} = \sqrt{2a_{max}\times (err-\frac{1}{2}a_{max}t^2)}=V_z$

也可以使刹车起点的期望速度就是当前速度。

ok,现在很明显了，计算刹车距离的函数，就是为了配合开方控制器，是刹车函数启动的时候，可以是刹车的瞬间，期望速度为当前速度，即可以平滑减速，不会有速度突变。

那期望速度和刹车距离到底是什么关系呢？所以我们需要搞懂开方控制器的原理。

之前分析过开方控制器的果，现在来分析一下开方控制器的原理

开方控制器的分界点是：

$linear_{dist}=\frac{a_{max}}{K_p^2}=a_{max}t^2 \\$

这个分界点是通过最大加速度来设计的，最大加速的在t时间内能得到的最大速度为V=a_max t,以此速度为基础，在时间t内，以最快速度能得到的距离为Vt=a_max t

$correction_{rate}=a_t^2*K_P= at , a<a_{max}$

所以这个分解点代表着以最大加速度能得到最大速度，能飞行的最远距离，在此距离之内都可以使用飞机最大的加速度来进行减速，整个减速过程不会超过飞行器的最大加速度。

再看第二段，err 大于 linear_{dist} 时

$err>a_{max}t^2$

可以把 err 写成：

$err = (2+c)*\frac{1}{2}a_{max}t^2,c>0$ $correction_{rate}=\sqrt{2a_{max}\times \frac{1}{2}a_{max}t^2(2+c-1)} \\ =\sqrt{2a_{max}\times \frac{1}{2}a_{max}t^2(1+c)}=a_{max}t\sqrt{1+c}$

现在就很清晰了开方控制器的逻辑

$correction_{rate}= at ,|a|<=|a_{max}| \\ correction_{rate}= a_{max}t\sqrt{1+c} ,|a|>|a_{max}|,c>0$

correction_{rate} 就是期望速度，与当前速度做差，经过PID ，就得到了期望加速度。

根据之前的分析，可以得到期望速度与刹车距离的曲线图。

我们的开始刹车时，误差是从大到小变化的，如果控制器参数合适的话,随着时间变化可以得到类似下面的曲线：

因为，可以确定刹车起点的期望速度就是当前速度，所以加速度变化可以得到类似下面的曲线：

做了这么多工作，就是为了让加速度按照这样的曲线变化！

1.初始期望加速度为0，飞机的姿态不会突变。

2.加速度缓慢增加，直到等于最大加速度，飞机在刹车过程姿态变化平滑。

相当于如果飞机当前速度非常快，加了个过度过程。

这个思路用于计算停止点，同时也用来计算leash，配合开方控制器，使飞机在飞行过程中姿态变化平滑，保障了飞行安全。

之前分析过一次开方控制器，不太理解设计原理，只是从结果上分析，现在对开方控制器的设计原理也有了一定的认识，配合停止点的计算，整套思路配合的很好，没想到一个简单的刹车距离也有这么多细节，值得借鉴学习。

配合阅读：开方控制器和倾斜角转加速度函数

附录：给出两个函数的源码，方便对照阅读
get_stopping_point_z

void AC_PosControl::get_stopping_point_z(Vector3f& stopping_point) const
{
    const float curr_pos_z = _inav.get_altitude();
    float curr_vel_z = _inav.get_velocity_z();
    float linear_distance;  
    float linear_velocity;  
   
    if (is_active_z()) {
        curr_vel_z += _vel_error.z;
        if (_flags.use_desvel_ff_z) {
            curr_vel_z -= _vel_desired.z;
        }
    }
   
    if (_p_pos_z.kP() <= 0.0f || _accel_z_cms <= 0.0f) {
        stopping_point.z = curr_pos_z;
        return;
    }
    linear_velocity = _accel_z_cms/_p_pos_z.kP();

    if (fabsf(curr_vel_z) < linear_velocity) {
        stopping_point.z = curr_pos_z + curr_vel_z/_p_pos_z.kP();
    } else {
        linear_distance = _accel_z_cms/(2.0f*_p_pos_z.kP()*_p_pos_z.kP());
        if (curr_vel_z > 0){
            stopping_point.z = curr_pos_z + (linear_distance + curr_vel_z*curr_vel_z/(2.0f*_accel_z_cms));
        } else {
            stopping_point.z = curr_pos_z - (linear_distance + curr_vel_z*curr_vel_z/(2.0f*_accel_z_cms));
        }
    }
    stopping_point.z = constrain_float(stopping_point.z, curr_pos_z - POSCONTROL_STOPPING_DIST_DOWN_MAX, curr_pos_z + POSCONTROL_STOPPING_DIST_UP_MAX);
}

sqrt_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)) {       
        correction_rate = error*p;  
    } else if (is_zero(p)) {
        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 {
        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 {
			//没有限制就是KP
            correction_rate = error*p;
        }
    }
    if (!is_zero(dt)) {
        return constrain_float(correction_rate, -fabsf(error)/dt, fabsf(error)/dt);
    } else {
        return correction_rate;
    }
}