一种基于地形高度的无人机仿地飞行控制方法和系统与流程

1.本发明涉及无人机技术领域，尤其涉及一种基于地形高度的无人机仿地飞行控制方法和系统。


背景技术：

2.仿地飞行是指无人机在飞行作业时，通过设定与已知三维地形相固定的高度，使得飞机自身与目标地物保持恒定高差的一种飞行方式。通过借助仿地飞行，无人机能够适应不同高度的地形，根据测区地形能够自动生成变高航线，从而保持地面分辨率一致，获取更好的的数据效果。
3.现有无人机主要采用相机拍照的方式获取地面信息，其主要不足在于需要实时进行三维重建，精度不高且对于复杂物体不具有穿透性，从而无法或者有效的地面信息，这将导致计算波动较大。为了解决上述问题，现有技术采用激光雷达采集地面数据。目前在无人机领域，利用激光雷达采集地面数据的技术已非常普遍，然而，数据采集工作依赖于飞手的手动控制。无人机的航线规划要么利用较低分辨率的卫星地形数据，要么单纯采用2d平面位置规划，这就导致无人机总是在同一高度飞行。
4.同一高度飞行，会导致一些复杂地形区域的数据采集较为困难，且增加了飞行风险。同一高度飞行，也会导致一些低洼区域例如山谷地带激光器扫描距离过长，反射强度不够导致数据缺失。此外，由于地形高度波动较大，不同的区域采集到的点密度相差很大，可能导致某些区域点密度不够，进而导致返工补飞，增加了无人机作业周期，影响项目正常推进。另外，利用已有卫星地形数据，可能存在分辨率低获取难度大，同时使用精度也会受到限制。


技术实现要素：

