[算法] 刹车距离 get_stopping_point 和开方控制器 sqrt_controller

函数名: get_stopping_point_z
位置:libraries\AC_AttitudeControl\AC_PosControl.cpp

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

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

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

linear_velocity = _accel_z_cms/_p_pos_z.kP();

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

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

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}$$

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

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_maxt**,a1为V_max到Vz的加速度。

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

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

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

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

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

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时

correction_rate = error*p;

$$correction_{rate} = err*K_p$$

|err|>linear_dist时

linear_dist = second_ord_lim/sq(p);
correction_rate = safe_sqrt(2.0f*second_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 {
            correction_rate = error*p;
        }
    }
    if (!is_zero(dt)) {
        return constrain_float(correction_rate, -fabsf(error)/dt, fabsf(error)/dt);
    } else {
        return correction_rate;
    }
}

今天就讲这么多,我是zing一个有趣的飞控算法工程师,欢迎大家多留言,多交流,我们下期再见。