模吧

 找回密码
 立即注册

QQ登录

只需一步,快速开始

手机号码,快捷登录

42查看 | 0回复

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

[复制链接]
发表于 2022-11-21 23:18:17 | 显示全部楼层 |阅读模式

马上注册,结交更多好友,享用更多功能,让你轻松玩转社区。

您需要 登录 才可以下载或查看,没有帐号?立即注册

x
[算法]刹车距离get_stopping_point和开方控制器sqrt_controller 刹车距离get_stopping_point和开方控制器sqrt_controller知乎zinghd​ 无人机,控制器,算法,AI,刹车 作者:15519743871 4982
zinghd
刹车距离get_stopping_point和开方控制器sqrt_controller知乎zinghd​ 无人机,控制器,算法,AI,刹车 作者:15519743871 1057

微信公众号【无人机干货铺】






函数名: get_stopping_point_z
位置:libraries\AC_AttitudeControl\AC_PosControl.cpp
使用我们之前分享过的思路,把KP可以看成 1/dt,这样更容易理解,我们以z轴的停止点为例进行分析。
可以看出停止点的计算方法,当前位置加上刹车距离,而刹车距离使用的是分段函数。
根据当前速度的大小采用不同的计算方法:
linear_velocity = _accel_z_cms/_p_pos_z.kP();
刹车距离get_stopping_point和开方控制器sqrt_controller知乎zinghd​ 无人机,控制器,算法,AI,刹车 作者:15519743871 1242
当前速度的绝对值小于线性速度时刹车距离为:
stopping_point.z = curr_pos_z + curr_vel_z/_p_pos_z.kP();刹车距离=curr_vel_z/_p_pos_z.kP()
刹车距离get_stopping_point和开方控制器sqrt_controller知乎zinghd​ 无人机,控制器,算法,AI,刹车 作者:15519743871 7295
当前速度的绝对值大于线性速度时刹车距离为:
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));
刹车距离get_stopping_point和开方控制器sqrt_controller知乎zinghd​ 无人机,控制器,算法,AI,刹车 作者:15519743871 4206 刹车距离get_stopping_point和开方控制器sqrt_controller知乎zinghd​ 无人机,控制器,算法,AI,刹车 作者:15519743871 9401
第一段比较好理解,当我的速度比较小的时候,一直使用最大加速度,把速度减到0,使用匀加速直线运动公式,可以得到这段距离为:
刹车距离get_stopping_point和开方控制器sqrt_controller知乎zinghd​ 无人机,控制器,算法,AI,刹车 作者:15519743871 5243
这和程序里的算法是一样的。
如果当前速度太快,超过了最大加速度能产生的最大速度。
刹车距离get_stopping_point和开方控制器sqrt_controller知乎zinghd​ 无人机,控制器,算法,AI,刹车 作者:15519743871 6715
那么应该刹车距离应该为:
刹车距离get_stopping_point和开方控制器sqrt_controller知乎zinghd​ 无人机,控制器,算法,AI,刹车 作者:15519743871 1448
V_max是单位时间内最大加速度能得到的最快速度,即V_max=a_max*t,a1为V_max到Vz的加速度。
为什么和程序里不一样呢?程序里为什么要这样处理呢?
通常遇到问题不能只盯着问题,要继续往下看。
这个刹车停止点,会被设为期望位置,进入位置控制器,位置控制器输入的是,期望位置与当前位置的差,也就是刹车距离。
刹车距离作为err,进入开方控制器sqrt_controller。
开方控制器同样分成两段,以线性距离linear_dist为分界点.
linear_dist = second_ord_lim/sq(p);
刹车距离get_stopping_point和开方控制器sqrt_controller知乎zinghd​ 无人机,控制器,算法,AI,刹车 作者:15519743871 5374 刹车距离get_stopping_point和开方控制器sqrt_controller知乎zinghd​ 无人机,控制器,算法,AI,刹车 作者:15519743871 2920 刹车距离get_stopping_point和开方控制器sqrt_controller知乎zinghd​ 无人机,控制器,算法,AI,刹车 作者:15519743871 6604
|err|<linear_dist时
correction_rate = error*p;
刹车距离get_stopping_point和开方控制器sqrt_controller知乎zinghd​ 无人机,控制器,算法,AI,刹车 作者:15519743871 3362
|err|>linear_dist时
linear_dist = second_ord_lim/sq(p);correction_rate = safe_sqrt(2.0f*second_ord_lim*(error-(linear_dist/2.0f)));
刹车距离get_stopping_point和开方控制器sqrt_controller知乎zinghd​ 无人机,控制器,算法,AI,刹车 作者:15519743871 6637
是不是完全看不懂这些函数在干什么,没关系,把刹车距离带入开方控制器非常直观。
计算刹车距离第一部分正好可以满足开方控制器的判断
刹车距离get_stopping_point和开方控制器sqrt_controller知乎zinghd​ 无人机,控制器,算法,AI,刹车 作者:15519743871 1388
所以对应的可以得到
刹车距离get_stopping_point和开方控制器sqrt_controller知乎zinghd​ 无人机,控制器,算法,AI,刹车 作者:15519743871 645
即刹车起点的期望速度就是当前速度。
再看第二部分,刹车距离的第二部分,正好也可以带入开发控制器的第二部分
刹车距离get_stopping_point和开方控制器sqrt_controller知乎zinghd​ 无人机,控制器,算法,AI,刹车 作者:15519743871 286
也可以使刹车起点的期望速度就是当前速度。
ok,现在很明显了,计算刹车距离的函数,就是为了配合开方控制器,是刹车函数启动的时候,可以是刹车的瞬间,期望速度为当前速度,即可以平滑减速,不会有速度突变。
那期望速度和刹车距离到底是什么关系呢?所以我们需要搞懂开方控制器的原理。
之前分析过开方控制器的果,现在来分析一下开方控制器的原理
开方控制器的分界点是:
这个分界点是通过最大加速度来设计的,最大加速的在t时间内能得到的最大速度为V=a_max * t,以此速度为基础,在时间t内,以最快速度能得到的距离为Vt=a_max * t
刹车距离get_stopping_point和开方控制器sqrt_controller知乎zinghd​ 无人机,控制器,算法,AI,刹车 作者:15519743871 406
所以这个分解点代表着以最大加速度能得到最大速度,能飞行的最远距离,在此距离之内都可以使用飞机最大的加速度来进行减速,整个减速过程不会超过飞行器的最大加速度。
再看第二段,err 大于 linear_{dist} 时
刹车距离get_stopping_point和开方控制器sqrt_controller知乎zinghd​ 无人机,控制器,算法,AI,刹车 作者:15519743871 1937
可以把 err 写成 :
刹车距离get_stopping_point和开方控制器sqrt_controller知乎zinghd​ 无人机,控制器,算法,AI,刹车 作者:15519743871 1244 刹车距离get_stopping_point和开方控制器sqrt_controller知乎zinghd​ 无人机,控制器,算法,AI,刹车 作者:15519743871 3942
现在就很清晰了开方控制器的逻辑
刹车距离get_stopping_point和开方控制器sqrt_controller知乎zinghd​ 无人机,控制器,算法,AI,刹车 作者:15519743871 4801
correction_{rate} 就是 期望速度 ,与当前速度做差,经过PID ,就得到了期望加速度。
根据之前的分析,可以得到期望速度与刹车距离的曲线图。
刹车距离get_stopping_point和开方控制器sqrt_controller知乎zinghd​ 无人机,控制器,算法,AI,刹车 作者:15519743871 4988
我们的开始刹车时,误差是从大到小变化的,如果控制器参数合适的话,随着时间变化可以得到类似下面的曲线:
刹车距离get_stopping_point和开方控制器sqrt_controller知乎zinghd​ 无人机,控制器,算法,AI,刹车 作者:15519743871 9466
因为,可以确定刹车起点的期望速度就是当前速度,所以加速度变化可以得到类似下面的曲线:
刹车距离get_stopping_point和开方控制器sqrt_controller知乎zinghd​ 无人机,控制器,算法,AI,刹车 作者:15519743871 956
做了这么多工作,就是为了让加速度按照这样的曲线变化!
1.初始期望加速度为0,飞机的姿态不会突变。
2.加速度缓慢增加,直到等于最大加速度,飞机在刹车过程姿态变化平滑。
相当于如果飞机当前速度非常快,加了个过度过程。
这个思路用于计算停止点,同时也用来计算leash,配合开方控制器,使飞机在飞行过程中姿态变化平滑,保障了飞行安全。
之前分析过一次开方控制器,不太理解设计原理,只是从结果上分析,现在对开方控制器的设计原理也有了一定的认识,配合停止点的计算,整套思路配合的很好,没想到一个简单的刹车距离也有这么多细节,值得借鉴学习。
关注无人机干货铺,更多干货与你分享
刹车距离get_stopping_point和开方控制器sqrt_controller知乎zinghd​ 无人机,控制器,算法,AI,刹车 作者:15519743871 2105
附录:给出两个函数的源码,方便对照阅读
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;    }}





您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

关闭

站长推荐上一条 /1 下一条

QQ|关于模吧|APP下载|广告报价|手机版|企业会员|商城入驻|联系我们|模吧 ( 黔ICP备2022002348号-1 )

© 2013-2020 Moz8.com 模吧,玩出精彩!