5.本发明提供一种基于地形高度的无人机仿地飞行控制方案，旨在解决现有技术中无人机同一高度飞行，导致复杂地形区域数据采集工作困难，增加飞行风险，且同一高度飞行导致数据缺失，点密度不够，导致返工补飞，增加作业周期，影响项目正常进行的问题。
6.为解决上述问题，根据本发明的第一方面，本发明提供了一种基于地形高度的无人机仿地飞行控制方法，包括：
7.实时获取无人机的点云pos数据，根据点云pos数据计算无人机的当前飞行位置和飞行方向；
8.根据无人机的当前飞行位置和飞行方向过滤点云pos数据，得到无人机载激光雷达的视中轴与地面交点预定距离范围内的激光点云数据；
9.构建激光点云数据的dem三维体素网格，查找dem三维体素网格内各体素对应平面位置的最低点，构建得到dem模型；
10.根据dem模型中任一像素点与相邻像素点的高程差，提取dem模型中的地面点；
11.根据dem模型中地面点的高程值，计算dem模型的地形高度；
12.根据dem模型的地形高度和无人机的当前飞行位置，计算并调整无人机相对地形的飞行高度。
13.优选的，上述无人机仿地飞行控制方法中，实时获取无人机的点云pos数据，根据点云pos数据计算无人机的当前飞行位置和飞行方向的步骤，包括：
14.使用无人机载激光雷达实时获取原始点云数据；
15.根据相同时刻原始点云数据对应的pos信息，转换原始点云数据的坐标，得到点云投影数据；
16.根据点云投影数据，提取无人机的当前飞行位置；
17.根据无人机的当前飞行位置与无人机的上一时刻飞行位置，估计无人机的飞行方向。
18.优选的，上述无人机仿地飞行控制方法中，根据无人机的当前飞行位置和飞行方向过滤点云pos数据，得到无人机载激光雷达的视中轴与地面交点预定距离范围内的激光点云数据的步骤，包括：
19.以无人机的当前飞行位置为起点，构建无人机的飞行方向直线方程；
20.根据飞行方向直线方程，计算点云pos数据中每一激光点到无人机的飞行方向直线的水平距离；
21.根据每一激光点到无人机的飞行方向直线的水平距离，过滤得到飞行方向直线预定水平距离范围内的第一激光点云数据；
22.水平旋转飞行方向直线方程，得到同一水平面上垂直于飞行方向的垂直方向直线方程；
23.根据垂直方向直线方程，逐点计算第一激光点云数据中、每一激光点到无人机的垂直方向直线的水平距离；
24.根据每一激光点到无人机的垂直飞行方向直线的水平距离，过滤得到垂直方向直线预定水平距离范围内的第二激光点云数据，作为无人机载激光雷达的视中轴与地面交点预定距离范围内的激光点云数据。
25.优选的，上述无人机仿地飞行控制方法，在得到无人机载激光雷达的视中轴与地面交点预定距离范围内的激光点云数据的步骤之后，方法还包括：
26.根据kdtree算法生成激光点云数据的k-d树，建立激光点云数据的拓扑关系；
27.遍历激光点云数据中未标记的激光点在k-d树中邻域内所有的激光点，标记所有搜索到的激光点；
28.在所述邻域内所有搜索到的激光点中保留一个激光点；
29.当遍历所述激光点云数据中所有未标记的激光点时，即完成所述激光点云数据的重采样；
30.构建对所述重采样后的点云数据的构建体素格网，当单个体素内点数小于预定一定数量阈值时，将体素内的点视为噪点并滤除。
31.优选的，无人机仿地飞行控制方法在将体素内的点视为噪点并滤除后，还包括：
32.计算激光点与邻域内所有激光点的距离平均值；
33.判断距离平均值是否超出预定距离阈值；
34.若超出预定距离阈值，则标记激光点为噪点；
35.当遍历激光点云数据的所有激光点，得到激光点云数据内的所有噪点时，使用体素滤波算法滤除所有噪点。
36.优选的，上述无人机仿地飞行控制方法，构建激光点云数据的dem三维体素网格，查找dem三维体素网格内各体素对应平面位置的最低点，构建得到dem模型的步骤，包括：
37.按照预定分辨率建立激光点云数据的dem三维体素网格；
38.计算激光点云数据中每一激光点在dem三维体素网格中的体素坐标；
39.根据激光点的体素坐标，计算每个体素包含的激光点数量和激光点id；
40.沿竖直方向从下向上遍历dem三维体素网格，根据每个体素包含的激光点数量和激光点id，查找各体素对应平面位置的最低点；
41.使用dem三维体素网格中所有体素对应平面位置的最低点，构建dem模型。
42.优选的，上述无人机仿地飞行控制方法中，根据dem模型中任一像素点与相邻像素点的高程差，提取dem模型中的地面点的步骤，包括：
43.从dem模型中，查找高程差绝对值大于或等于最大高程差阈值的所有相邻像素点对；
44.以相邻像素点对中、高程最低的像素点为参考地面点，查找与参考地面点的高程差大于或等于最大高程差阈值的所有相邻的超高像素点；
45.从dem模型中剔除所有相邻的超高像素点，以及剔除与所有超高像素点中任一超高像素点的高程差绝对值小于或等于最小高程差阈值的像素点，得到dem模型中的所有地面点。
46.优选的，上述无人机仿地飞行控制方法中，从dem模型中剔除所有相邻的超高像素点，以及剔除与所有超高像素点中任一超高像素点的高程差绝对值小于或等于最小高程差阈值的像素点，得到dem模型中的所有地面点的步骤，包括：
47.遍历dem模型中的每一像素点，判断当前像素点与任一相邻像素点的高程差绝对值是否大于或等于最大高程差阈值；
48.若高程差绝对值大于或等于最大高程差阈值，则选取当前像素点或任一相邻像素点中高程最低的像素点作为参考地面点，剔除高程最高的像素点；
49.判断当前像素点的其他相邻像素点中、是否存在与参考地面点的高程差大于或等于最大高程差阈值的超高像素点；
50.若存在与参考地面点的高程差大于或等于最大高程差阈值的超高像素点；则剔除超高像素点，以及剔除与超高像素点的高程差绝对值小于或等于最小高程差阈值的所有相邻像素点；
51.当遍历dem模型中所有像素点后，将dem模型中所有剩余像素点选作dem模型的地面点。
52.优选的，上述无人机仿地飞行控制方法中，根据dem模型中地面点的高程值，计算dem模型的地形高度的步骤，包括：
53.将dem模型划分为多个dem网格；
54.统计每个dem网格中所有地面点的高程值，按照从大到小顺序对所有地面点的高程值进行排序；
55.对排序结果的前预定比例数量的地面点的高程值进行求和，得到当前地形的平均
高度；
56.根据当前地形的平均高度、前一帧地形的平均高度以及前后帧地形的权重，计算得到当前地形的加权高度，将当前地形的加权高度作为dem模型的地形高度。
57.优选的，上述无人机仿地飞行控制方法，在根据dem模型中地面点的高程值，计算dem模型的地形高度的步骤之后，还包括：
58.构建激光点云数据的dsm体素网格；
59.沿竖直方向顺序遍历dsm体素网格中的每一体素；
60.分别统计dsm体素网格每一体素中激光点的数量，判断激光点的数量是否大于或等于预定数量阈值；
61.若激光点的数量大于或等于预定数量阈值，则确定查找到体素对应平面位置的最高处地物点；
62.当得到dsm体素网格中所有体素对应平面位置的最高处地物点时，从所有最高处地物点中选取高程值最大的最高处地物点，作为激光点云数据对应的最大地物高度。
63.根据本发明的第二方面，本发明还提供了一种基于地形高度的无人机仿地飞行控制系统，包括：
64.存储器、处理器及存储在存储器上并在处理器上运行的基于地形高度的无人机仿地飞行控制程序，无人机仿地飞行程序被处理器执行时实现上述任一项技术方案提供的无人机仿地飞行控制方法的步骤。
65.综上，本发明提供的基于地形高度的无人机仿地飞行控制方案，采用无人机载激光雷达技术，对飞行过程实时采集的激光点云进行处理并估计地形，实时确认无人机与地形的高度偏差，综合历史地形以及飞行轨迹合理预测下一个飞行位置，根据预设的飞行高度实时调整飞行状态，以保持与地面相对稳定的飞行高度，最终达到仿地飞行的效果。
附图说明
66.为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图示出的结构获得其他的附图。
67.图1是本发明实施例提供的一种基于地形高度的无人机仿地飞行控制方法的流程示意图；
68.图2是图1所示实施例提供的一种无人机的当前飞行位置和飞行方向的计算方法的流程示意图；
69.图3是图1所示实施例提供的一种激光点云数据的获取方法的流程示意图；
70.图4是本发明实施例提供的一种激光点云数据的过滤方法的流程示意图；
71.图5是图1所示实施例提供的一种dem模型的构建方法的流程示意图；
72.图6是图1所示实施例提供的一种dem模型中地面点的提取方法的流程示意图；
73.图7是图6所示实施例提供的一种dem模型中所有地面点的获取方法的流程示意图；
74.图8是图1所示实施例提供的一种dem模型的地形高度的计算方法的流程示意图；
75.图9是本发明实施例提供的一种最大地物高度的选取方法的流程示意图；
76.图10是本发明实施例提供的一种基于地形高度的无人机仿地飞行控制系统的结构示意图。
77.本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。
具体实施方式
78.应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
79.本发明实施例的主要解决的技术问题是：
80.目前在无人机领域，数据采集工作依赖于飞手的手动控制。无人机的航线规划要么利用较低分辨率的卫星地形数据，要么单纯采用2d平面位置规划，这就导致无人机总是在同一高度飞行。同一高度飞行，会导致一些复杂地形区域的数据采集较为困难，且增加了飞行风险。此外，由于地形高度波动较大，不同的区域采集到的点密度相差很大，可能导致某些区域点密度不够，进而导致返工补飞，增加了无人机作业周期，影响项目正常推进。为减少上述问题，现有技术采用相机拍照方式或者利用距离和平面拟合方法估计地形。前者需要实时进行三维重建，精度不高，且对林地无法像激光雷达一样具有穿透性，难以获得有效地面信息，导致计算波动大。后者未能对地形整体趋势和飞行轨迹有效估计，也会引起较大波动。
81.为了解决上述问题，本发明下述实施例提供了基于地形高度的无人机仿地飞行控制方案，通过根据激光点云数据建立dem三维体素网格，构建dem模型，然后根据dem模型中任一像素点与相邻像素点的高程差，提取dem模型中的地面点，根据该地面点的高程，计算得到dem模型的地形高度，进而引导无人机在采集数据过程与地形保持相对固定的飞行高度，实现平稳飞行，该方案无需事先打点或者获取高精度地形数据，可应用于城区、山区和林地等多种场景。
82.为实现上述目的，参见图1，图1为本发明实施例提供的一种基于地形高度的无人机仿地飞行控制方法的流程示意图。如图1所示，该无人机仿地飞行控制方法包括：
83.s110：实时获取无人机的点云pos数据，根据点云pos数据计算无人机的当前飞行位置和飞行方向。
84.无人机载激光雷达获取的原始点云数据通常是通过udp传输，传输的数据包内包含有激光点的强度、方位和距离信息，需要通过坐标转换将极坐标转换为笛卡尔坐标，具体转换方式结合当前时刻的pos信息(惯导姿态和gnss位置)计算出激光点的世界坐标，这样就能够转换为utm投影坐标，进而得到包括点云和pos信息在内的点云pos数据，再根据该点云pos数据就能够计算得到无人机的当前飞行位置和飞行方向。在无人机采集端解算原始点云数据后直接发送至仿地算法模块，该仿地算法模块内构建线程安全的缓存存储原始点云数据以及pos信息，累积逐条记录。当算法需要解算数据时，则从缓存中按一定时间间隔读取一定量的原始点云数据和pos数据进行处理。
85.具体地，作为一种优选的实施例，如图2所示，上述无人机仿地飞行控制方法中，步骤s110：实时获取无人机的点云pos数据，根据点云pos数据计算无人机的当前飞行位置和飞行方向的步骤，包括：
86.s111：使用无人机载激光雷达实时获取原始点云数据。无人机载激光雷达实时获
99.z＝nz·
t+z0100.其中，(n
x
,ny,nz)为无人机的直线飞行方向，(x0,y0,z0)为当前飞行位置。
101.s122：根据飞行方向直线方程，计算点云pos数据中每一激光点到无人机的飞行方向直线的水平距离。
102.s123：根据每一激光点到无人机的飞行方向直线的水平距离，过滤得到飞行方向直线预定水平距离范围内的第一激光点云数据。
103.本方案在于去除无人机的飞行方向左右两侧的点云数据，逐点计算上述点云pos数据中每一激光点到纵向直线方程的水平距离，该水平距离即激光点在水平方向上到该飞行方向直线的垂线段，将超出预定水平距离阈值范围的激光点将全部过滤，在预定水平阈值范围内的激光点全部保留，这样就能够将无人机飞行方向的左右两侧较远的点过滤掉。
104.s124：水平旋转飞行方向直线方程，得到同一水平面上垂直于飞行方向的垂直方向直线方程。因为点云pos数据内部使用utm坐标，因此以无人机的所在位置作为坐标原点，竖直方向为z轴，绕着z轴逆时针心还能赚上述飞行方向直线方程90度，即可相当于水平旋转上述飞行方向直线方程，得到同一水平面上垂直于飞行方向的垂直方向直线方程。
105.s125：根据垂直方向直线方程，逐点计算第一激光点云数据中、每一激光点到无人机的垂直方向直线的水平距离。
106.s126：根据每一激光点到无人机的垂直飞行方向直线的水平距离，过滤得到垂直方向直线预定水平距离范围内的第二激光点云数据，作为无人机载激光雷达的视中轴与地面交点预定距离范围内的激光点云数据。
107.逐点计算上述第一激光点云数据中，每一激光点到垂直方向直线方程的距离，超出预定水平距离范围的激光点全部过滤，在该预定水平距离阈值范围内的激光点保留，这样就将无人机当前飞行方向上前后较远的激光点全部过滤，得到上述第二激光点云数据，即无人机载激光雷达的视中轴与地面交点预定距离范围内的激光点云数据。
108.图1所示实施例提供的技术方案，在计算得到无人机载激光雷达的视中轴与地面交点预定距离范围内的激光点云数据后，还包括：
109.s130：构建激光点云数据的dem三维体素网格，查找dem三维体素网格内各体素对应平面位置的最低点，构建得到dem模型。
110.dem(digital elevation model，数字高程模型)是用一组有序数值阵列形式表示地面高程的一种实体地面模型，是数字地形模型dtm的一个分支，其它各种地形特征值均可由此派生。dem模型能够描述包括高程在内的各种地貌因子，如坡度、坡向、坡度变化率等因子在内的线性和非线性组合的空间分布，其中dem模型是零阶单纯的单项数字地貌模型，其他如坡度、坡向及坡度变化率等地貌特性可在dem的基础上派生。本技术实施例在构建dem模型时，首先按照一定分辨率构建一个dem三维体素网格，估算得到点云包围盒，根据包围盒的范围大小查找dem三维体素网格内各体素对应平面位置的最低点，从而构建得到dem模型，使用该dem模型能够确定激光点云数据各激光点对应地面的地形特征。
111.具体地，作为一种优选的实施例，如图5所示，上述无人机仿地飞行控制方法，步骤s130：构建激光点云数据的dem三维体素网格，查找dem三维体素网格内各体素对应平面位置的最低点，构建得到dem模型的步骤，包括：
112.s131：按照预定分辨率建立激光点云数据的dem三维体素网格。具体地，首先按照
一定分辨率构建上述激光点云数据的dem三维体素网格，估算点云包围盒，确定包围盒范围的大小，这样就能够设置dem三维体素网格内各激光点所在的体素坐标。
113.s132：计算激光点云数据中每一激光点在dem三维体素网格中的体素坐标。
114.具体地，得到dem三维体素网格的左下角pl和右上角点pr。在utm坐标系下，以den三维体素网格的左下角坐标点(plx,ply,plz)为原点，假设dem三维体素网格的分辨率大小为r，体素网格的长宽高分别为(l，w，z)，那么dem三维体素网格的大小可表示为：
[0115][0116][0117][0118]
点云中每个激光点pt所在的体素坐标可表示为：
[0119]
nx＝(pt.x-pl.x)/r
[0120]
ny＝(pt.y-pl.y)/r
[0121]
nz＝(pt.z-pl.z)/r
[0122]
根据以上公式，就能够计算得到每个激光点所在的体素并将激光点的id保存到体素中。进而经过计算可知每个体素中包含激光点的点数以及对应的id。
[0123]
s133：根据激光点的体素坐标，计算每个体素包含的激光点数量和激光点id。因为上述公式能够计算得到每个激光点的体素坐标，这样根据上述dem三维体素网格中各体素的左下角点pl和右上角点pr就能够确知每个体素的坐标范围，在得到每个激光点的体素坐标后，就能够确定每个激光点所在的体素，得到每个体素包含激光点的数量和id。
[0124]
s134：沿竖直方向从下向上遍历dem三维体素网格，根据每个体素包含的激光点数量和激光点id，查找各体素对应平面位置的最低点。因为体素对应平面位置的最低点处，其激光点的数量远较其他高度多，因此以dem三维体素网格为基础，沿着竖直方向(即z轴方向)从下向上遍历整个dem三维体素网格，若存在一个体素中激光点的数量大于或等于预定数量n，则认为找到了该体素对应平面位置的最低点，则当前位置不再向上寻找，继续由下向上差值另一平面位置的最低点，知道dem三维体素网格的所有体素平面位置都遍历完成。
[0125]
s135：使用dem三维体素网格中所有体素对应平面位置的最低点，构建dem模型。通过上述遍历过程遍历dem三维体素网格中所有体素对应平面位置的最低点，直到所有体素对应平面位置的最低点遍历完成，通过上述步骤就能够得到一个初始的二维dem模型。
[0126]
另外，若在某些体素对应平面位置找不到有效数量的点，那么将找到的激光点标记为无效值。例如：对于一些可穿透的林地或者地面，能够得到地面点；对于一些建筑区域，提取到的最低点仍然是房顶位置，这些激光点需要去除。
[0127]
本发明实施例提供的技术方案，通过按照预定分辨率建立激光点云数据的dem三维体素网格，然后计算激光点云数据中每一激光点在dem三维体素网格中的体素坐标，就能够得到dem三维体素网格的每一体素包含的激光点数量和激光点id，这样根据每个体素包含的激光点数量和激光点id，从下向上遍历dem三维体素网格，这样就能够得到各体素对应平面位置的最低点，该最低点即体素坐标最低的激光点及其id，使用dem三维体素网格中所
有体素对应平面位置的最低点，就能够构建得到一个二维的dem模型。
[0128]
在构建得到二维初始dem模型后，需要将历史多帧激光点云数据对应的二维初始dem模型添加到当前帧对应的dem模型，得到更大地形范围的dem模型。针对该dem模型，需要对该dem模型进行地面点的快速滤波。具体参见图1。图1所示实施例提供的技术方案在构建得到dem模型后，还包括以下步骤：
[0129]
s140：根据dem模型中任一像素点与相邻像素点的高程差，提取dem模型中的地面点。
[0130]
因为上述步骤得到的dem模型中可能存在地面点，也可能存在高程远超出地面的建筑物，例如巨树和高楼屋顶等，这些建筑物较高且成片存在，需要滤除这些远超出地面的建筑物的像素点，得到dem模型中的地面点。本技术该步骤的具体技术思路是利用dem模型中任一像素点与相邻像素点的高程差，提取dem模型中的地面点，当高程差大于或等于设计的最大高程差阈值时，则确定高程较高的像素点为远超地面点的高大建筑物的像素点，此时对该像素点进行滤除，当滤除所有超过最大高程差阈值的像素点及其相邻高程接近的像素点时，就能够得到dem模型中的地面点。
[0131]
具体如图6所示，作为一种优选的实施例，上述根据dem模型中任一像素点与相邻像素点的高程差，提取dem模型中的地面点的步骤，包括：
[0132]
s141：从dem模型中，查找高程差绝对值大于或等于最大高程差阈值的所有相邻像素点对。
[0133]
s142：以相邻像素点对中、高程最低的像素点为参考地面点，查找与参考地面点的高程差大于或等于最大高程差阈值的所有相邻的超高像素点。
[0134]
s143：从dem模型中剔除所有相邻的超高像素点，以及剔除与所有超高像素点中任一超高像素点的高程差绝对值小于或等于最小高程差阈值的像素点，得到dem模型中的所有地面点。
[0135]
本发明实施例提供的技术方案，从dem模型中查找高程差绝对值大于或等于最大高程差阈值的所有相邻像素点对，这样该相邻像素点对中，高程最高的像素点可能为上述高大建筑物对应的像素点，即超高像素点，此时剔除该高程最高的像素点，将高程最低的像素点作为参考地面点，比较与该参考地面点的高程差大于或等于最大高程差阈值的相邻的像素点，该像素点即超高像素点，将该超高像素点剔除以及剔除高超高像素点周围高程差接近的像素点，就能够滤除dem模型中的高大建筑物。例如，针对一些高楼大厦或者树木的点云，其地面点相对较低，而建筑物或树木点较高且成片分布。在dem三维体素网格进行前后左右递归处理过程中，若发现位置较低的参考地面点，则将相邻高程偏差较小且与该参考地面点的高程偏差较大的成片像素点滤除，就能够将dem模型中所有高大建筑或树木的像素点滤除，只保留dem模型中的地面点。
[0136]
具体地，作为一种优选的实施例，如图7所示，上述无人机仿地飞行控制方法中，步骤s143：从dem模型中剔除所有相邻的超高像素点，以及剔除与所有超高像素点中任一超高像素点的高程差绝对值小于或等于最小高程差阈值的像素点，得到dem模型中的所有地面点的步骤，包括：
[0137]
s1431：遍历dem模型中的每一像素点，判断当前像素点与任一相邻像素点的高程差绝对值是否大于或等于最大高程差阈值。
[0138]
s1432：若高程差绝对值大于或等于最大高程差阈值，则选取当前像素点或任一相邻像素点中高程最低的像素点作为参考地面点，剔除高程最高的像素点。
[0139]
s1433：判断当前像素点的其他相邻像素点中、是否存在与参考地面点的高程差大于或等于最大高程差阈值的超高像素点。
[0140]
s1434：若存在与参考地面点的高程差大于或等于最大高程差阈值的超高像素点；则剔除超高像素点，以及剔除与超高像素点的高程差绝对值小于或等于最小高程差阈值的所有相邻像素点。
[0141]
s1435：当遍历dem模型中所有像素点后，将dem模型中所有剩余像素点选作dem模型的地面点。
[0142]
本发明实施例提供的技术方案，通过遍历dem模型中的每一像素点，判断该当前像素点与任一相邻像素点的高程差绝对值是否大于或等于最大高程差阈值，若是，则两个相邻像素点中高程最高的像素点可能为高大建筑物对应的像素点需要剔除，此时选取高程最低的像素点作为参考地面点，判断当前像素点的其他相邻像素点中，是否存在于该参考地面点的高程差大于或等于最大高程差阈值的超高像素点，从而将超高像素点和与该超高像素点高程差小于或等于最小高程差阈值的所有相邻像素点剔除，通过遍历完dem模型的所有像素点后，就能够将dem模型中所有剩余像素点作为dem模型的地面点，将与地面点高程相差过大的像素点(高大建筑物)全部滤除。
[0143]
具体地，作为一种具体的实施例，逐点遍历dem模型中每个像素点，若遍历过程中发现该像素点标记为无效值或已处理，则不予处理；否则进行下一步骤；
[0144]
分别比较像素点的当前位置a(nx,ny)与临近上下左右位置的高程差，如果当前位置a与临近上下左右临近位置b的高程差绝对值大于或等于最大高程差阈值dmax，那么将当前位置a标记为已处理，设置该当前位置a的高程为无效值。此时以临近位置b的高程(即高程最低的点)为参考高度，递归处理该临近位置b的下一个临近位置c；若下一个临近位置c与临近位置d高程差小于或等于最小高程差阈值dmin，且该下一临近位置c与临近位置b的高程差大于或等于最大高程差阈值dmax，则剔除该临近位置c和和临近位置d，这样递归处理，直到当前点附近的所有位置的坐标标记完成或者不满足递归条件。
[0145]
反之，如果当前位置a的高程小于临近位置b的高程差达到阈值-dmax，那么将临近位置b标记为已处理，且设置该临近位置b的高程为无效值，将当前位置a的高程设置为参考高程，递归处理下一个临近位置c。如果下一临近位置c与该临近位置b的高程差小于或等于最小高程差阈值dmin，且该临近位置c与当前位置a的参考高程的高程差大于或等于最大高程差阈值dmax，那么可持续递归进行，直到满足要求的像素全部处理完毕。
[0146]
通过以上步骤，便可把地面点附近较高的点，即把临近位置存在高差达到最大高程差阈值dmax的高点剔除，仅保留地面点。在得到地面点后还需要对dem模型进行过滤，剔除dem模型中所有的离群点。具体遍历dem模型中所有像素，对任何一个有效值必须满足该位置上下左右像素至少存在一个有效值，否则设为无效值予以剔除。遍历dem模型的所有像素，提取包括当前位置在内的临近9个像素位置的高程值，按照高程值大小顺序排序，若当高程的前值大于中值，则将当前值用中值替代。通过上述步骤能够进一步将dem模型中的一些临近高点去除，得到更加精确的地面点。
[0147]
在得到dem模型的地面点后，需要根据该地面点计算地形高度，具体参见图1，图1
所示实施例提供的技术方案，在提取dem模型的地面点后，还包括以下步骤：
[0148]
s150：根据dem模型中地面点的高程值，计算dem模型的地形高度。上述步骤已经滤除了dem模型中高度较高的点，仅保留了dem模型中的地面点，因此通过计算地面点的高程值就能够计算得到dem模型的地形高度，进而进行地形趋势的估计和高度补偿，确定无人机下方地形高度的变化趋势。
[0149]
具体地，作为一种优选的实施例，如图8所示，上述无人机仿地飞行控制方法中，步骤s150：根据dem模型中地面点的高程值，计算dem模型的地形高度的步骤，包括：
[0150]
s151：将dem模型划分为多个dem网格。具体地，将当前dem模型划分为3*3的大网格，将多个小网格合并为一个大网格中。若原始的dem模型的长宽分别为l和w，则每个新的大网格大小分布为l/3和w/3。
[0151]
s152：统计每个dem网格中所有地面点的高程值，按照从大到小顺序对所有地面点的高程值进行排序。
[0152]
s153：对排序结果的前预定比例数量的地面点的高程值进行求和，得到当前地形的平均高度。统计每个dem网格中所有地面点的高程值，然后从大到小对所有地面点的高程值进行排序，然后选取前预定比例数量，例如10％的地面点的高程值进行求和，从而得到当前地形的平均高度。
[0153]
s154：根据当前地形的平均高度、前一帧地形的平均高度以及前后帧地形的权重，计算得到当前地形的加权高度，将当前地形的加权高度作为dem模型的地形高度。
[0154]
由于估算得到的当前地形的平均高度仍然可能存在较大波动，并且由于相邻区域的高程变化不大，因此为了减少波动，本技术实施例利用上一帧估算得到的高程与当前帧估算得到的高程进行加权平均，从而得到当前估算最终的高程值。设上一帧估算的地形高度为h1，其权重为w1；当前帧估算的地形高度为h2，其权重为w2，那么加权高度h＝h1*w1+h2*w2，其中w1+w2＝1。该加权高度即减少波动后的当前地形的高度。最后在计算得到dem所有地面点的高程值后对地形趋势进行估计和高度补偿，就能够得到dem所有地面点的加权后的地形高度。
[0155]
图1所示实施例提供的技术方案，在得到dem模型的地形高度后，还包括以下步骤：
[0156]
s160：根据dem模型的地形高度和无人机的当前飞行位置，计算并调整无人机相对地形的飞行高度。
[0157]
在获取dem模型中地面的地形高度，就能够调整无人机的当前飞行位置，使得无人机实时跟踪地形高度，相对地形高度以特定的高度差仿地飞行，保持无人机与地面的相对高度，从而保持无人机飞行平稳，减少了无人机的飞行风险，减少返工补飞的情况，本发明实施例无需事先打点或获取高精度地形数据，能够应用于城区、山地和林地等多种场景。
[0158]
综上，本发明上述实施例提供的无人机仿地飞行控制方法，通过实时获取无人机的点云pos数据，根据该点云pos数据计算无人机的当前飞行位置和飞行方向，然后根据该无人机的当前飞行位置和飞行方向过滤点云pos数据，从而得到无人机载就昂雷达的视中轴与地面交点预定距离范围内的激光点云数据，该激光点云数据反映了地面点的三维坐标，通过该激光点云数据能够构建得到dem三维体素网格，然后查找dem三维体素网格内各体素对应平面位置的最低点，从而构建得到dem模型，这样使用该dem模型中任一像素点与相邻像素点的高程差就能够提取得到地面点，进而计算得到dem模型的地形高度，在得到
dem模型的地形高度后，调整无人机的当前飞行位置，使得无人机跟随地面的地形高度，就能够调整无人机的飞行高度，使得无人机仿地飞行。通过上述方式，能够保持无人机与地面的相对高度，从而保持无人机飞行平稳，减少了无人机的飞行风险，减少返工补飞的情况，本发明实施例无需事先打点或获取高精度地形数据，能够应用于城区、山地和林地等多种场景。
[0159]
另外，在上述步骤s120，得到无人机机载激光雷达的视中轴与地面交点预定距离范围内的激光点云数据的步骤之后，还需要对点云密度进行过滤，去除采集到的多帧点云的噪点和离群点。具体地，作为一种优选的实施例，如图4所示，上述无人机仿地飞行控制方法，在步骤s120：得到无人机载激光雷达的视中轴与地面交点预定距离范围内的激光点云数据的步骤之后，方法还包括：
[0160]
s210：根据kdtree算法生成激光点云数据的k-d树，建立激光点云数据的拓扑关系。
[0161]
s220：遍历激光点云数据中未标记的激光点在k-d树中邻域内所有的激光点，标记所有搜索到的激光点；当遍历所述激光点云数据中所有未标记的激光点时，即完成所述激光点云数据的重采样；
[0162]
s230：构建对所述重采样后的点云数据的构建体素格网，当单个体素内点数小于预定一定数量阈值时，将体素内的点被视为噪点并滤除；
[0163]
s240：判断距离平均值是否超出预定距离阈值。
[0164]
s250：若超出预定距离阈值，则标记激光点为噪点。
[0165]
s260：当遍历激光点云数据的所有激光点，得到激光点云数据内的所有噪点时，使用体素滤波算法滤除所有噪点。
[0166]
因为实时采集的多帧点云存在重复扫描和位置重叠等情况，这就导致采集到的多帧点云存在大量的噪点和离群点。所以本发明实施例提供的技术方案根据激光点云数据的密度，过滤处理可有效去除一定噪点和离群点。首先构建kdtree进行采样，遍历激光点云数据中的所有激光点，利用kdtree按照一定半径查找点云。若查找的点已处理，则不再处理；若查找的点未处理，则将未处理的点标记已处理，并将当前点加入到结果中，迭代进行。在采样完成后利用体素滤波，单个体素内点数小于一定预定距离阈值的点云全部过滤，这样就能够剔除离群点和噪点，达到去噪效果。
[0167]
另外，作为一种优选的实施例，如图9所示，上述无人机仿地飞行控制方法，在步骤150：根据dem模型中地面点的高程值，计算dem模型的地形高度的步骤之后，还包括：
[0168]
s310：构建激光点云数据的dsm体素网格；dsm体素网格的构建方式与dem体素网格的构建方式类似，在此不再赘述。
[0169]
s320：沿竖直方向顺序遍历dsm体素网格中的每一体素；以dsm网格为举出，沿着竖直方向从下往上逐个遍历dsm网格中的每个体素，就能够确定每个体素中激光点的数量。
[0170]
s330：分别统计dsm体素网格每一体素中激光点的数量，判断激光点的数量是否大于或等于预定数量阈值；
[0171]
s340：若激光点的数量大于或等于预定数量阈值，则确定查找到体素对应平面位置的最高处地物点；
[0172]
s350：当得到dsm体素网格中所有体素对应平面位置的最高处地物点时，从所有最
高处地物点中选取高程值最大的最高处地物点，作为激光点云数据对应的最大地物高度。
[0173]
本发明实施例提供的技术方案，以dsm体素网格为基础，沿着竖直方向从上往下逐个遍历dsm网格中每个体素，若发现存在点数达到设定数量阈值时，则认为找到体素对应平面位置的最高处地物点，不再向下查找；否则，继续向下查找直到遍历完竖直方向的所有体素。当遍历完所有平面位置后得到初始dsm模型。之后遍历该dsm模型所有高程值，选取当前dsm模型高程范围内的最大值，作为最大地物高度。
[0174]
另外，基于上述方法实施例的同一构思，本发明实施例还提供了一种基于地形高度的无人机仿地飞行控制系统，用于实现本发明的上述方法，由于该系统实施例解决问题的原理与方法相似，因此至少具有上述实施例的技术方案所带来的所有有益效果，在此不再一一赘述。
[0175]
参见图10，图10为本发明提供的一种基于地形高度的无人机仿地飞行控制系统的结构示意图。如图10所示，该基于地形高度的无人机仿地飞行控制系统，包括：
[0176]
处理器1001、通信线路1002、通信模块1003和存储器1004以及存储在存储器1004上并在处理器1001上运行的基于地形高度的无人机仿地飞行控制程序，无人机仿地飞行程序被处理器1001执行时实现上述任一实施例提供的无人机仿地飞行控制方法的步骤。
[0177]
综上，本发明提供的基于地形高度的无人机仿地飞行控制方案，采用无人机激光雷达技术，对飞行过程实时采集的激光点云进行处理并估计地形，实时确认无人机与地形的高度偏差，综合历史地形以及飞行轨迹合理预测下一个飞行位置，根据预设的飞行高度实时调整飞行状态，以保持与地面相对稳定的飞行高度，最终达到仿地飞行的效果。
[0178]
本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。
[0179]
本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
[0180]
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
[0181]
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
[0182]
应当注意的是，在权利要求中，不应将位于括号之间的任何参考符号构造成对权
利要求的限制。单词“包含”不排除存在未列在权利要求中的部件或步骤。位于部件之前的单词“一”或“一个”不排除存在多个这样的部件。本发明可以借助于包括有若干不同部件的硬件以及借助于适当编程的计算机来实现。在列举了若干装置的单元权利要求中，这些装置中的若干个可以是通过同一个硬件项来具体体现。单词第一、第二、以及第三等的使用不表示任何顺序。可将这些单词解释为名称。
[0183]
尽管已描述了本发明的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例作出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明范围的所有变更和修改。
[0184]
显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。

