一种基于点云最近邻查询的无人机轨迹生成方法与流程

1.本发明属于无人机飞行控制领域，具体是指一种基于点云最近邻查询的无人机轨迹生成方法，适用于搭载直接或间接深度传感器的旋翼类无人机直接基于原始点云进行轨迹生成。


背景技术：

2.无人机在复杂环境中进行飞行任务时，一般要借助深度类传感器如深度相机、激光雷达等，获得障碍物的相对位置信息，并依此建立栅格化地图，进一步规划可执行的轨迹。而在线维护融合后的栅格化地图需要较大的内存和计算开销，且在定位较差的情况下，维持地图的一致性也需要付出较大的算力来保证规划质量。因此，传统环境表征方法直接将传感器数据投影成三维空间点云，利用空间分割如建kd树、r树等进行管理，以便对点云进行最近邻查询。然而这样轻量级的环境表征形式缺少对环境信息的抽象表达，较难直接用来进行轨迹规划。
3.现有技术[1]（ryll markus, et al. "efficient trajectory planning for high speed flight in unknown environments", 2019 international conference on robotics and automation (icra). ieee, 2019）中，直接离散采样轨迹的状态，然后利用最近邻查询进行安全性检测，并进行动力学检测，最终选出可执行的轨迹。由于受到采样分辨率以及轨迹状态表征能力的限制，这类先采样轨迹再检测的方法产生的轨迹远非最优，无法应对较为复杂的环境，且由于完全没有对环境信息做抽象，局部规划的短视性表现地较为明显。现有技术[2]（gao fei, et al. "flying on point clouds: online trajectory generation and autonomous navigation for quadrotors in cluttered environments",journal of field robotics,2019,36(4):710-733）中，通过在线维护一棵随机搜索树，并由该树生成由一串球组成的飞行走廊，最后在飞行走廊中求解qcqp(二次型约束二次规划)来优化贝塞尔曲线所表达的轨迹(参考z wang, et al. "alternating minimization based trajectory generation for quadrotor aggressive flight", ieee robotics and automation letters,2020, 5(3):4836-4843)。这种方式可以应对较为复杂的环境，但后端优化需要较大的开销，且轨迹的时间分配只能提前给定，无法进行优化。同时，随机搜索树是在线根据点云的更新不断修剪的，对定位要求较高，且无法应对动态障碍物的环境。
[0004]
综上所述，现有的方法应用在无人机轨迹生成中普遍存在对算力要求较高、无法保证轨迹规划质量以及稳定性较差等问题。


技术实现要素：

