1. 引言
垂直起降无人机是近几年飞速发展的一种新型飞行器 [1]。复合式VTOL无人机兼具了固定翼型无人机和多旋翼型无人机的优点，是近年来发展较快的无人机类型。
随着复合式VTOL无人机的大量飞行实践，人们发现在固定翼模式飞行时，经常出现通过油门改变飞行速度但导致了无人机高度发生变化的现象，或者只进行俯仰操作想改变高度，无人机的速度却发生了改变。造成这样的原因是由于无人机在低动压条件下升降舵和油门杆对无人机响应的耦合特性，使得控制器不能实现飞行速度和飞行高度的单独控制，因此，如何解耦是提升复合式VTOL无人机飞行品质的关键。一般的做法是设计一个油门和升降舵分别单独控制的系统。但由油门杆间合适的协同控制，解耦控制效果并不理想 [2]。Faleiro和Lambregts将飞机作为一个能量系统进行分析，对飞机速度/航迹进行解耦控制 [3]。在Faleiro和Lambregts研究的基础上，将偏差控制引入到基于总能量控制的飞行航迹/速度解耦控制中，从而实现对飞行速度、飞行高度的无稳差控制 [4]。总能量控制在大型有人飞机和导弹上已有大量应用但是在复合式VTOL无人机上的应用还是一个较新的课题。本文以总能量控制算法为核心，设计了把PX4飞控系统数据转化为总能量控制核心算法输入的外回路。
文章还通过对现有的PX4飞控系统仔细研究设计了可嵌入的总能量控制模块，并借助PX4飞控硬件和Gazebo仿真软件验证在完整飞行过程中总能量控制模块的效果。
2. 总能量控制算法在复合式VTOL上的应用
2.1. 总能量控制核心算法
无人机总能量为无人机的动能和势能之和：
(1)
故单位质量的总能量为：
(2)
考虑到复合式VTOL无人机在固定翼模式飞行时的航迹角一般比较小，所以我们假设
，无人机在空中所需要的推力为：
(3)
对(2)式进行微分并且整理可得：
(4)
空气动力学中当飞行器处在小航迹角、低动压飞行状态下有：
(5)
当复合式VTOL无人机处于固定翼模式平飞时，主电机产生的推力T用于抵消无人机飞行时的阻力D。由于此时无人机纵向机动一般较小，速度变化不甚剧烈，因而在短周期运动中，其所受阻力可近似认为不变，从而需用推力的增量只与期望的航迹角切向加速度和飞行重量有关，则推力控制和飞行状态增量方程可写为：
(6)
式中
表示推力的改变量，下标e表示偏差，由上再由(3) (5)易得：
(7)
由上可知，无人机总能量的变化是由推力的变化控制的，升降舵的偏转对无人机总能量的变化影响很小。在推力不变时，升降舵可使无人机的势能和动能相互转换而不发生明显的能量损失。也就是说无人机在主电机输出功率的改变的同时也能按一定比例改变整体无人机系统的能量变化率。
基于上述特性，可以采用PI (比例–积分)控制表达推力与能量误差变化率之间的关系为：
(8)
式中
代表飞行状态改变后的期望推力，这条控制率的作用是当无人机的飞行状态改变时，其能量的变化率为零。
由于将升降舵作为无人机动能、势能的分配器，从而在升降舵控制通道使用能量分配率为控制量，它定义为动能与势能之差：
(9)
由推力的控制律为模板，可以用类似的方法来描述能量分配律误差和升降舵偏角的控制关系为：
(10)
由于系统运行时会在(8) (9)两式的比例通道上产生不期望的零点，从而使得控制响应超调。故而在比例控制中，引入总能量变化率反馈量
和分配率反馈量
，从而可以得到改进后的总能量控制核心算法：
(11)
(12)
再由(5) (9)两式可得：
(13)
(14)
总能量控制系统核心算法结构图如图1所示：
Figure 1. Core algorithm of TECS
图1. 总能量控制系统核心算法结构图
图中
和
为系统实际的航迹角和期望的航迹角，
和
为系统实际的加速度反馈量和期望的加速度反馈量，而该系统输出
为位置控制和无人姿态控制的输入信号。
2.2. 总能量控制核心算法在PX4控制系统上的实现
对于PX4控制系统而言，总能量控制核心算法的系统输入无法在PX4飞控中直接获得，因此需要设计一个系统将PX4飞控中速度、加速度、高度等的期望值、实际值数据转化为总能量控制核心算法的系统输入，称之为总能量控制模块的外回路。
由于在低动压、小航迹角飞行状态下存在
的关系，通过这样的转换就将高度的控制转化为对航迹角的控制。要实现对无人机的高度进行控制，需要须引入升降率参数。按照普遍的升降率获得方法，采用比例控制，以高度控制的输入与高度反馈量的偏差按一定比例关系获取升降率信息。有：
(15)
进而得到期望的航迹角计算公式：
(16)
综上可得高度控制结构如图2所示：
Figure 2. Height control structure diagram of external circuit of TECS
图2. 总能量控制外回路高度控制结构图
同样在PX4系统中加速度反馈期望值
也不能直接获取，可以通过飞行速度的期望值和实际值的差，再通过P控制可以得到：
(17)
综上可得速度控制结构如图3所示：
Figure 3. Speed control structure diagram of external circuit of TECS
图3. 总能量控制外回路速度控制结构图
综合前述的高度控制和速度控制，共同构成了总能量控制模块的外回路，总能量控制核心算法组成总能量控制模块的内回路。从而有PX4下复合式VTOL总能量控制模块结构图如图4所示：
Figure 4. Overall flow chart of composite VTOL total energy control
图4. PX4下复合式VTOL总能量控制模块结构图
3. PX4下复合式VTOL无人机总能量控制模块设计
根据前文所述，依照PX4飞控的编程模式可将总能量控制模块的设计分为，外回路，油门控制和俯仰控制(内回路)三部分。
1) 外回路将获得的参数进行解算并为内回路提供系统输入，头文件如下；
外回路工作完成后，将数据传输给内回路开始能量的运算。在PX4中动能表示为SKE，势能表示为SPE，总能量表示为STE。
2) 油门控制
首先设置一套油门和能量变化率STE_rate的线性对应关系，最小油门–总能量减少最大速率、平飞巡航油门–0、最大油门–总能量增加最大速率，通过计算总能量变化率，即可得出输出油门值。但是这个输出值是一个参考数值，还需要由目标能量和当前能量，通过PID控制器对油门输出动态修正；综上，油门控制流程如图5所示。
Figure 5. Total energy throttle control
图5. 总能量油门控制
代码执行目标：1) 根据空速的期望值和最大、最小空速来计算势能可能的最大值最小值；2) 计算总能量差；3) 计算期望的总能量率；4) 计算能量率差(期望的总能量率与当前势能率和动能率差)；5) 用0.5 s的低通滤波器对能量率差进行滤波，目的是去除加速度计的噪声(1/(0.2 × 10) = 0.5s)；6) 计算前馈控制加PID控制。部分代码如下：
4. 俯仰控制
升降舵作为总能量的分配器需设置动能和势能分配的权重W，其值在0~2之间变化，能量分配 = W*动能参数 + (2 − W)*势能参数，再设置一组能量分配和俯仰角的线性对应关系，之后将能量分配和俯仰角分配输入PID控制器就可以得到俯仰输出。与油门控制相同也需设置SEB变化率和输出之间的线性对应关系如图6所示。
Figure 6. Flow chart of total energy pitch control
图6. 总能量俯仰控制流程图
代码执行目标：1) 确认动能和势能分配的权重；2) 计算期望势能和期望动能的差值；3) 计算期望势能率和期望动能率的差值；4) 计算势能和动能差值；5) 计算势能率和动能率差值；6) PID控制；部分代码如下：
根据前文，要实现复合式VTOL无人机的总能量控制，需要控制无人机的主电机输出和俯仰舵机输出。因此我们模块的嵌入位置应在固定翼模式下。经过对飞控程序的研读，找到了固定翼模式指令TRANSITION_TO_FW故而可得总能量控制模块的工作流程如图7所示。
5. 仿真结果
Gazebo仿真环境支持PX4的硬件在环仿真，在Gazebo软件内加载地图信息和Standard_vtol仿真模型选择复合式VTOL无人机如图8所示。
在搭建好仿真环境后，本次仿真需要验证在总能量控制模块的控制下维持高度一定，改变无人机的速度，观察其高度是否会随之发生改变。为此进行了速度设置分别为15 m/s、0 m/s和5 m/s，分别模拟了实际飞行过程中无人机的速度改变。为了保证仿真过程中高度的维持，使用到了QGroundControl地面站的航点规划功能。在上传航迹地图给PX4飞控和Gazebo仿真环境后(航迹地图如图9所示)，我们将无人机飞行模式设置为Mission，启动无人机模型，使其依照航迹地图完成仿真实验。在仿真飞行完成后，从QGroundControl地面站中下载飞行日志。PX4官网提供了飞行日志解析功能，我们将飞行日志文件上传后可以得到仿真结果如图10、图11所示。
图10所示在整个仿真过程中复合式VTOL无人机的速度由1号航点的5 m/s逐渐加速至3号航点的约15 m/s，之后在4号航点处迅速减小至接近0 m/s，然后又在6号航点处逐步上升至5 m/s最后至8号航点处逐步减少至0进入模式切换，注意图中的蓝色曲线基本为0说明无人机在竖直方向上的速度基本为0；在图11中系统的高度控制曲线基本维持一条直线即50 m左右(说明：图中的500是由于加载的地图地形的海拔高度在500 m左右)，无人机的高度并没有明显的变化。综上可以得出所嵌入的总能量控制模块基本实现了复合式VTOL无人机在固定翼模式下高度和速度的解耦控制。
Figure 7. Work flow of total energy control module
图7. 总能量控制模块的工作流程
6. 总结
本文根据总能量控制原理，进行了总能量控制核心算法的详细推导。设计了PX4下复合式VTOL无人机的总能量控制模块。通过对PX4飞控软件的嵌入并进行了仿真验证，最终实现了PX4控件在复合式VTOL无人机上的总能量控制的应用。