基于总能量控制的固定翼无人机纵向位速控制系统的制作方法

1.本发明涉及无人机控制技术领域，尤其涉及一种基于总能量控制的固定翼无人机纵向位速控制系统及控制系统的仿真方法。


背景技术：

2.固定翼无人机凭借其优秀的续航在目前无人机研究及应用中崭露头角，但相较于目前火热的四旋翼无人机，固定翼无人机由于运动耦合导致其纵向位速控制愈加复杂。无人机的纵向位速控制回路控制无人机的高度和速度，对于油动固定翼无人机将通过控制升降舵和油门节气门开度来实现。但是由于通过升降舵来控制无人机的高度时会造成无人机速度的变化，通过节气门开度控制无人机的速度时会造成无人机高度的变化，因此基于单输入单输出(single
‑
input single
‑
output，siso)的控制器设计方法会造成无人机纵向控制的耦合。为了实现对速度控制与高度控制的解耦，业内人士提出了总能量控制(total energy control system，tecs)的概念，从控制无人机总能量的变化与分配的角度出发来设计纵向位速控制器，并取得了较好的效果。
3.目前已有的基于总能量控制的小型固定翼无人机纵向位速控制方法有两种，一种是通过模型预测控制和总能量控制计算期望油门和期望俯仰角,然后通过姿态控制进行舵机输出和油门输出,实现无人机的高度智能控制。另一种是根据总能量模型对应的总能量变化率,采用第一比例积分控制律得到推力对应的控制量,根据总能量模型,计算无人机的总能量分配率,根据期望总能量分配率和总能量分配率的差值,采用第二比例积分控制率律得到推力差动的控制量,采用粒子群算法,对第一比例系数、第一积分系数、第二比例系数和第二积分系数进行优化,根据优化后的第一比例系数、第一积分系数、第二比例系数和第二积分系数计算得到推力对应的控制量和推力差动的控制量,根据推力对应的控制量和推力差动的控制量进行无人机飞行控制。
4.上述两种方法中，基于总能量控制的固定翼无人机纵向位速控制器具有参数众多、对系统模型强依赖的特点，因此很难将参数调整到较优的状态。现有的调参方法均具有较差的实时性，无法满足固定翼无人机使用的需求。


技术实现要素：

