刹车距离get_stopping_point和开方控制器sqrt_controller知乎zinghd​

15519743871

[算法]刹车距离get_stopping_point和开方控制器sqrt_controller
zinghd
​
微信公众号【无人机干货铺】

函数名： get_stopping_point_z

位置：libraries\AC_AttitudeControl\AC_PosControl.cpp

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

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

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

linear_velocity = _accel_z_cms/_p_pos_z.kP();

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

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

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

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));

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

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

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

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

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

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

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

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

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

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

linear_dist = second_ord_lim/sq(p);

|err|<linear_dist时

correction_rate = error*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)));

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

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

所以对应的可以得到

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

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

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

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

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

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

开方控制器的分界点是：

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

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

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

可以把 err 写成 ：

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

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