一种基于地形高度的无人机轨迹平滑预测方法和系统技术方案

本发明专利技术提供了一种基于地形高度的无人机轨迹平滑预测方法和系统，其中，无人机轨迹平滑预测方法包括：提取点云数据对应的原始地形高度并插入至双端队列；计算得到地形高度偏差，判断地形高度偏差的绝对值是否小于或等于限定高度偏差阈值；若是，则估计得到当前帧估算地形高度，插入至双端队列；若否，则对当前帧原始地形高度进行地形趋势过滤并插入至双端队列，计算得到当前帧估算地形高度并插入至双端队列；根据当前帧估算地形高度计算无人机的当前飞行高度并插入至双端队列；对队列进行样条平滑处理，得到平滑后的高程估计值；计算得到无人机的当前预测高度。本发明专利技术的技术方案能解决现有技术中无人机飞行高度变化大，飞行轨迹不够平滑的问题。迹不够平滑的问题。迹不够平滑的问题。

【技术实现步骤摘要】
一种基于地形高度的无人机轨迹平滑预测方法和系统


[0001]本专利技术涉及无人机
，尤其涉及一种基于地形高度的无人机轨迹平滑预测方法和系统。

技术介绍

[0002]仿地飞行是指无人机在飞行作业时，通过设定与已知三维地形相固定的高度，使得飞机自身与目标地物保持恒定高差的一种飞行方式。通过借助仿地飞行，无人机能够适应不同高度的地形，根据测区地形能够自动生成变高航线，从而保持地面分辨率一致，获取更好的的数据效果。
[0003]现有无人机主要采用相机拍照的方式获取地面信息，其主要不足在于需要实时进行三维重建，精度不高且对于复杂物体不具有穿透性，从而无法或者有效的地面信息，这将导致计算波动较大。为了解决上述问题，现有技术采用激光雷达采集地面数据。目前在无人机领域，利用激光雷达采集地面数据的技术已非常普遍，然而，数据采集工作依赖于飞手的手动控制。无人机的航线规划要么利用较低分辨率的卫星地形数据，要么单纯采用2d平面位置规划，这就导致地形高度数据的采集精度差，这就导致无人机总是在同一高度飞行。
[0004]即使能够获取高精度的地形高度数据，无人机在根据地形高度设置飞行高度进行仿地飞行时，前后帧图像以及无人机前后方的地形高度并不一致，仅仅采用相邻数据平均不能有效地平滑地形波动，尤其是出现连续多次估算失误，导致地形波动较大时，这就导致无人机飞行过程中需要快速拉升或下降，无人机仿地飞行过程中飞行高度变化大，飞行轨迹不够平滑。进而导致无人机返工补飞，增加了无人机作业周期，影响项目正常推进。

