一种无人机仿地飞行控制方法及系统技术领域
本发明涉及无人机技术领域，具体涉及一种无人机仿地飞行控制方法及系统。
背景技术
无人机是一种由无线电遥控设备或自身程序控制装置操纵的无人驾驶飞行器。目
前，随着科技的发展，无人机技术日趋成熟，无人机以其速度快、操作灵活的特点被广泛应
用。尤其在农业、林业植保方面，农用植保无人机相对于其它农用机具有巨大的优势。
农业植保无人机的作业场地包括平原地带和地形起伏地带，地形起伏地带指丘陵
地带、山地和梯田等，在进行作业过程中，为了使得药物均匀喷洒在作物上、提高作业效率，
无人机与作业场地上所种植作物需要保持恒定的距离，即无人机随着地面起伏仿地作业，
无人机始终与地面保持恒定的高度。
现有无人机的仿地作业效果较差，难以适应地形起伏变化，一般采用GPS定高飞
行，以恒定的海拔高度飞行作业，或者采用人工飞行，但是操作困难，而且受到视距影响，导
致作业效率较低。
发明内容
本发明要解决的技术问题是，提供一种地形适应性佳、能够根据地面起伏仿地作
业，仿地效果较好且作业效率较高的无人机仿地飞行控制方法。
解决上述技术问题，本发明提供了一种无人机仿地飞行控制方法，包括如下步骤：
(1)获取无人机相对地面的实时相对高度H；
(2)获取无人机的垂直速度V，所述垂直速度V为以海拔为基准的速度；
(3)将实时相对高度H与垂直速度V进行数据融合滤波，融合后得到伪高度Hf，利用
获取的所述伪高度进行无人机高度控制。
更进一步，所述实时相对高度H获取的步骤如下：
a)采用雷达或超声波获取无人机的测量高度h；
b)采用存储历史数据和求平均值的方式对测量高度进行预处理；
c)通过三轴加速度计获取无人机对象坐标系下的三轴加速度，结合无人机的姿态
角信息进行坐标系变换，获得大地参考系下的垂直加速度a；
d)通过互补滤波器将获取的测量高度h与加速度计获取的垂直加速度a进行数据
融合滤波，从而获取无人机相对地面的实时相对高度H。
更进一步，所述步骤d中互补滤波器的融合过程包括：
通过积分算法对垂直加速度a进行积分获得高度信息h1，
加速度积分公式为：
测量高度h与加速度计获取的高度信息h1进行融合，从而获取无人机的实时相对
高度H，
融合公式：为
更进一步，根据所述伪高度进行无人机高度控制的方法如下：将Hr、Hf、a和V输送给
控制器，控制器控制输出Cs＝f(Hr,Hf,V,a)，其中Hr为高度指令，a为垂直加速度，f(·)为控
制函数，Hr为人为设定的高度值。
更进一步，所述步骤(2)中垂直速度V的获取方法为：无人机上的气压计或者GPS所
获取的数据信息与加速度计所获取的数据信息进行融合后得到该垂直速度V。
更进一步，采用互补滤波器进行步骤(3)的数据融合滤波。
更进一步，所述步骤(3)中垂直速度V进行速度积分得到高度所述
互补滤波器融合的方式为伪高度Hf＝(1-β1)h2+β1H，其中β1为滤波系数。
基于上述本发明提供一种无人机仿地飞行控制系统，包括：
测量单元，用以获取无人机相对地面的实时相对高度H；
速度计算单元，用以获取无人机的垂直速度V，所述垂直速度V为以海拔为基准的
速度；
融合单元，用以将实时相对高度H与垂直速度V进行数据融合滤波，融合后得到伪
高度Hf；
控制输出单元，用以利用获取的所述伪高度进行无人机高度控制。
更进一步，采用互补滤波器进行上述数据融合滤波。
本发明的有益效果：本发明提供的无人机仿地飞行控制方法，能够使得无人机随
着地面起伏调整飞行高度，在植保作业时，能够与植物保持恒定的距离，不仅提高了无人机
的作业效率和植保喷洒效果，而且提升了无人机的环境适应能力。
附图说明
图1是本发明一实施例中的方法流程示意图；
图2是本发明一实施例中的系统结构示意图；
图3是本发明的一实施例中的控制器输入、输出示意图。
具体实施方式
现在将参考一些示例实施例描述本公开的原理。可以理解，这些实施例仅出于说
明并且帮助本领域的技术人员理解和实施例本公开的目的而描述，而非建议对本公开的范
围的任何限制。在此描述的本公开的内容可以以下文描述的方式之外的各种方式实施。
如本文中所述，术语“包括”及其各种变体可以被理解为开放式术语，其意味着“包
括但不限于”。术语“基于”可以被理解为“至少部分地基于”。术语“一个实施例”可以被理解
为“至少一个实施例”。术语“另一实施例”可以被理解为“至少一个其它实施例”。
本领域技术人员能够明了，在飞行控制中，高度数据源通常有绝对高度和相对高
度两种。绝对高度源包括气压计(本身可以感知大气压力，气压与高度存在数学关系)、GPS
(接收机计算其与GPS卫星之间距离获得当前海拔高度)等，通常测量范围可达10000米；相
对高度源有超声波、雷达(均通过对地发射特定波束，通过接收反馈波束计算相对距离等，
通常测量距离<10米。本申请中仿地飞行中测量的是相对高度，即采用雷达或超声波获取无
人机的相对高度(相对地面的高度)。
图1是本发明一实施例中的方法流程示意图，本实施例中的一种无人机仿地飞行
控制方法，包括如下步骤：
步骤(S1)获取无人机相对地面的实时相对高度H；
步骤(S2)获取无人机的垂直速度V，所述垂直速度V为以海拔为基准的速度；
步骤(S3)将实时相对高度H与垂直速度V进行数据融合滤波，融合后得到伪高度
Hf，利用获取的所述伪高度进行无人机高度控制。
作为本实施例中的优选，所述实时相对高度H获取的步骤如下：
a)采用雷达或超声波获取无人机的测量高度h；
b)采用存储历史数据和求平均值的方式对测量高度进行预处理；
c)通过三轴加速度计获取无人机对象坐标系下的三轴加速度，结合无人机的姿态
角信息进行坐标系变换，获得大地参考系下的垂直加速度a；
d)通过互补滤波器将获取的测量高度h与加速度计获取的垂直加速度a进行数据
融合滤波融合，从而获取无人机相对地面的实时相对高度H。
作为本实施例中的优选，所述步骤d中互补滤波器的融合过程包括：
通过积分算法对垂直加速度a进行积分获得高度信息h1，
加速度积分公式为：
测量高度h与加速度计获取的高度信息h1进行融合，从而获取无人机的实时相对
高度H，
融合公式：为
作为本实施例中的优选，根据所述伪高度进行无人机高度控制的方法如下：将Hr、
Hf、a和V输送给控制器，控制器控制输出Cs＝f(Hr,Hf,V,a)，其中Hr为高度指令，a为垂直加速
度，f(·)为控制函数。
作为本实施例中的优选，所述步骤(2)中垂直速度V的获取方法为：无人机上的气
压计或者GPS所获取的数据信息与加速度计所获取的数据信息进行融合后得到该垂直速度
V。
作为本实施例中的优选，采用互补滤波器进行步骤(3)的数据融合滤波。
作为本实施例中的优选，所述步骤(3)中垂直速度V进行速度积分得到高度
所述互补滤波器融合的方式为伪高度Hf＝(1-β1)h2+β1H，其中β1为滤波系
数。
数据融合滤波图3是本发明的一实施例中的控制器输入、输出示意图，作为本实施
例中的优选，根据所述伪高度进行无人机高度控制的方法如下：将Hr、H<、a和V输送给控制
器，控制器控制输出Cs＝f(Hr,Hf,V,a)，其中Hr为高度指令，a为加速度，f(·)为控制函数。
图2是本发明一实施例中的系统结构示意图，本实施例中的一种无人机仿地飞行
控制系统，包括：测量单元1，用以获取无人机相对地面的实时相对高度H；速度计算单元2，
用以获取无人机的垂直速度V，所述垂直速度V为以海拔为基准的速度；融合单元3，用以将
实时相对高度H与垂直速度V进行数据融合滤波，融合后得到伪高度Hf；控制输出单元4，用
以利用获取的所述伪高度进行无人机高度控制。
作为本实施例中的优选，采用互补滤波器进行上述数据融合滤波。
发明原理：
测量阶段：
需要说明的是，在飞行控制中，高度数据源通常有绝对高度和相对高度两种。绝对
高度源包括气压计(本身可以感知大气压力，气压与高度存在数学关系)、GPS(接收机计算
其与GPS卫星之间距离获得当前海拔高度)等，通常测量范围可达10000米；相对高度源有超
声波、雷达(均通过对地发射特定波束，通过接收反馈波束计算相对距离等，通常测量距离<
10米。本发明仿地飞行中测量的是相对高度，即采用雷达或超声波获取无人机的相对高度
(相对地面的高度)。
具体测量过程需要解决两个问题：
1)数据稳定性问题：
红外雷达或超声波在测量过程中受外界环境或飞机姿态影响，测量数据会出现偏
差或跳变，为了解决该问题，需要剔除测得的错误数据，获取较准确的数据；
通过一个算法实现：存储历史数据，剔除前三次最大值和前三次最小值后求平均，
获取一个较为准确的测量高度h，解决了数据稳定性问题。
例如：获取前50ms的十个历史测量数据，剔除前三次最大值和后三次最小值，对其
余四个测量数据进行求平均，得到一个较为准确的测量数据。
2)解决实时性问题
上述解决数据稳定性问题时会带来数据滞后问题，为了解决该问题，包括如下步
骤
a、通过三轴加速度计获取无人机对象坐标系下的三轴加速度，结合无人机的姿态
角信息进行坐标系变换，获得大地参考系下的垂直加速度a；再通过积分算法对垂直加速度
进行积分获得高度信息h1；
加速度积分公式：
(加速度积分产生垂直速度，再积分得到高度)
b、采用互补滤波器算法将1)中获取的测量高度h与加速度计获取的高度信息h1进
行融合，从而获取无人机的高精度测量高度H。充分利用了雷达(或超声波)的精确性和加速
度计的快速性优势，解决了数据不稳定和实时性较差的问题。
互补滤波算法公式：
其中为滤波器系数，的计算方法如下：
Δt为计算频率，fc为平滑系数
控制阶段：
本申请中的无人机的仿地飞行方法，在仿地飞行的控制阶段具体步骤为：
在仿地作业过程中，由于地面起伏不定1)需要控制无人机跟随地面起伏仿地作
业。即保证无人机与地面保持一个相对恒定的高度。2)无人机在进行植保作业时，有些作物
的种植间距较大，需要保证无人机平稳飞经作物间距段。
本申请中通过以下步骤实现上述目的：
1)获取无人机相对地面的实时相对高度H(即测量阶段融合后得到的高度)；
2)获取无人机的垂直速度V(以海拔为基准的速度)；优选地，该速度的获取方法
为：通过气压计或GPS与加速度计融合后获取；
3)采用互补滤波器，将相对高度H与垂直速度V进行数据融合滤波，融合后得到伪
高度Hf。
垂直速度V进行速度积分得到高度hV，即
互补滤波器融合的公式：Hf＝(1-β1)hV+β1H，其中β1为滤波系数，计算公式同
4)利用获取的伪高度进行无人机高度控制，Cs＝f(Hr,Hf,V,a)，其中，f(·)为控制
函数(比如PID积分控制)，其中Hr为高度指令，将Hr、Hf、a和V输送给控制器，控制器控制输
出。
应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述
实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件
或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下
列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路
的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(PGA)，现场
可编程门阵列(FPGA)等。
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示
例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特
点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不
一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何
的一个或多个实施例或示例中以合适的方式结合。
总体而言，本公开的各种实施例可以以硬件或专用电路、软件、逻辑或其任意组合
实施。一些方面可以以硬件实施，而其它一些方面可以以固件或软件实施，该固件或软件可
以由控制器、微处理器或其它计算设备执行。虽然本公开的各种方面被示出和描述为框图、
流程图或使用其它一些绘图表示，但是可以理解本文描述的框、设备、系统、技术或方法可
以以非限制性的方式以硬件、软件、固件、专用电路或逻辑、通用硬件或控制器或其它计算
设备或其一些组合实施。
此外，虽然操作以特定顺序描述，但是这不应被理解为要求这类操作以所示的顺
序执行或是以顺序序列执行，或是要求所有所示的操作被执行以实现期望结果。在一些情
形下，多任务或并行处理可以是有利的。类似地，虽然若干具体实现方式的细节在上面的讨
论中被包含，但是这些不应被解释为对本公开的范围的任何限制，而是特征的描述仅是针
对具体实施例。在分离的一些实施例中描述的某些特征也可以在单个实施例中组合地执
行。相反对，在单个实施例中描述的各种特征也可以在多个实施例中分离地实施或是以任
何合适的子组合的方式实施。