前言前面说过,sqrt_controller是对P项进行整定用途的,目的就是让P项的控制响应“软”下来,实际上就是一个经过改进的P控制器。读懂了sqrt_controller,那么你对APM的P控制器就了解得差不多了。 仅讨论P控制器,sqrt_controller的作用就是设定一个量linear_dist,以这个量的正负值为分界线划分出一个线性区域范围,在这个范围内表示误差量还很小,遵循Kp*error计算方式输出控制量。 超过linear_dist后,表明误差较大,此时不能由着误差单纯按照线性方式乘以Kp输出控制量,需要按照开方形式,使用另外的方式计算出实际控制量,此处后面补充。 在开始讲解源码之前,先来补充个前提: 了解飞控的应该都清楚,一般是使用串级PID来进行姿态等的控制。在APM中,外环角度环就是接收期望与实际的角度误差,然后输入到P控制器中得到期望的角速率。也就是说,这个控制过程可以表示为: 而我们同样知道,假定小角度内速率恒定,那么可知
w
=
e
r
r
o
r
θ
/
d
t
w=error_θ/dt
w=errorθ/dt,而由此可直接定义 而实际上APM中广泛采用了这个方式,因此,后续过程中我们谈论到Kp表明的都是时间的倒数。
源码阅读数学公式的原理放在后面讲解,这边先放源码
数学原理初看这部分程序容易看的云里雾里的,没事,这边带大家重头开始梳理。 后续过程中,我们定义参数p为Kp,second_ord_lim为加速度最大值 a m a x a_{max} amax;后面部分用error表明位移量的误差,v表示速度进行推导。 首先,我们来带大家回顾一下高中知识,还记得高中小车问题里面,位移、速度、加速度和时间中如果已知其中几个量怎么求另外几个吗,我这边直接给出: 那么回到我们的源程序中,我将直接从同时定义了Kp和二阶限制的else部分开始讲。 首先是这部分函数
如前所述,
K
p
=
1
/
d
t
K_p=1/dt
Kp=1/dt,那么下面这段程序可以解释为如下(后面把
d
t
dt
dt简化表示为
t
t
t) 那么由此也可以推算下面这行代码表示的公式
c o r r e c t i o n _ r a t e = 2 a m a x ( e r r o r − ( l i n e a r _ d i s t 2 ) ) = 2 a m a x ( e r r o r − a m a x t 2 2 ) correction\_rate=\sqrt{2a_{max}(error-(\frac{linear\_dist}{2}))}=\sqrt{2a_{max}(error-\frac{a_{max}t^2}{2}}) correction_rate=2amax(error−(2linear_dist)) =2amax(error−2amaxt2 ) 假定初始速度
v
0
v_0
v0为0,那么由
2
a
x
=
v
t
2
2ax=v_t^2
2ax=vt2 和
x
=
1
2
a
t
2
x=\frac{1}{2}at^2
x=21at2 可进一步推算上面的公式 如果 e r r o r error error表示的是角度误差,角速度用 w w w表示的话,那么同样的 c o r r e c t i o n _ r a t e correction\_rate correction_rate计算出来的即是期望的角速率,实际上姿态控制器中就是如此的计算。 那么如果没有定义Kp仅定义了二阶限制的这部分函数我想你肯定也已经能够看懂了,我就不多做解释了
由此,sqrt_controller的内容想必大家应该就能完全理解了。
MATLAB绘制曲线这边用MATLAB绘制了一下sqrt_controller的控制曲线。可以很明显看到在一段区域内是以线性方式进行递增,而当误差过大,达到一定值的时候,则是按照开方函数曲线进行递增。 函数给出如下(太久没用Matlab了,原谅我写的简陋): sqrt_controller函数:
绘图程序:
参考资料: |
|