5.(一)要解决的技术问题
6.针对现有技术的不足，本发明实施例提供一种基于总能量控制的固定翼无人机纵向位速控制系统及控制系统的仿真方法。
7.(二)技术方案
8.为了达到上述目的，本发明采用的主要技术方案包括：
9.第一方面，本发明实施例提供一种基于总能量控制的固定翼无人机纵向位速控制系统，包括：tecs外回路和tecs核心内回路；
10.所述固定翼无人机根据预先定义的路径，获取下一时刻的期望高度和期望速度；
11.所述tecs外回路根据所述期望高度、期望速度和当前时刻所述固定翼无人机的感测数据，通过闭环控制的比例控制器获取用于输出至tecs核心内回路的总能量控制回路信息及总能量分配率控制回路信息；
12.所述tecs核心内回路中的更新期望推力子系统基于所述总能量控制回路信息及总能量分配率控制回路信息输出对应所述期望速度的期望推力；
13.所述tecs核心内回路中的更新期望俯仰角子系统基于所述总能量控制回路信息及总能量分配率控制回路信息输出对应期望高度的期望俯仰角。
14.可选地，所述tecs外回路包括：
15.更新无人机状态子系统、更新速度状态子系统、更新期望速度子系统、更新期望高度子系统和更新能量估计值子系统；
16.所述更新无人机状态子系统用于接收固定翼无人机的旋转矩阵、机体加速度、垂直速度，所述旋转矩阵、机体加速度和垂直速度均为当前时刻的感测数据；输出速度导数、垂直位置状态和垂直速度状态；
17.更新速度状态子系统用于接收期望速度和实际速度，输出真空速和期望真空速；
18.更新期望速度子系统用于接收期望真空速和真空速，输出期望真空速变化率和调整幅度后的期望真空速(即限幅后的期望真空速，使无人机能够达到的合理范围)；
19.更新期望高度子系统用于接收期望高度和实际高度，输出期望高度变化率；
20.更新能量估计值子系统用于接收期望高度、期望真空速、期望真空速变化率、真空速、期望高度变化率、高度、垂直速度和速度导数，输出与总能量控制回路信息及总能量分配率控制回路信息对应的期望势能、期望动能、期望势能变化率、期望动能变化率、估计势能、估计动能、估计势能变化率和估计动能变化率。
21.可选地，tecs核心内回路包括：
22.总能量控制回路子系统，用于接收旋转矩阵、期望势能、期望动能、估计势能、估计动能、期望势能变化率、期望动能变化率、估计势能变化率和估计动能变化率，输出期望推力；
23.总能量分配率控制回路子系统，用于接收期望势能、期望动能、估计势能、估计动能、期望势能变化率、期望动能变化率、估计势能变化率和估计动能变化率，输出期望俯仰角。
24.可选地，总能量控制回路子系统基于下述公式(a1)，获取期望推力；
[0025][0026]
e
e
是能量误差，是能量变化率误差，k
i
为推力积分增益，k
d
为推力微分增益，为能量变化率到推力的增益，τ
thr
为动力时间常数，t
ff
为前馈推力，t
sp
为推力设定值；
[0027]
总能量分配率控制回路子系统基于下述公式(a2)，获取期望俯仰角；
[0028]
[0029]
其中，b
e
是能量分配率误差，是能量分配率变化率误差，θ
sp
为期望俯仰角，为能量分配率变化率到俯仰角的增益，τ
θ
为俯仰角时间常数，为能量分配率变化率设定值，k
i
为俯仰角积分增益，k
d
为俯仰角微分增益。
[0030]
可选地，tecs外回路和tecs核心内回路为基于simulink仿真进行可视化编辑并仿真获取的，且转化为程序代码的总能量控制模型。
[0031]
第二方面，本发明实施例还提供一种基于总能量控制的固定翼无人机纵向位速控制系统的仿真方法，包括：
[0032]
s01、获取所述固定翼无人机中当前时刻的感测数据和下一时刻的期望高度和期望速度；
[0033]
s02、基于simulink的subsystem模块中采用可视化的方式构建tecs外回路和tecs内回路；以使tecs外回路根据所述期望高度、期望速度和当前时刻所述固定翼无人机的感测数据，通过闭环控制的比例控制器获取用于输出至tecs内回路的总能量控制回路信息及总能量分配率控制回路信息；所述tecs核心内回路中的更新期望推力子系统基于所述总能量控制回路信息及总能量分配率控制回路信息输出对应所述期望速度的期望推力；
[0034]
所述tecs核心内回路中的更新期望俯仰角子系统基于所述总能量控制回路信息及总能量分配率控制回路信息输出对应期望高度的期望俯仰角。
[0035]
可选地，s02包括：
[0036]
s021、构建更新无人机状态子系统、更新速度状态子系统、更新期望速度子系统、更新期望高度子系统和更新能量估计值子系统；
[0037]
其中，更新无人机状态子系统基于所述感测数据中的旋转矩阵、机体系加速度、高度和垂直高度，用于实现互补滤波器的功能以计算真空速的导数和垂直高度，用于实现互补滤波器的功能以计算真空速的导数
[0038]
更新速度状态子系统基于期望速度和实际速度，用于实现二阶互补滤波器的功能以计算真空速v
t
；
[0039]
ω为融合机体坐标系x轴加速度和速度的互补滤波器的交叉频率，v
e
为实际速度与期望速度的差值，为速度的二阶导数，为速度的一阶导数，v为速度，通过积分传播到v，δt为采样时间即固定翼无人机采集传感器数据的时间间隔；
[0040]
更新期望速度子系统基于期望真空速和真空速，实现期望空速和期望空速变化率的更新；
[0041][0041]
为期望空速变化率，k
v
为比例因子，v
sp
为期望空速，v为估计空速；
[0042]
更新期望高度子系统基于期望真空速和真空速，实现期望高度和期望高度变化率
的更新；
[0043]
h
sp
(k)＝0.1
×
h
sp
′
(k) 0.9
×
h
sp
(k
‑
1)；
[0044][0045]
其中，h
sp
期望高度，k时刻，当前k时刻设定的期望高度；
[0046]
为期望高度变化率；h为固定翼无人机传感器估计的无人机的实际高度，k
h
是比例控制因子，k
ff
是前馈控制因子；
[0047]
更新能量估计值子系统基于与无人机的高度、速度相关的参数，获取与无人机的单位动能和单位势能相关的参数；
[0048]
e
p
＝gh，g为固定翼无人机的重心加速度，e
p
为单位势能；
[0049]
e
k
＝0.5
×
v
t2
，e
k
为单位动能；
[0050][0050]
为单位势能变化率，
[0051]051]
为单位动能变化率。
[0052]
可选地，s02还包括：
[0053]
s022、构建总能量控制回路子系统，用于接收旋转矩阵、期望势能、期望动能、估计势能、估计动能、期望势能变化率、期望动能变化率、估计势能变化率和估计动能变化率，输出期望推力；
[0054]
s023、构建总能量分配率控制回路子系统，用于接收期望势能、期望动能、估计势能、估计动能、期望势能变化率、期望动能变化率、估计势能变化率和估计动能变化率，输出期望俯仰角。
[0055]
可选地，s022包括：
[0056]
总能量控制回路子系统基于下述公式获取期望推力t
sp
；
[0057][0058]
其中，e
sp
为能量计算值，e为能量，为能量变化率，为能量变化率计算值，k
i
为推力积分增益，k
d
为推力微分增益，为能量变化率到推力的增益，τ
thr
为动力时间常数，t
ff
为前馈推力，t
cruise
为巡航推力；
[0059]
e
e
是能量误差，是能量变化率误差。
[0060]
可选地，s023包括：
[0061]
总能量分配率控制回路子系统基于下述公式获取期望俯仰角；
[0062]
[0063]
其中，b
e
是能量分配率误差，是能量分配率变化率误差，θ
sp
为期望俯仰角，为能量分配率变化率到俯仰角的增益，τ
θ
为俯仰角时间常数，为能量分配率变化率设定值，k
i
为俯仰角积分增益，k
d
为俯仰角微分增益，b
sp
为能量分配率设定值，b为能量分配率，为能量分配率变化率。
[0064]
(三)有益效果
[0065]
本发明的有益效果是：
[0066]
本发明的基于总能量控制的固定翼无人机纵向位速控制系统相较于现有技术的系统能够在无人机执行飞行任务前将飞行参数调整到较优的状态，极大地降低了无人机因为不恰当的飞行参数在飞行实验中造成的经济损失。
[0067]
本发明的基于总能量控制的固定翼无人机纵向位速控制系统其参数少，由于参数少使得计算过程快速，进而使得计算结果的实时性强，可在固定翼无人机中进行推广使用。
[0068]
本发明中基于总能量控制的固定翼无人机纵向位速控制系统的仿真方法，其能够可视化操作的形式查看无人机的高度和速度响应，因为是可视化操作的形式，由此可便于调试人员在仿真过程中进行修改和调试。
[0069]
另外，还可以将仿真的纵向位速控制系统直接转化工程使用，即基于simulink提供的uav toolbox support package for px4 autopilots工具箱可以直接将设计的simulink模型自动转换成c 代码，并基于px4工具链在pixhawk上编译和部署代码，以便工程化应用。
附图说明
[0070]
图1为本发明一实施例提供的仿真过程中基于simulink的顶层示意图；
[0071]
图2为图1中的第一子系统的具体结构示意图；
[0072]
图3为图2中的第一子系统的tecs外回路的子系统的示意图；
[0073]
图4为图2中tecs外回路中更新无人机状态的子系统的仿真示意图；
[0074]
图5为图2中tecs外回路中更新速度状态子系统的仿真示意图；
[0075]
图6为图2中tecs外回路的限制积分饱和的子系统的仿真示意图；
[0076]
图7为图2中tecs外回路的更新期望速度的子系统的仿真示意图；
[0077]
图8为图2中tecs外回路的更新期望高度的子系统的仿真示意图；
[0078]
图9为图2中tecs外回路的更新能量估计值子系统的仿真示意图；
[0079]
图10为tecs核心内回路的仿真示意图；
[0080]
图11为基于无人机的总能量来控制无人机的推力的仿真原理图；
[0081]
图12为总能量控制回路的simulink仿真模型的示意图；
[0082]
图13为前馈油门计算simulink仿真示意图；
[0083]
图14为总能量分配率控制回路simulink仿真原理图；
[0084]
图15为总能量分配率控制回路的simulink仿真模型的示意图。
具体实施方式
[0085]
为了更好的解释本发明，以便于理解，下面结合附图，通过具体实施方式，对本发
明作详细描述。
[0086]
在说明具体方案之前，对使用较多的期望值和估计值进行说明，期望值是固定翼无人机通过计算得出的希望固定翼无人机达到的状态，估计值是固定翼无人机采集真实传感器的感测数据后估计出的无人机的实际状态信息。
[0087]
实施例一
[0088]
本发明实施例提供一种基于总能量控制的固定翼无人机纵向位速控制系统，其包括：tecs外回路和tecs核心内回路；
[0089]
所述固定翼无人机根据预先定义的路径，获取下一时刻的期望高度和期望速度；
[0090]
所述tecs外回路根据所述期望高度、期望速度和当前时刻所述固定翼无人机的感测数据，通过闭环控制的比例控制器获取用于输出至tecs核心内回路的总能量控制回路信息及总能量分配率控制回路信息；
[0091]
所述tecs核心内回路中的更新期望推力子系统基于所述总能量控制回路信息及总能量分配率控制回路信息输出对应所述期望速度的期望推力；
[0092]
所述tecs核心内回路中的更新期望俯仰角子系统基于所述总能量控制回路信息及总能量分配率控制回路信息输出对应期望高度的期望俯仰角。
[0093]
本实施例的控制系统其参数少，实时性强，可在固定翼无人机中进行推广使用。
[0094]
本实施例中的tecs外回路可包括：更新无人机状态子系统、更新速度状态子系统、更新期望速度子系统、更新期望高度子系统和更新能量估计值子系统；
[0095]
所述更新无人机状态子系统用于接收固定翼无人机的旋转矩阵、机体加速度、垂直速度，所述旋转矩阵、机体加速度和垂直速度均为当前时刻的感测数据；输出速度导数、垂直位置状态和垂直速度状态；
[0096]
更新速度状态子系统用于接收期望速度和实际速度，输出真空速和期望真空速；
[0097]
更新期望速度子系统用于接收期望真空速和真空速，输出期望真空速变化率和调整幅度后的期望真空速；该调整幅度后的期望真空速用于实现对输入的真空速进行限幅后再输出，以使得无人机能达到合理范围；
[0098]
更新期望高度子系统用于接收期望高度和实际高度，输出期望高度变化率；
[0099]
更新能量估计值子系统用于接收期望高度、期望真空速、期望真空速变化率、真空速、期望高度变化率、高度、垂直速度和速度导数，输出与总能量控制回路信息及总能量分配率控制回路信息对应的期望势能、期望动能、期望势能变化率、期望动能变化率、估计势能、估计动能、估计势能变化率、估计动能变化率。
[0100]
另外，tecs核心内回路包括：总能量控制回路子系统，用于接收旋转矩阵、期望势能、期望动能、估计势能、估计动能、期望势能变化率、期望动能变化率、估计势能变化率和估计动能变化率，输出期望推力；
[0101]
总能量分配率控制回路子系统，用于接收期望势能、期望动能、估计势能、估计动能、期望势能变化率、期望动能变化率、估计势能变化率和估计动能变化率，输出期望俯仰角。
[0102]
上述实施例中的tecs外回路和tecs核心内回路为基于simulink仿真进行可视化编辑并仿真获取的，能够且转化为程序代码的总能量控制模型应用在固定翼无人机中，应用于固定翼无人机的控制系统相较于现有的系统能够在无人机执行飞行任务前将飞行参
数调整到较优的状态，极大地降低了无人机因为不恰当的飞行参数在飞行实验中造成的经济损失。
[0103]
实施例二
[0104]
本发明实施例提供一种基于总能量控制的固定翼无人机纵向位速控制系统的仿真方法，其包括：
[0105]
s01、获取所述固定翼无人机中当前时刻的感测数据和下一时刻的期望高度和期望速度；
[0106]
s02、基于simulink的subsystem模块中采用可视化的方式构建tecs外回路和tecs内回路；以使tecs外回路根据所述期望高度、期望速度和当前时刻所述固定翼无人机的感测数据，基于闭环控制的比例控制器获取用于输出至tecs内回路的总能量控制回路信息及总能量分配率控制回路信息；tecs内回路中的更新期望推力子系统和更新期望俯仰角子系统基于所述总能量控制回路信息及总能量分配率控制回路信息分别输出对应所述期望高度和期望速度的期望推力和期望俯仰角。
[0107]
在具体实现中，s02包括：
[0108]
s021、构建更新无人机状态子系统、更新速度状态子系统、更新期望速度子系统、更新期望高度子系统和更新能量估计值子系统；
[0109]
其中，更新无人机状态子系统基于所述感测数据中的旋转矩阵、机体系加速度、高度和垂直高度，用于实现互补滤波器的功能以计算真空速的导数度和垂直高度，用于实现互补滤波器的功能以计算真空速的导数
[0110]
更新速度状态子系统基于期望速度和实际速度，用于实现二阶互补滤波器的功能以计算真空速v
t
；
[0111]
ω为融合机体坐标系x轴加速度和速度的互补滤波器的交叉频率，v
e
为实际速度与期望速度的差值，为速度的二阶导数，为速度的一阶导数，v为速度，通过积分传播到v，，δt为采样时间即固定翼无人机采集传感器数据的时间间隔；
[0112]
更新期望速度子系统基于期望真空速和真空速，实现期望空速和期望空速变化率的更新；
[0113][0113]
为期望空速变化率，k
v
为比例因子，v
sp
为期望空速，v为估计空速；
[0114]
更新期望高度子系统基于期望真空速和真空速，实现期望高度和期望高度变化率的更新；
[0115]
h
sp
(k)＝0.1
×
h
sp
′
(k) 0.9
×
h
sp
(k
‑
1)；
[0116][0117]
其中，h
sp
期望高度，k时刻，当前k时刻设定的期望高度，该处加上横线是为了区分，输入的期望高度经过一系列处理重新输出期望高度；
[0118]
为期望高度变化率；h为固定翼无人机传感器估计的无人机的实际高度，k
h
是比例控制因子，k
ff
是前馈控制因子；
[0119]
更新能量估计值子系统基于与无人机的高度、速度相关的参数，获取与无人机的单位动能和单位势能相关的参数；
[0120]
e
p
＝gh，g为固定翼无人机的重心加速度，e
p
为单位势能；
[0121]
e
k
＝0.5
×
v
t2
，e
k
为单位动能；
[0122][0122]
为单位势能变化率，
[0123][0123]
为单位动能变化率。
[0124]
s022、构建总能量控制回路子系统，用于接收旋转矩阵、期望势能、期望动能、估计势能、估计动能、期望势能变化率、期望动能变化率、估计势能变化率和估计动能变化率，输出期望推力；
[0125]
例如，总能量控制回路子系统基于下述公式获取期望推力t
sp
；
[0126][0127]
其中，e
sp
为能量计算值，e为能量，为能量变化率，为能量变化率计算值，k
i
为推力积分增益，k
d
为推力微分增益，为能量变化率到推力的增益，τ
thr
为动力时间常数，t
ff
为前馈推力；e
e
是能量误差，是能量变化率误差。
[0128]
s023、构建总能量分配率控制回路子系统，用于接收期望势能、期望动能、估计势能、估计动能、期望势能变化率、期望动能变化率、估计势能变化率和估计动能变化率，输出期望俯仰角。
[0129]
例如，总能量分配率控制回路子系统基于下述公式获取期望俯仰角；
[0130][0131]
其中，b
e
是能量分配率误差，是能量分配率变化率误差，θ
sp
为期望俯仰角，为能量分配率变化率到俯仰角的增益，τ
θ
为俯仰角时间常数，为能量分配率变化率设定值，k
i
为俯仰角积分增益，k
d
为俯仰角微分增益，b
sp
为能量分配率设定值，b为能量分配率，为能量分配率变化率。
[0132]
本实施例的仿真方法，其能够可视化操作的形式查看无人机的高度和速度响应，便于调试人员在仿真过程中进行修改和调试，以实现在无人机执行飞行任务前可将飞行参
数调整到最优的状态，以较好的仿真降低固定翼无人机在飞行实验中的危险状况。
[0133]
实施例三
[0134]
本发明实施例中针对基于总能量控制的固定翼无人机纵向位速控制系统的仿真方法进行详细说明。
[0135]
本发明实施例提供一种控制系统设计快速开发框架，可以使得入门者快速学习总能量控制的相关理论，以可视化的形式查看无人机的高度和速度响应，使开发者能基于该框架进一步优化固定翼无人机的纵向位速控制系统。最后，可基于simulink提供的uav toolbox support package for px4 autopilots工具箱可以直接将设计的simulink模型自动转换成c 代码，并基于px4工具链在pixhawk上编译和部署代码，以便将其应用到固定翼无人机中，实现工程化应用。
[0136]
本实施例中基于可视化的仿真工具建立实现tecs更新推力及俯仰角的控制系统。
[0137]
需要说明的是，控制系统是通过simulink方式可视化仿真实现的。本实施例中基于tecs理论设计了小型固定翼无人机的用于实现高度控制与速度控制的控制系统，并通过simulink的subsystem模块建立了图1所示的控制系统来封装实现。
[0138]
本实施例的控制系统的输入是期望高度(即下述的高度设定值)、期望速度(即下述的速度设定值)和固定翼无人机上的各传感器数据，控制系统的输出是期望推力和期望俯仰角。若在固定翼无人机中使用时，期望高度和期望速度可为固定翼无人机根据预先定义的路径，获取的下一时刻的高度和速度信息。
[0139]
为更好的理解上述的控制系统，以下对控制系统进行各功能划分，并对划分的子系统进行说明。
[0140]
如图1和图2所示，本实施例的控制系统可划分为tecs外回路和tecs核心内回路。
[0141]
tecs外回路的输入为高度设定值和速度设定值，输出总能量控制回路信息及总能量分配率控制回路信息。
[0142]
如图2所示，tecs的输出可包括：能量计算值、实际的能量、能量变化率计算值、实际的能量变化率、能量分配率计算值、实际的能量分配率、能量分配率变化率计算值以及实际的能量分配率变化率。
[0143]
将tecs外回路输出的总能量控制回路信息及总能量分配率控制回路信息作为tecs核心内回路的输入，tecs核心内回路输出期望推力(推力计算值)和期望俯仰角(俯仰角计算值)。
[0144]
上述tecs外回路输入的期望高度和期望速度是无人机根据自身的任务目标通过路径规划计算求得；tecs核心内回路输出的期望推力将转换为无人机螺旋桨的转速控制器来控制无人机螺旋桨的转速，输出的期望俯仰角则作为无人机俯仰角姿态控制器的输入来控制无人机的俯仰姿态。
[0145]
1、tecs外回路的结构，如图3至图9所示。
[0146]
tecs外回路主要是根据期望高度、期望速度和固定翼无人机的感测数据求出与无人机的动能和势能相关的参数。
[0147]
如图3所示，本实施例的tecs外回路可包括：更新无人机状态子系统、更新速度状态子系统、更新期望速度子系统、更新期望高度子系统和更新能量估计值子系统。该些子系统均采用simulink封装实现。
[0148]
如图4所示，更新无人机状态子系统主要实现了一个互补滤波器来估计无人机的爬升率，并且计算出真空速的导数用来给更新速度状态子系统中的空速互补滤波器使用。图4示出的是更新无人机状态的子系统内部的simulink仿真示意图，图4中的旋转矩阵、机体系加速度、高度和垂直高度均为固定翼无人机的感测数据。
[0149]
地面坐标系下的z
g
轴垂直于地面并指向地心，为了计算出固定翼无人机势能和势能变化率的数值，从通过传感器输出的垂直高度和垂直速度均需要通过参数为
‑
1的增益块。
[0150]
假定无人机飞行时机体坐标系的x轴正方向指向机头，使用x轴测量的加速度来补偿重力，求出参数，然后通过一阶iir滤波器来求出滤波器的参数为0.05：
[0151][0152]
为真空速的导数，为x轴测量的加速度。
[0153]
如图5所示，更新速度状态子系统内部的simulink仿真结构，更新速度状态子系统主要实现了通过二阶互补滤波器来估算真空速v
t
。
[0154]
需要说明的是，传感器输入的当量空速，当量空速是校正空速数据经过具体高度的绝热压缩性修正后的空速值。
[0155]
在该更新速度状态子系统中，首先需要将输入的当量空速转换成真空速，然后再通过二阶互补滤波来获得平滑的真空速v
t
估计值，具体实现过程为：
[0156][0157]
公式(2)中ω为融合机体坐标系x轴加速度和空速的互补滤波器的交叉频率，ω增加将使滤波器中空速的权重增加，ω减少将使滤波器中加速度的权重增加；v
e
为实际空速与估计空速的差值，形式上相当于空速的二阶导数，形式上相当于空速的一阶导数，v形式上相当于空速。通过积分传播到v，为了防止积分饱和，当估计空速太小时需要选择积分器的输入大于0，这在限制积分输出防止饱和子系统中实现。最后v融合根据加速度估算的空速导数通过积分传播到v
t
。
[0158]
真空速表示飞行器飞行时相对于周围空气运动的速度，其属于无人机速度的一种描述形式。
[0159]
如图6所示，在图6示出了限制积分饱和子系统的输入真空速、真空速变化率，输出是真空速变化率输入，图6是对图5中的限制积分输出防止饱和子系统进行放大示意的示意图。
[0160]
需要说明的是，限制积分饱和子系统是更新无人机状态子系统的子系统，属于更新无人机状态子系统的一个组成部分。
[0161]
如图7所示，图7中的更新期望速度子系统主要实现期望空速和期望空速变化率的更新。
[0162]
期望空速v
sp
的更新需要满足不能超过设定的最大空速和最小空速的限制。
[0163]
期望空速变化率的更新是通过比例控制来实现的，将期望空速v
sp
与估计空速v的偏差通过比例因子k
v
，就可以得到期望空速变化率，表示为：
[0164][0165]
为了满足无人机的动力学约束，也需要根据当前的估计空速v来进行限幅，限幅时需要留有50％的余量来确保tecs控制器能够纠正这个误差。
[0166]
更新期望高度子系统主要实现期望高度和期望高度变化率的更新。该子系统内部的simulink仿真示意图如图8所示，更新后的是高度和速度。
[0167]
期望高度h
sp
的更新主要通过移动平滑滤波和一阶iir滤波来实现。当前k时刻设定的期望高度进入该子系统后，首先通过两点移动平滑滤波来降低采样噪声的影响，即：
[0168][0169]
然后再限制h
sp
′
(k)与h
sp
′
(k
‑
1)之间的差值不能超过无人机最大爬升率物理性能的限制，最后通过一阶iir滤波来更新当前k时刻的期望高度h
sp
，滤波器的系数为0.1：
[0170]
h
sp
(k)＝0.1
×
h
sp
′
(k) 0.9
×
h
sp
(k
‑
1)
ꢀꢀꢀ
(5)
[0171]
期望高度变化率的更新是通过基于前馈的比例控制器实现的。基于前馈的比例控制器的设计基于无人机的爬升率与期望高度h
sp
和估计高度h之间的偏差h
e
成正比的假设，控制器的前馈项与无人机当前k时刻的期望高度h
sp
(k)和前一时刻的期望高度h
sp
(k
‑
1)之间的偏差有关，使得无人机在爬升阶段和下降阶段都能实现对高度的紧密跟踪：
[0172][0173]
公式(6)中k
h
是比例控制因子，k
ff
是前馈控制因子。
[0174]
更新能量估计值子系统主要根据输入的与无人机的高度、速度相关的参数转换为与无人机的单位动能和单位势能相关的参数。该子系统内部的simulink仿真示意图如图9所示。
[0175]
在该系统中，将单位势能e
p
表示为：
[0176]
e
p
＝gh
ꢀꢀꢀ
(7)
[0177]
公式(1.15)中g为当地的重心加速度，这里取9.80665。
[0178]
将单位动能e
k
表示为：
[0179]
e
k
＝0.5
×
v
t2
ꢀꢀꢀ
(8)
[0180]
将单位势能变化率表示为：
[0181][0182]
将单位动能变化率表示为：
[0183][0184]
2、tecs核心内回路的结构，如图10至图15所示。
[0185]
tecs核心内回路包括：总能量控制回路子系统(即更新期望推力子系统)、总能量分配率控制回路子系统(即更新期望俯仰角子系统)，如图10所示。
[0186]
总能量控制回路子系统和总能量分配率控制回路子系统的输入均包括单位动能和单位势能的计算值和期望值以及单位动能变化率和单位势能变化率的期望值和期望值，并分别输出期望推力和期望俯仰角。
[0187]
2.1、总能量控制回路子系统的说明
[0188]
总能量控制回路设计的控制器采用的控制方法为基于前馈的pid控制，基于无人机的总能量来控制无人机的推力，simulink仿真原理图如图11所示。需要说明的是上文中提到的均是期望推力，但是实际上控制的是期望油门(从0到1)，油门值越大推力也就越大，所以两个概念是可以等同的。
[0189]
图11中，e
sp
为能量设定值，e为能量，为能量变化率，为能量变化率设定值，k
i
为推力积分增益，k
d
为推力微分增益，为能量变化率到推力的增益，τ
thr
为动力时间常数，t
ff
为前馈推力，t
cruise
为巡航推力，t
sp
为期望推力。
[0190]
从图11可以看出，期望推力的表达式为：
[0191][0192]
公式(11)中e
e
是能量误差，是能量变化率误差。
[0193]
总能量控制回路的simulink仿真模型如图12所示，其中最关键的参数为前馈推力t
ff
。
[0194]
公式(11)中的e
e
可以根据输入的单位动能和单位势能的设定值和估计值求出：
[0195]
e
e
＝p
sp
‑
p
es
k
sp
‑
k
es
ꢀꢀꢀ
(12)
[0196]
公式(12)中p
sp
是单位势能设定值，p
es
单位势能估计值，k
sp
是单位动能设定值，k
es
是单位动能估计值。
[0197]
根据输入的单位动能变化率和单位势能变化率的设定值可以求出并根据无人机动力学约束进行限幅：
[0198][0199]
公式(13)中是单位势能变化率设定值，是单位动能变化率设定值，是能量变化率最小值限制，是能量变化率最大值限制。
[0200]
再根据和输入的单位动能变化率和单位势能变化率的估计值求出并应用一阶iir滤波器来减弱加速度计噪声的影响，滤波器系数为0.2：
[0201][0202]
公式(14)中是单位势能变化率估计值，是单位动能变化率估计值。
[0203]
求出后还需要进一步调整来补偿无人机在转弯时引起的阻力增加，以便求出前馈推力t
ff
。假定诱导阻力与法向过载成线性关系，附加的法向过载则:
[0204][0205]
公式(15)中c
n
为法向过载修正因数。
[0206]
在“计算预测推力”子系统中实现了前馈推力的计算，如图13所示。
[0207]
假定当t＝t
spmax
时当t＝t
cruise
时当t＝t
spmin
时则的具体表达式为：
[0208][0209]
公式(16)中t
spmax
为最大推力期望值，t
spmin
为最小推力期望值。
[0210]
2.2、总能量分配率控制回路子系统的说明
[0211]
总能量分配率控制回路设计的控制器采用的控制方法也是基于前馈的pid控制，通过无人机的总能量分配率来控制无人机的俯仰角，simulink仿真原理图如图14所示。
[0212]
图14中，b
sp
为能量分配率设定值，b为能量分配率，为能量分配率变化率，为能量分配率变化率设定值，k
i
为俯仰角积分增益，k
d
为俯仰角微分增益，为能量分配率变化率到俯仰角的增益，τ
θ
为俯仰角时间常数，θ
sp
为期望俯仰角。
[0213]
从图14可以看出，期望俯仰角的表达式为：
[0214][0215]
公式(17)中b
e
是能量分配率误差，是能量分配率变化率误差。
[0216]
总能量分配率控制回路的simulink仿真模型如图15所示。
[0217]
从图15可以看出，总能量分配率控制回路的输入进入该回路时与势能相关的输入均通过了值为“spe_weighting”的增益w
p
，与动能相关的输入均通过了值为“ske_weighting”的增益w
k
。这两个增益决定了俯仰角是优先控制速度还是优先控制高度。需要先说明：
[0218]
w
p
w
k
＝2
ꢀꢀꢀ
(18)
[0219]
当w
k
＝1时，高度控制和速度控制具有相同的优先级；当w
k
＝0时高度控制的优先级为100％，此时无人机的俯仰角不再主动控制无人机的速度；当w
k
＝2时速度控制的优先级
为100％，此时无人机的俯仰角不再主动控制无人机的高度。
[0220]
本发明的基于总能量控制的固定翼无人机纵向位速控制系统相较于现有技术的系统能够在无人机执行飞行任务前将飞行参数调整到较优的状态，极大地降低了无人机因为不恰当的飞行参数在飞行实验中造成的经济损失。
[0221]
本实施例中的基于总能量控制的固定翼无人机纵向位速控制系统其参数少，实时性强，可在固定翼无人机中进行推广使用。仿真过程中，本发明实施例以可视化操作的形式查看无人机的高度和速度响应，便于调试人员在仿真过程中进行修改和调试。
[0222]
本实施例中，还可以将仿真的纵向位速控制系统直接转化工程使用，即基于simulink提供的uav toolbox support package for px4 autopilots工具箱可以直接将设计的simulink模型自动转换成c 代码，并基于px4工具链在pixhawk上编译和部署代码，以便工程化应用。
[0223]
本领域内的技术人员应明白，本发明的实施例可提供为方法、系统或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例，或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd
‑
rom、光学存储器等)上实施的计算机程序产品的形式。
[0224]
尽管已描述了本发明的优选实施例，但本领域的技术人员在得知了基本创造性概念后，则可对这些实施例作出另外的变更和修改。所以，权利要求应该解释为包括优选实施例以及落入本发明范围的所有变更和修改。
[0225]
显然，本领域的技术人员可以对本发明进行各种修改和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也应该包含这些修改和变型在内。