技术特征：
1.一种基于地形高度的无人机仿地飞行控制方法，其特征在于，包括：实时获取无人机的点云pos数据，根据所述点云pos数据计算所述无人机的当前飞行位置和飞行方向；根据所述无人机的当前飞行位置和飞行方向过滤所述点云pos数据，得到无人机载激光雷达的视中轴与地面交点预定距离范围内的激光点云数据；构建所述激光点云数据的dem三维体素网格，查找所述dem三维体素网格内各体素对应平面位置的最低点，构建得到dem模型；根据所述dem模型中任一像素点与相邻像素点的高程差，提取所述dem模型中的地面点；根据所述dem模型中地面点的高程值，计算所述dem模型的地形高度；根据所述dem模型的地形高度和所述无人机的当前飞行位置，计算并调整所述无人机相对地形的飞行高度。2.根据权利要求1所述的无人机仿地飞行控制方法，其特征在于，所述实时获取无人机的点云pos数据，根据所述点云pos数据计算所述无人机的当前飞行位置和飞行方向的步骤，包括：使用所述无人机载激光雷达实时获取原始点云数据；根据相同时刻所述原始点云数据对应的pos信息，转换所述原始点云数据的坐标，得到点云投影坐标数据；根据所述点云投影数据，提取所述无人机的当前飞行位置；根据所述无人机的当前飞行位置与所述无人机的上一时刻飞行位置，估计所述无人机的飞行方向。3.根据权利要求1所述的无人机仿地飞行控制方法，其特征在于，所述根据所述无人机的当前飞行位置和飞行方向过滤所述点云pos数据，得到无人机载激光雷达的视中轴与地面交点预定距离范围内的激光点云数据的步骤，包括：以所述无人机的当前飞行位置为起点，构建所述无人机的飞行方向直线方程；根据所述飞行方向直线方程，计算所述点云pos数据中每一激光点到所述无人机的飞行方向直线的水平距离；根据所述每一激光点到所述无人机的飞行方向直线的水平距离，过滤得到所述飞行方向直线预定水平距离范围内的第一激光点云数据；水平旋转所述飞行方向直线方程，得到同一水平面上垂直于所述飞行方向的垂直方向直线方程；根据所述垂直方向直线方程，逐点计算所述第一激光点云数据中、每一激光点到所述无人机的垂直方向直线的水平距离；根据所述每一激光点到所述无人机的垂直飞行方向直线的水平距离，过滤得到所述垂直方向直线预定水平距离范围内的第二激光点云数据，作为所述无人机载激光雷达的视中轴与地面交点预定距离范围内的激光点云数据。4.根据权利要求1所述的无人机仿地飞行控制方法，其特征在于，在得到无人机载激光雷达的视中轴与地面交点预定距离范围内的激光点云数据的步骤之后，所述方法还包括：根据kdtree算法生成所述激光点云数据的k-d树，建立所述激光点云数据的拓扑关系；
遍历所述激光点云数据中未标记的激光点在所述k-d树中邻域内所有的激光点，标记所有搜索到的激光点；在所述邻域内所有搜索到的激光点中保留一个激光点；当遍历所述激光点云数据中所有未标记的激光点时，即完成所述激光点云数据的重采样；构建重采样后的点云数据的体素格网，当单个体素内点数小于预定数量阈值时，将体素内的点视为噪点并滤除。5.根据权利要求1所述的无人机仿地飞行控制方法，其特征在于，所述构建所述激光点云数据的dem三维体素网格，查找所述dem三维体素网格内各体素对应平面位置的最低点，构建得到dem模型的步骤，包括：按照预定分辨率建立所述激光点云数据的dem三维体素网格；计算所述激光点云数据中每一激光点在所述dem三维体素网格中的体素坐标；根据所述激光点的体素坐标，计算每个体素包含的激光点数量和激光点id；沿竖直方向从下向上遍历所述dem三维体素网格，根据每个体素包含的所述激光点数量和激光点id，查找所述各体素对应平面位置的最低点；使用所述dem三维体素网格中所有体素对应平面位置的最低点，构建所述dem模型。6.根据权利要求5所述的无人机仿地飞行控制方法，其特征在于，所述根据所述dem模型中任一像素点与相邻像素点的高程差，提取所述dem模型中的地面点的步骤，包括：从所述dem模型中，查找高程差绝对值大于或等于最大高程差阈值的所有相邻像素点对；以所述相邻像素点对中、高程最低的像素点为参考地面点，查找与所述参考地面点的高程差大于或等于最大高程差阈值的所有相邻的超高像素点；从所述dem模型中剔除所述所有相邻的超高像素点，以及剔除与所述所有超高像素点中任一超高像素点的高程差绝对值小于或等于最小高程差阈值的像素点，得到所述dem模型中的所有地面点。7.根据权利要求6所述的无人机仿地飞行控制方法，其特征在于，所述从所述dem模型中剔除所述所有相邻的超高像素点，以及剔除与所述所有超高像素点中任一超高像素点的高程差绝对值小于或等于最小高程差阈值的像素点，得到所述dem模型中的所有地面点的步骤，包括：遍历所述dem模型中的每一像素点，判断当前像素点与任一相邻像素点的高程差绝对值是否大于或等于最大高程差阈值；若所述高程差绝对值大于或等于最大高程差阈值，则选取当前像素点或任一相邻像素点中高程最低的像素点作为所述参考地面点，剔除高程最高的像素点；判断所述当前像素点的其他相邻像素点中、是否存在与所述参考地面点的高程差大于或等于所述最大高程差阈值的超高像素点；若存在与所述参考地面点的高程差大于或等于所述最大高程差阈值的超高像素点；则剔除所述超高像素点，以及剔除与所述超高像素点的高程差绝对值小于或等于最小高程差阈值的所有相邻像素点；当遍历所述dem模型中所有像素点后，将所述dem模型中所有剩余像素点选作所述dem
模型的地面点。8.根据权利要求6所述的无人机仿地飞行控制方法，其特征在于，所述根据所述dem模型中地面点的高程值，计算所述dem模型的地形高度的步骤，包括：将所述dem模型划分为多个dem网格；统计每个dem网格中所有地面点的高程值，按照从大到小顺序对所述所有地面点的高程值进行排序；对排序结果的前预定比例数量的地面点的高程值进行求和，得到当前地形的平均高度；根据所述当前地形的平均高度、前一帧地形的平均高度以及前后帧地形的权重，计算得到所述当前地形的加权高度，将所述当前地形的加权高度作为所述dem模型的地形高度。9.根据权利要求1所述的无人机仿地飞行控制方法，其特征在于，所述根据所述dem模型中地面点的高程值，计算所述dem模型的地形高度的步骤之后，所述方法还包括：构建所述激光点云数据的dsm体素网格；沿竖直方向顺序遍历所述dsm体素网格中的每一体素；分别统计所述dsm体素网格每一体素中激光点的数量，判断所述激光点的数量是否大于或等于预定数量阈值；若所述激光点的数量大于或等于预定数量阈值，则确定查找到所述体素对应平面位置的最高处地物点；当得到所述dsm体素网格中所有体素对应平面位置的最高处地物点时，从所述所有最高处地物点中选取高程值最大的最高处地物点，作为所述激光点云数据对应的最大地物高度。10.一种基于地形高度的无人机仿地飞行控制系统，其特征在于，包括：存储器、处理器及存储在所述存储器上并在所述处理器上运行的基于地形高度的无人机仿地飞行控制程序，所述无人机仿地飞行程序被所述处理器执行时实现如权利要求1至9中任一项所述的无人机仿地飞行控制方法的步骤。

技术总结
本发明提供了一种基于地形高度的无人机仿地飞行控制方法和系统，其中，无人机仿地飞行控制方法包括获取无人机的点云POS数据，根据点云POS数据计算无人机的当前飞行位置和飞行方向；根据当前飞行位置和飞行方向过滤点云POS数据，得到预定范围内的激光点云数据；根据激光点云数据构建DEM三维体素网格，查找网格内各体素对应平面位置的最低点，构建得到DEM模型；根据DEM模型中任一像素点与相邻像素点的高程差提取地面点；根据DEM模型中地面点的高程值计算DEM模型的地形高度；根据DEM模型的地形高度和无人机的当前飞行位置，计算并调整无人机相对地形的飞行高度。本发明的技术方案能解决现有技术中难以提取有效的地面信息，导致无人机总是同一高度飞行的问题。致无人机总是同一高度飞行的问题。致无人机总是同一高度飞行的问题。


技术研发人员：张勇 覃卫文 赵宝林 徐光彩 张衡 康泰钟 胡诚
受保护的技术使用者：深圳绿土智新科技有限公司
技术研发日：2023.04.13
技术公布日：2023/8/24