Author:敬文义
Date:2018/11/20
1、 能量率
Px4 中固定翼的最大最小能量率
Px4 总能量控制系统
E=mgh
h=vt
∂E∂m∂t=gv
2、 根据需要的姿态 roll 的值来求飞机空气动力学负荷系数
3、 根据气压计的高度,求出等效空速和真空速的转化值系数
1
Px4 总能量控制系统
4、 用空气动力学负荷系数来更新能量控制的速度
速度更新的 pi 滤波器(只能通过改变加速度才能改变速度,速度只是一个表象,
所以积分的对象是加速度)
1) 求速度差
3) 求加速度 P 的值
In=In−1+ (β2∗∆V)∗∆T (0.5 2䁡
∆V=Vn−Vn−1
2) 求加速度的 I 积分,β为互补滤波器频率
P= 2∗β∗∆V
an=In+an−1+P
Vn=Vn−1+an∗∆T
4) 求当前加速度
5) 求当前速度
代码如下
2
Px4 总能量控制系统
5、期望高度和实际高度
3
Px4 总能量控制系统
期望速度和实际速度
6、没有空速计更新油门
代码思路:1、获取设置模式的固定油门;
2、pitch 方向的油门补偿;
3、Roll 方向的油门补偿,
rollk
_
gain
*
(tan
)
2
,
rollk
_ 横滚角度补
gain
偿增益;
代码如下:
4
Px4 总能量控制系统
7、有空速计的油门更新
油门提供的加速度是用来增加总能量的,飞行的过程中由于空气阻力和重力做
的负功,总能力在不断的减小,所以需要油门不断的提供加速度,维持飞机飞行时需
要的总能量。
代码思路:1、根据期望空速和最大最小空速来计算势能可以变化的最大最小
值;
2、计算总能量差(动能差和势能差之和);
3、计算期望的总能量率;
4、计算能量率差(期望的总能量率与当前势能率和动能率之差);
5、用 0.5s 的低通滤波器对能量率差进行滤波,目的是去除加速度
计的噪声(1/(0.2*10) = 0.5s);
6、计算前馈控制+pid 控制;
5
Px4 总能量控制系统
代码如下:
6
Px4 总能量控制系统
8、pitch 控制
Pitch 的控制关系到在总能量保持不变的情况下,动能和势能分配问题,抬头
(pitch > 0)把动能装换为势能,由于重力一直做负功,所以正常情况下飞机一直有
个 3°左右的仰角(迎风角),低头(pitch < 0)把势能转换成动能;这个过程是通过
升降舵来进行动能和势能的分配的。
代码思路:1、确认动能和势能分配的权重;
2、计算期望势能和期望动能的差值;
3、计算期望势能率和期望动能率的差值;
4、计算势能和动能差值;
5、计算势能率和动能率差值;
6、PID 控制;
代码如下:
7
Px4 总能量控制系统
8