[0005]
针对航迹规划过程中在线维护地图耗费计算资源，而轨迹规划又通常需要通过融合良好的地图进行环境抽象的问题，本发明提出了一种基于点云最近邻查询的无人机轨迹生成方法，该方法提出的规划器不同于传统无图规划器，可以有效地提取和利用环境信息，
提高了在线重规划的一致性和成功率。
[0006]
为了实现上述目的，本发明采用的技术方案为：一种基于点云最近邻查询的无人机轨迹生成方法，包括以下步骤：步骤1：将点云数据利用kd树数据结构进行存储；步骤2：当无人机首次经过未知区域时，基于rrt*方法生成采样点，并构建一个随机抽样树，其中顶点是球形安全区域的中心，边是这些区域之间的连接安全区域；步骤3：根据到起始位置的距离对所有采样点进行排序，并按距离由近及远的顺序加入路径搜索树中；步骤4：根据树枝的生长方向为路径搜索树的每个叶子节点打分，然后依据打分对整棵路径搜索树进行剪枝；步骤5：随着新采样点的产生，对路径搜索树进行扩展并重新布线，生成连接起点和终点的一系列相连的球形安全区域，即安全飞行走廊；步骤6：当无人机重访已经过的区域时，采用先验引导下的重采样策略生成采样点，以提升在不同重规划中生成的飞行走廊的一致性；步骤7：根据安全飞行走廊，采用动态插点法，生成最终轨迹。
[0007]
进一步地，步骤2中，通过kd树中的快速最近邻搜索来找到点云地图中的最近点，求出任意采样点的安全半径，并在最近点上定义一个球形安全区域。
[0008]
进一步地，所述路径搜索树为前向生成树；生成路径搜索树时，先对批采样的节点根据到起始位置的距离进行排序，然后按距离由近及远的顺序加入路径搜索树中。
[0009]
进一步地，步骤4的具体方式为：步骤4a：利用障碍物位置和树枝生长方向对叶子节点进行打分，其中，障碍物位置通过最近邻搜索获得；步骤4b：后序遍历整棵路径搜索树，将每个节点的分数置为其子节点分数的和；步骤4c：进行层序遍历，对每一个树枝，选取其兄弟树枝中的最高分作为标准，将低于此标准一定比例的分枝剪掉，比例的取值范围为30%~50%。
[0010]
进一步地，步骤6中，采用先验引导下的重采样策略生成采样点的方式为，根据每帧的采样结果生成相应的球形安全区域，然后通过连续多帧的球形安全区域估计可行区域分布，形成在空间中一系列重叠的三维高斯分布，基于三维高斯分布生成采样点。
[0011]
进一步地，步骤7中采用的动态插点法，插入点的位置选在球与球的相交部分的边缘，并选取其中到现有轨迹距离最远的一个；通过不断地在安全飞行走廊中找到最合适的位置作为航点，迭代进行轨迹优化，直到轨迹被限制在整个安全飞行走廊中。
[0012]
本发明与现有技术相比具有如下优点：（1）通过动态重采样策略，以更高的概率对前一帧采样区域重新进行查询。基于无人机飞行过程中环境变化具有连续特性，且无人机传感器读数不会突变的基本假设，可以显著提高获得有效采样的成功率。此外，该方法是一种先验分布引导采样，间接提升了与之前规划轨迹的重叠率，从而提升了与历史规划轨迹的一致性。
[0013]
（2）路径搜索树的打分和剪枝，充分利用了最近邻查询的障碍物位置信息，得到了树枝的生长方向撞向障碍物的程度，避免了局部陷阱，同时也给多拓扑规划提供了可通行区域的骨架。
[0014]
（3）在复杂环境下，生成安全飞行走廊的节点球的个数一般都在10个左右，该方法通过动态插点策略，仅需插入1~2个点即可覆盖大部分情景。
[0015]
（4）与传统在线维护地图耗费大量计算资源的轨迹生成方法相比，本发明采用了一种无图规划器，可以直接对未融合的传感器数据进行抽象。
[0016]
（5）为了维护原始的历史信息，本发明采用具有可靠邻近查询算法的有限内存数据结构，又基于采样方案提取自由空间骨架，并在最终的飞行走廊内生成高质量的轨迹。
[0017]
（6）本发明提出的规划器不同于其他无地图的规划器，可以有效地提取和利用环境信息，显著提高了在线重规划的一致性和成功率。
附图说明
[0018]
图1为本发明实施例方法的整体流程图。
[0019]
图2为本发明实施例中通过点云的最近邻查询方式，得到采样点的安全半径示意图。
[0020]
图3为本发明实施例中采用先验引导下的重采样策略生成采样点的动态重采样过程示意图。
[0021]
图4为本发明实施例中路径搜索树打分和剪枝的实现方式示意图。
[0022]
图5为本发明实施例中所获得飞行路径示意图。
具体实施方式
[0023]
以下结合附图对本发明的技术方案和效果作进一步详细描述。
[0024]
一种基于点云最近邻查询的无人机轨迹生成方法，该方法首先通过采样方法得到一棵路径搜索树，进而得到由一串球组成的飞行走廊，然后在走廊中优化轨迹；其次，通过一种无图规划器对未融合的传感器数据进行抽象，并采用具有可靠邻近查询算法的有限内存数据结构；最后，基于采样方案提取自由空间骨架，通过智能航路点选择策略在最终的飞行走廊内生成高质量的轨迹。
[0025]
参照图 1，该方法具体包括以下步骤：步骤1：将点云数据利用kd树数据结构进行存储。
[0026]
步骤2：当无人机首次经过未知区域时，基于rrt*方法生成采样点，并构建一个随机抽样树；参照图2，生成的采样点通过kd树中的快速最近邻搜索来找到点云地图的最近点，并以此计算该采样点的安全半径，在这一点上定义一个球形安全区域，其中顶点是球形安全区域的中心，边是这些区域之间的连接安全区域。
[0027]
步骤3：对所有采样点根据到起始位置的距离进行排序，并按距离由近及远的顺序加入树中，生成路径搜索树。这样做主要是为了避开rrt*十分耗时的重连机制，使其可以快速建立，而不必在线去修改先前生成的树。
[0028]
步骤4：如图4所示，对生成的路径搜索树进行打分和剪枝，根据树枝的生长方向为每个叶子节点打分，其中，图4（b）中的α代表了数值方向与采样点朝障碍物方向的夹角，α越小代表碰撞障碍物可能性越大，得分越小，然后依此对整棵树进行剪枝。具体来说，首先对叶子节点打分，随后后序遍历整棵树，将每个节点的分数为其子节点分数的和，就如图4（c）中每个节点分数为其所有节点分数之和。然后进行层序遍历，对每一个树枝，选取其兄弟树
枝中最高分作为标准，低于此标准一定比例的即被剪掉。经过以上步骤，可以迅速剪掉大部分无用的节点，留下真正表达环境可行区域骨架的树枝。图4（d）即为被保留的可行区域骨架树枝，其中对兄弟节点分数进行归一化处理，其中数字代表了选取该叶子节点的可能性大小。
[0029]
步骤5：随着新采样点的产生，树被扩展并重新布线，生成连接起点和终点的一系列相连的球形安全区域，即安全飞行走廊，如图3所示。
[0030]
步骤6：参照图3，为进一步提升在不同重规划中生成的安全飞行走廊的一致性，采用先验引导下的重采样策略生成采样点。该方案通过连续两帧甚至多帧的采样结果估计可行区域分布，形成在空间中一系列重叠的三维高斯分布，逐步将飞行空间均匀采样过渡到具有一定先验信息的采样过程。其中fov代表传感器在某一刻的视场，s代表无人机的位置，traj代表无人机轨迹。
[0031]
步骤7：参照图 5，在生成由一串球组成的飞行走廊以后，采用动态插点法在飞行走廊中找到最合适的位置作为航点，其中插入点位置选择在球与球相交部分的边缘，并选取距离现有轨迹最远的一个。不断地在飞行走廊中找到最合适的位置作为航点，迭代进行轨迹优化，直到轨迹被限制在整个飞行走廊中，以保证飞行路径的安全性。后端的轨迹生成使用交替优化的方法，可同时优化轨迹的能量和时间消耗，还能满足动力学约束。
[0032]
总之，本发明通过一种基于采样的方案来提取自由空间骨架，通过智能航路点的选择策略在最终的安全飞行走廊内生成高质量的轨迹。不同于其他无地图的规划器，本发明可以有效地提取和利用环境信息。与传统的无图方法相比，本发明在线重规划的一致性和成功率都得到了显著提高，适用于搭载直接或间接深度传感器的旋翼类无人机直接基于原始点云进行轨迹生成。
[0033]
以上描述仅是本发明的一个具体实例，并未构成对本发明的任何限制。显然对于本领域的专业人员来说，在了解了本发明内容和原理后，都可能在不背离本发明原理、结构的情况下，进行形式和细节上的各种修正和改变，但是这些基于本发明思想的修正和改变仍在本发明的权利要求保护范围之内。如：1.对于路径搜索树的实现，任何采样方法的路径搜索树都可以成为本发明的替代方案。如rrt(rapidly-exploring random tree)、rrt*、fmt(fast marching tree)等。
[0034]
2.对于树的打分和剪枝，任何利用了最近邻查询障碍物位置与本身树枝生长方向的判断的方法都可以成为本发明的替代方案。
[0035]
3.对于动态插点法，每一步插点后的轨迹优化方法可以被替代为其他优化方法。如minimum snap。