技术实现思路
/>[0005]本专利技术提供一种基于地形高度的无人机轨迹平滑预测方案，旨在解决现有技术中中无人机仿地飞行过程中飞行高度变化大，飞行轨迹不够平滑的问题。
[0006]为解决上述问题，根据本专利技术的第一方面，本专利技术提出了一种基于地形高度的无人机轨迹平滑预测方法，包括：
[0007]使用无人机载激光雷达获取多帧点云数据，从多帧点云数据中提取每一帧点云数据对应的原始地形高度；
[0008]构建双端队列，按时间顺序依次将每一帧原始地形高度插入至双端队列；
[0009]根据上一帧估算地形高度和当前帧原始地形高度计算得到地形高度偏差，判断双端队列中地形高度偏差的绝对值是否小于或等于限定高度偏差阈值；
[0010]若地形高度偏差的绝对值小于或等于限定高度偏差阈值，则根据双端队列中上一帧估算地形高度和当前帧原始地形高度之间的相邻高度约束关系，计算得到当前帧估算地形高度，将当前帧估算地形高度插入至双端队列；
[0011]若地形高度偏差的绝对值大于限定高度阈值，则对当前帧原始地形高度进行地形趋势过滤，将过滤后的当前帧估算地形高度插入至双端队列，构建双端队列的地形轨迹矩
阵，根据地形轨迹矩阵计算得到当前帧估算地形高度并插入至双端队列；
[0012]根据当前帧估算地形高度，计算得到无人机的当前飞行高度并插入至双端队列；
[0013]使用地形平滑参数对双端队列中的数据进行样条平滑处理，得到平滑后的高程估计值；
[0014]根据当前场景的最大允许坡度和平滑后的高程估计值，计算得到无人机的当前预测高度。
[0015]优选的，上述无人机轨迹平滑预测方法，在构建双端队列，按时间顺序依次将每一帧原始地形高度插入至双端队列的步骤之后还包括：
[0016]判断双端队列的数据长度是否小于或等于预定最小数据长度；
[0017]若双端队列的数据长度小于或等于预定最小数据长度，则根据双端队列中上一帧估算地形高度和当前帧原始地形高度之间的相邻高度约束关系，计算得到当前帧估算地形高度；
[0018]若双端队列的数据长度大于预定最小数据长度，则执行根据上一帧估算地形高度和当前帧原始地形高度计算得到地形高度偏差的步骤。
[0019]优选的，上述轨迹平滑预测方法中，根据双端队列中上一帧估算地形高度和当前帧原始地形高度之间的相邻高度约束关系，计算得到当前帧估算地形高度的步骤，包括：
[0020]使用上一帧估算地形高度、当前帧原始地形高度和相邻最大高度偏差，根据当前帧估算地形高度计算公式：
[0021]H1＝max{Hraw,Hmean
‑
Hdiff}
[0022]H2＝min{H1,Hmean+Hdiff}
[0023]He＝(H2+Hmean)/2
[0024]计算得到当前帧估算地形高度，其中，Hraw为当前帧原始地形高度，Hmean为上一帧估算地形高度，Hdiff为相邻最大高度偏差，H1为第一估计地形高度，H2为第二估计地形高度，He为当前帧估算地形高度；
[0025]将当前帧估算地形高度插入至双端队列中。
[0026]优选的，上述轨迹平滑预测方法中，根据上一帧估算地形高度和当前帧原始地形高度计算得到地形高度偏差的步骤，包括：
[0027]使用上一帧估算地形高度和当前帧原始地形高度，根据地形高度偏差计算公式：dh＝Hmean
–
Hraw，计算得到地形高度偏差，其中，dh为地形高度偏差，Hmean为上一帧估算地形高度，Hraw为当前帧原始地形高度；
[0028]计算双端队列中预定数量帧地形高度的方差；
[0029]使用预定数量帧地形高度的方差，根据限定高度偏差阈值计算公式：
[0030]Hdelta1＝2*S
[0031]Hdelta2＝3*S
[0032]分别计算得到第一限定高度偏差阈值和第二限定高度偏差阈值，其中，Hdelta1为第一限定高度偏差阈值，Hdelta2为第二限定高度偏差阈值，S为预定数量帧地形高度的方差。
[0033]优选的，上述轨迹平滑预测方法中，对当前帧原始地形高度进行地形趋势过滤，将过滤后的当前帧估算地形高度插入至双端队列的步骤，包括：
[0034]使用上一帧估算地形高度和第二限定高度偏差阈值，根据地形趋势过滤公式：
[0035]Hf＝Hmean
–
Hdelta2(dh≥0)
[0036]Hf＝Hmean+Hdelta2(dh<0)
[0037]计算得到过滤后的当前帧估算地形高度；其中，Hf为过滤后的当前帧估算地形高度，Hmean为上一帧估算地形高度，Hdelta2为第二限定高度偏差阈值；
[0038]将过滤后的当前帧估算地形高度插入至双端队列中。
[0039]优选的，上述轨迹平滑预测方法中，构建双端队列的地形轨迹矩阵，根据地形轨迹矩阵计算得到当前帧估算地形高度并插入至双端队列的步骤，包括：
[0040]使用双端队列构建地形轨迹矩阵；
[0041]将地形轨迹矩阵进行奇异值分解，得到左矩阵、右矩阵和奇异值；
[0042]使用左矩阵、右矩阵和奇异值，根据矩阵计算公式：
[0043][0044]构建与地形轨迹矩阵大小相同的地形高度转置矩阵，其中，E0为最大奇异值，U0为左矩阵U的第一列，为右矩阵V的第一列转置；<本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于地形高度的无人机轨迹平滑预测方法，其特征在于，包括：使用无人机载激光雷达获取多帧点云数据，从所述多帧点云数据中提取每一帧点云数据对应的原始地形高度；构建双端队列，按时间顺序依次将每一帧原始地形高度插入至所述双端队列；根据上一帧估算地形高度和当前帧原始地形高度计算得到地形高度偏差，判断所述双端队列中地形高度偏差的绝对值是否小于或等于限定高度偏差阈值；若所述地形高度偏差的绝对值小于或等于限定高度偏差阈值，则根据所述双端队列中上一帧估算地形高度和当前帧原始地形高度之间的相邻高度约束关系，计算得到当前帧估算地形高度，将所述当前帧估算地形高度插入至所述双端队列；若所述地形高度偏差的绝对值大于限定高度阈值，则对所述当前帧原始地形高度进行地形趋势过滤，将过滤后的当前帧估算地形高度插入至所述双端队列，构建所述双端队列的地形轨迹矩阵，根据所述地形轨迹矩阵计算得到当前帧估算地形高度并插入至所述双端队列；根据所述当前帧估算地形高度，计算得到无人机的当前飞行高度并插入至所述双端队列；使用地形平滑参数对所述双端队列中的数据进行样条平滑处理，得到平滑后的高程估计值；根据当前场景的最大允许坡度和所述平滑后的高程估计值，计算得到所述无人机的当前预测高度。2.根据权利要求1所述的无人机轨迹平滑预测方法，其特征在于，所述构建双端队列，按时间顺序依次将每一帧原始地形高度插入至所述双端队列的步骤之后，所述方法还包括：判断所述双端队列的数据长度是否小于或等于预定最小数据长度；若所述双端队列的数据长度小于或等于所述预定最小数据长度，则根据所述双端队列中上一帧估算地形高度和当前帧原始地形高度之间的相邻高度约束关系，计算得到所述当前帧估算地形高度；若所述双端队列的数据长度大于所述预定最小数据长度，则执行根据上一帧估算地形高度和当前帧原始地形高度计算得到地形高度偏差的步骤。3.根据权利要求2所述的轨迹平滑预测方法，其特征在于，所述根据所述双端队列中上一帧估算地形高度和当前帧原始地形高度之间的相邻高度约束关系，计算得到所述当前帧估算地形高度的步骤，包括：使用所述上一帧估算地形高度、当前帧原始地形高度和相邻最大高度偏差，根据当前帧估算地形高度计算公式：H1＝max{Hraw,Hmean
‑
Hdiff}H2＝min{H1,Hmean+Hdiff}He＝(H2+Hmean)/2计算得到所述当前帧估算地形高度，其中，Hraw为当前帧原始地形高度，Hmean为上一帧估算地形高度，Hdiff为相邻最大高度偏差，H1为第一估计地形高度，H2为第二估计地形高度，He为所述当前帧估算地形高度；
将所述当前帧估算地形高度插入至所述双端队列中。4.根据权利要求1或2所述的轨迹平滑预测方法，其特征在于，所述根据上一帧估算地形高度和当前帧原始地形高度计算得到地形高度偏差的步骤，包括：使用所述上一帧估算地形高度和当前帧原始地形高度，根据地形高度偏差计算公式：dh＝Hmean
–
Hraw，计算得到所述地形高度偏差，其中，dh为地形高度偏差，Hmean为上一帧估算地形高度，Hraw为当前帧原始地形高度；计算所述双端队列中预定数量帧地形高度的方差；使用所述预定数量帧地形高度的方差，根据限定高度偏差阈值计算公式：Hdelta1＝2*SHdelta2＝3*S分别计算得到第一限定高度偏差阈值和第二限定高度偏差阈值，其中，Hdelta1为第一限定高度偏差阈值，Hdelta2为第二限定高度偏差阈值，S为所述预定数量帧地形高度的方差。5.根据权利要求4所述的轨迹平滑预测方法，其特征在于，所述对所述当前帧原始地形高度进行地形趋势过滤，将过滤后的当前帧估算地形高度插入至所述双端队列的步骤，包括：使用所述上一帧估算地形高度和所述第二限定高度偏差阈值，根据地形趋势过滤公式：Hf＝Hmean
–...

【专利技术属性】
技术研发人员：张勇，李微微，袁新星，王军，高上，徐光彩，张衡，康泰钟，胡诚，
申请(专利权)人：深圳绿土智新科技有限公司，
类型：发明
国别省市：