又一个WordPress站点
热门搜索:
欢迎您,项茜乔 的忠实网友, , 希望你在本站能找到对您有用的东西。
你现在的位置:网站首页 / 全部文章 / 千帜雪txt新浪【飞控】浅谈姿态控制开方控制器和倾斜角转加速度函数-无人机频道

千帜雪txt新浪【飞控】浅谈姿态控制开方控制器和倾斜角转加速度函数-无人机频道

作者:admin | 分类:全部文章 | 超过 89 人围观
【飞控】浅谈姿态控制开方控制器和倾斜角转加速度函数-无人机频道
文 /zinghd
【作者简介】
作者还没有想好怎么描述自己
函数名:sqrt_controller(开方控制器)
位置:
librariesAC_AttitudeControlAC_AttitudeControl.cpp(1047行)
用途:经过改良的kp控制,当kp过大时,会呈现根号曲线,使响应变软
_vel_target.z=AC_AttitudeControl::sqrt_controller(_pos_error.z,_p_pos_z.kP(),_accel_z_cms,_dt);
floatAC_AttitudeControl::sqrt_controller(floaterror,floatp冯瀚圃,floatsecond_ord_lim,floatdt){floatcorrection_rate;if(is_negative(second_ord_lim)||is_zero(second_ord_lim)){//二阶限制为零或负数在这为_accel_z_cmscorrection_rate=error*p;}elseif(is_zero(p)){//p为0在这_p_pos_z.kP()=1if(is_positive(error)){correction_rate=safe_sqrt(2.0f*second_ord_lim*(error));}elseif(is_negative(error)){correction_rate=-safe_sqrt(2.0f*second_ord_lim*(-error));}else{correction_rate=0.0f;}}else{//zing-note:正常进这个函数floatlinear_dist=second_ord_lim/sq(p);if(error>linear_dist){//误差非常大correction_rate=safe_sqrt(2.0f*second_ord_lim*(error-(linear_dist/2.0f)));}elseif(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)){returnconstrain_float(correction_rate北邮世纪学院,-fabsf(error)/dt,fabsf(error)/dt);}else{returncorrection_rate;}}
向左滑动图片可以查看完整代码
可以看出这个函数是在KP控制上加了一些限制我爱神仙遮 ,如果超过限制就做一些处理,如果没有超过限制,这个控制的效果就是普通的KP千帜雪txt新浪 ,_vel_target.z=_pos_error.z*_p_pos_z.kP()在物理上可以理解为这个KP=1/dt,所以这个算法可以认为是位置误差微分得到速度,这个思路在APM中被广泛应用。
那么经过限制后会有一个什么效果呢?最直观的方式就是把函数图像画出来史前巨鸟,假设误差是从[-10,10]匀速变化,经过这个控制器在不同p值下的输出曲线。
可以很明显的看出,在没有达到限制时竹马难当 ,章丽厚输出曲线斜率随着p值增大而变大,慢慢的限制要求达到,曲线慢慢逼近一条曲线,p越大输出越接近这条曲线,这条曲线类似于根号函数的曲线,所以这个函数取名为开方控制器。
这个控制器被广泛应用在APM的控制过程中,甚至是用在处理谢彬dd 遥控器的输入信号,来控制贺满姑 遥控器的软硬程度库斯克邮车,看的代码多了就发现,大家思路都差不多,区别就是这些不起眼的限制条件,先学习框架在学习细节,比如这个误差乘kp得到微分的思路你学会了吗?
函数名:lean_angles_to_accel
位置:
librariesAC_AttitudeControlAC_PosControl.cpp
用途:倾斜角转加速度
原函数:
voidAC_PosControl::lean_angles_to_accel(float&accel_x_cmss,float&accel_y_cmss)const{accel_x_cmss=(GRAVITY_MSS*100)*(-_ahrs.cos_yaw()*_ahrs.sin_pitch()*_ahrs.cos_roll()-_ahrs.sin_yaw()*_ahrs.sin_roll())/MAX(_ahrs.cos_roll()*_ahrs.cos_pitch(),0.5f);accel_y_cmss=(GRAVITY_MSS*100)*(-_ahrs.sin_yaw()*_ahrs.sin_pitch()*_ahrs.cos_roll()+_ahrs.cos_yaw()*_ahrs.sin_roll())/MAX(_ahrs.cos_roll()*_ahrs.cos_pitch(),0.5f);}
向左滑动图片可以查看完整代码
上次讲了加速度转倾斜角,不知道有没有人去思考倾斜角转加速度呢?我们已知倾斜角,也就是已知旋转矩阵,使用该旋转矩阵,将机体坐标系下的加速度转换到地理坐标系下即可。倾斜角转加速度的思路是一样的,假设当前飞机只受重力和旋翼产生的支持力郑日英 ,保持z轴平衡状态。所以有:其中机体坐标系转NED坐标系的旋转矩阵。(是转置哦有兴趣可以去查一下资料
可得:所以有:因产生在NED坐标系下的加速度有:
所以:至于最后为什么有个MAX,作者是这样说的
3 is dependent on there being a large lean angle and angle boost running. If we are in ACRO and at a large lean angle then you are probably holding altitude and the Fbz is probably closer to -mg/(cos_theta * cos_rho). Using the result of 3 will also have the effect of the aircraft going through the unconstrained acceleration part of the Fbz curve on it's way to the low angle causing a large acceleration as it levels. So 3 is not a good option in general.
1 and 2 are reasonable and both will result in a good starting point. 2 will result in a twitch as you say but due to the acceleration limiting in most navigation modes 1 will to. In any case we have to limit the angle to something to prevent the cos terms going to zero so we need some version of 2.
For now I will leave it using the 2 version until we update the output of the navigation to a full 3D acceleration controller.
其实这个限制有三种情况,最终选择了第二种限制方法得到了现在的函数。这个函数的讨论的全过程在https://github.com/ArduPilot/ardupilot/issues/7267可以看看这个最终函数是如何修改得到的。
看完姿态控制觉得姿态部分真的是代码的精华所在贷易查官网,以前只是走马观花的了解了大概真是浪费陈曼青,之后的文章后把姿态控制进行详细的分析,来感受一下精华的魅力。
关注微信公众号,回复【租姿态】即可获取画sqrt_controller函数图的python源码,同时欢迎加作者微信交流。
每周更新
把握产业链脉络
追踪高价值的产品
坚持深度有价值内容定位无人机频道
微信号:auscor
专注于观察分析行业应用无人机领域暴风一号,有深度、有争议、有真话、有干货
历史爆文【干货】浅析APM姿态控制中两个常用的函数为何植保厂商与用户纠纷不断,售后成本高,代理维权难,厂商心里苦?植保无人机市场完全爆发,为何服务市场还是鸡肋?组装机植保厂商未来堪忧,植保无人机上游供应链升级加速走入歧途的植保无人机,出路在何方大汉封禅 ?
« 上一篇 下一篇 »