CN116182859A - 路径规划方法及装置、设备、存储介质 - Google Patents

Abstract

本申请提供了一种路径规划方法及装置、设备、存储介质；包括：将待规划路径的起始点作为第一采样点，根据第一采样点所处位置的空旷区域的体积，确定第一采样点对应的随机概率；基于第一采样点对应的随机概率，生成第二采样点；从第一采样点和第二采样点中选出与待规划路径的终点之间的距离满足第一条件的采样点作为参考点；根据参考点所处位置的空旷区域的体积，生成第三采样点，从已有的采样点中选出与终点之间的距离满足第一条件的采样点作为新的参考点，直至最终得到的新的参考点与终点的距离满足第二条件为止，对由已有的采样点和终点组成的路径进行裁剪，得到目标路径。如此，能够更加快速地确定出通往待规划路径的终点的路径。

Description

路径规划方法及装置、设备、存储介质

技术领域

本申请涉及信息处理技术，涉及但不限于一种路径规划方法及装置、设备、存储介质。

背景技术

近年来，无人机的研究和应用在民用方面越来越多，如目标跟踪、图像拼接、电力巡线、海岛监测、海岸线巡查、灾后监测以及河流汛期监测等。然而无人机要完成上述的任务，必须具备自主飞行的能力，也就是面对任何复杂的环境，无人机不需要外部协助能独立完成任务。而导航功能是无人机自主飞行中的关键环节，需要使其在复杂的飞行环境中也能完美的避开障碍物，规划出一条航程代价小并且满足无人机约束条件的飞行路径，完成无人机的预设任务。

相关技术中的路径规划算法有人工势场法、模糊规则法、遗传算法、神经网络、模拟退火算法、蚁群优化算法等。但这些方法都需要在一个确定的空间内对障碍物进行建模，不适合解决无人机在复杂环境中的规划。

基于改进型快速扩展随机树(Rapidly-exploring random tree，RRT)的路径规划算法，通过对状态空间中的采样点进行碰撞检测，避免了对空间的建模，能够有效地解决高维空间和复杂约束的路径规划问题。该方法的特点是能够快速有效地搜索高维空间，通过状态空间的随机采样点，把搜索导向空白区域，从而寻找到一条从起始点到终点的规划路径，适合解决无人机在复杂环境下和动态环境中的路径规划。其中，如何在复杂环境下快速确定出电子设备的目标路径，是一个至关重要的问题。

发明内容

有鉴于此，本申请提供的路径规划方法及装置、设备、存储介质，能够更加快速地确定出通往待规划路径的终点的路径，从而更加快速地完成路径规划。

根据本申请实施例的一个方面，提供一种路径规划方法，包括：将待规划路径的起始点作为第一采样点，根据所述第一采样点在地图中所处位置的空旷区域的体积，确定第一采样点对应的随机概率；基于第一采样点对应的随机概率，生成第二采样点；从所述第一采样点和所述第二采样点中选出与所述待规划路径的终点之间的距离满足第一条件的采样点作为参考点；根据所述参考点所处位置的空旷区域的体积，生成第三采样点，以及从已有的采样点中选出与所述终点之间的距离满足所述第一条件的采样点作为新的参考点，直至最终得到的新的参考点与所述终点的距离满足第二条件为止，对由已有的采样点和所述终点组成的路径进行裁剪，得到目标路径。

根据本申请实施例的一个方面，提供一种路径规划装置，包括：生成模块，用于将待规划路径的起始点作为第一采样点，根据所述第一采样点在地图中所处位置的空旷区域的体积，确定第一采样点对应的随机概率；基于第一采样点对应的随机概率，生成第二采样点；选择模块，用于从所述第一采样点和所述第二采样点中选出与所述待规划路径的终点之间的距离满足第一条件的采样点作为参考点；所述生成模块，还用于根据所述参考点所处位置的空旷区域的体积，生成第三采样点，以及所述选择模块，还用于从已有的采样点中选出与所述终点之间的距离满足所述第一条件的采样点作为新的参考点，直至最终得到的新的参考点与所述终点的距离满足第二条件为止；裁剪模块，用于对由已有的采样点和所述终点组成的路径进行裁剪，得到目标路径。

根据本申请实施例的一个方面，提供一种电子设备，包括存储器和处理器，所述存储器存储有可在处理器上运行的计算机程序，所述处理器执行所述程序时实现本申请实施例所述的方法。

根据本申请实施例的一个方面，提供一种计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现本申请实施例提供的所述的方法。

在本申请实施例中，在基于上一采样点生成下一采样点时，是基于上一采样点所处位置的空旷区域的体积，确定出上一采样点对应的随机概率P；基于随机概率P，来生成下一采样点的。一方面，采样点所处位置的空旷区域越大，说明当前采样点朝着终点延伸的方向碰撞到障碍物的概率越小，相应地，其对应的随机概率P也较大，从而使得下一采样点能够以较大概率在上一采样点朝向终点延伸的方向上生成，从而能够更快速地确定出通往待规划路径的终点的路径；另一方面，采样点所处位置的空旷区域越小，说明当前采样点朝着终点前进的方向碰撞到障碍物的概率越大，相应地，其对应的随机概率P也较小，从而使得生成的下一采样点能够以较大概率沿着随机方向延伸，避免因为新的路径分支产生过少而出现不能够确定出通往终点的路径的问题。

应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本申请。

附图说明

此处的附图被并入说明书中并构成本说明书的一部分，这些附图示出了符合本申请的实施例，并与说明书一起用于说明本申请的技术方案。显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

附图中所示的流程图仅是示例性说明，不是必须包括所有的内容和操作/步骤，也不是必须按所描述的顺序执行。例如，有的操作/步骤还可以分解，而有的操作/步骤可以合并或部分合并，因此实际执行的顺序有可能根据实际情况改变。

图1为本申请实施例提供的一种路径规划方法的实现流程示意图；

图2为本申请实施例提供的一种八叉树的划分示意图；

图3为本申请实施例提供的一种路径规划方法的实现流程示意图；

图4为本申请实施例提供的一种路径规划方法的实现流程示意图；

图5为本申请实施例提供的基于RRT算法的无人机导航方法示意图；

图6为本申请实施例提供的改进型RRT算法的实现流程示意图；

图7为本申请实施例路径规划装置的结构示意图；

图8为本申请实施例提供的电子设备的结构示意图。

具体实施方式

为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请的具体技术方案做进一步详细描述。以下实施例用于说明本申请，但不用来限制本申请的范围。

除非另有定义，本文所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同。本文中所使用的术语只是为了描述本申请实施例的目的，不是旨在限制本申请。

在以下的描述中，涉及到“一些实施例”，其描述了所有可能实施例的子集，但是可以理解，“一些实施例”可以是所有可能实施例的相同子集或不同子集，并且可以在不冲突的情况下相互结合。

本申请实施例提供一种路径规划方法，该方法应用于电子设备，该电子设备在实施的过程中可以为各种类型的具有信息处理能力的设备，例如所述电子设备可以包括可移动设备，如无人机、机器人或车辆等；还可以包括能够为可移动设备进行路径规划的设备，如个人计算机、笔记本电脑、手机或车载电脑等。该方法所实现的功能可以通过电子设备中的处理器调用程序代码来实现，当然程序代码可以保存在计算机存储介质中，可见，该电子设备至少包括处理器和存储介质。

图1为本申请实施例提供的路径规划方法的实现流程示意图，如图1所示，该方法可以包括以下步骤101至步骤104：

步骤101，将待规划路径的起始点作为第一采样点，根据第一采样点在地图中所处位置的空旷区域的体积，确定第一采样点对应的随机概率；其中，随机概率用于表征第一采样点的延伸方向朝向待规划路径的终点的可能性大小。

本申请实施例提供的路径规划方法可以应用于多种场景，例如，应用于无人机的航迹规划，室内机器人的路径规划或无人驾驶汽车的路线规划等场景。

可以理解地，在对路径进行规划之前，需要先获取电子设备待活动区域的地图。在一些实施例中，可以利用电子设备上的传感器设备获取待行驶区域的点云数据，根据点云数据构建三维离线地图。其中，对于传感器设备的类型不做限定，例如为摄像头、高分辨率激光设备、红外摄像仪等可感知周围环境的设备。电子设备可借助传感器设备来感知获取地图范围内的环境信息，例如哪个方位存在障碍物、存在何种障碍物等等。

在一些实施例中，在构建出三维离线地图之后，可以将其转换为八叉树结构的格式进行存储。

其中，八叉树地图是三维栅格地图的一种表现形式，在八叉树中，当某个节点的子节点均被占据或者空闲时，则不用对其进行进一步的分解，也即不用再分解为更精细的子节点，只需要存储该节点(父节点)即可，从而能够节省存储空间，提高三维栅格地图的效率，从而能够解决分辨率和算法效率的矛盾。

如图2所示，可以将三维离线地图看作一个最大的立方体，对应八叉树的根节点；再将该立方体划分成八个一级立方体，每个一级立方体对应根节点的一级子节点，其中的每个节点表示在立方体中的空间，被称为体素(Voxel)；更进一步地，将每个一级立方体再划分成八个二级立方体，每个二级立方体对应该一级立方体所对应的一级子节点的子节点，即二级子节点……直到得到符合分辨率大小的子节点，也就是每个最小子节点的大小为分辨率，如分辨率为0.05m，则说明每个最小子节点的大小就是5cm的立方体。其中，每个节点表示在立方体中的空间，通过上述方法将空间层层划分，对应的八叉树各节点存储对应的空间划分区域的环境信息。每个立方体通常会有一个数来描述它是否被占据或者空闲，如果占据则表示为障碍物，电子设备不能通过，当为空闲则表示为自由区域，电子设备可以通过。最后，可根据八叉树结构的各个立方体中所存储的环境信息建立三维栅格地图。

在本申请实施例中，在得到电子设备对应的待行驶区域的三维栅格地图后，利用RRT算法进行三维栅格地图中的路径规划。其中，RRT算法能够快速有效地搜索高维空间，通过状态空间的随机采样点，把搜索导向空白区域，从而确定出至少一条从起始点到终点的路径。

需要说明的是，在一些实施例中，在利用RRT算法进行路径规划时，引入的随机概率P为固定值。例如，在一些场景中，将随机概率P设置为0.1，也即，在生成下一采样点时，有10％的可能性是在上一采样点朝向终点的延伸方向上生成下一采样点；有90％的可能性是基于上一采样点，在三维栅格地图中随机选择一个方向生成下一采样点。但这种方式路径规划方式并不能够快速确定出通往待规划路径的终点的路径，这是因为：假如采样点和终点之间并无障碍物，但在执行路径规划时，还是以较大概率随机选择一个方向生成下一采样点，显然这样会造成较大的资源浪费，导致确定速度变慢。

而在本申请实施例中，随机概率P的大小与上一采样点在地图中所处位置的空旷区域的体积有关。即，第一采样点在地图中所处位置的空旷区域的体积越大，说明第一采样点朝向终点延伸的方向碰撞到障碍物的概率越小，相应地，其对应的随机概率P也较大；第一采样点所处位置的空旷区域越小，说明当前采样点朝向终点延伸的方向碰撞到障碍物的概率越大，相应地，其对应的随机概率P也较小。随机概率P的数值是动态调整的，并非是固定不变的。

在一些实施例中，采样点在地图中所处位置的空旷区域的体积，可以根据八叉树结构中被占据的立方体个数来确定。举例来说，如图2所示，假设八叉树结构划分为两层即满足分辨率要求，第一采样点位于八叉树结构的第一层划分出的立方体201的中心，对201进一步划分后，得到第二层中由8个小立方体组成的大立方体202。从图2中可以看出，立方体202中有部分空间被占据(呈现为黑色小立方体)，部分空间未被占据(呈现为白色小立方体)，则白色小立方体的体积和为第一采样点在地图中所处位置的空旷区域的体积。

步骤102，基于第一采样点对应的随机概率，生成第二采样点。

可以理解地，在进行路径规划时，待规划路径的起始点和终点是已知的，据此建立采样点集合V，将起始点作为第一采样点存储入采样点集合V中(即初始时刻采样点集合V中仅包含起始点)，然后基于第一采样点生成第二采样点，再将第二采样点存储在采样点集合V中。

在本申请实施例中，在基于上一采样点生成下一采样点时，是基于上一采样点所处位置的空旷区域的体积，确定出上一采样点对应的随机概率P；基于随机概率P，来生成下一采样点的。一方面，采样点所处位置的空旷区域越大，说明当前采样点朝着终点延伸的方向碰撞到障碍物的概率越小，相应地，其对应的随机概率P也较大，从而使得下一采样点能够以较大概率在上一采样点朝向终点延伸的方向上生成，从而能够更快速地确定出通往待规划路径的终点的路径；另一方面，采样点所处位置的空旷区域越小，说明当前采样点朝着终点前进的方向碰撞到障碍物的概率越大，相应地，其对应的随机概率P也较小，从而使得生成的下一采样点能够以较大概率沿着随机方向延伸，避免因为新的路径分支产生过少而出现不能够确定出通往终点的路径的问题。

步骤103，从第一采样点和第二采样点中选出与待规划路径的终点之间的距离满足第一条件的采样点作为参考点。

在本申请实施例中，在基于上一采样点生成下一采样点时，是从采样点集合V中包含的所有采样点中选出与待规划路径的终点之间的距离满足第一条件的采样点作为参考点，基于该参考点来生成下一采样点的。

举例来说，当采样点集合V中仅包含起始点(第一采样点)时，该起始点即为与终点之间的距离满足第一条件的采样点，因此，在生成第二采样点时，是基于起始点(第一采样点)来生成的；当采样点集合V中包含第一采样点和第二采样点时，在生成下一采样点(即第三采样点)时，需要先从第一采样点和第二采样点中选出与待规划路径的终点之间的距离满足第一条件的采样点作为参考点(该参考点可能是第一采样点也可能是第二采样点)，再基于该参考点生成第三采样点。

在一些实施例中，满足第一条件为与待规划路径的终点之间的距离最小。

步骤104，根据参考点所处位置的空旷区域的体积，生成第三采样点，以及从已有的采样点中选出与所述终点之间的距离满足第一条件的采样点作为新的参考点，直至最终得到的新的参考点与所述终点的距离满足第二条件为止，对由已有的采样点和所述终点组成的路径进行裁剪，得到目标路径。

在本申请实施例中，在生成第三采样点后，将其存储在采样点集合V中，然后重复执行从采样点集合V中选出新的参考点的步骤，直至选出的新的参考点与终点之间的距离满足第二条件为止。

在一些实施例中，满足第二条件，即为从采样点集合V中选出的最新的参考点与终点的距离小于特定数值即可，该特定数值可以根据实际应用需求来确定。这样，在搜寻待规划路径的终点时，并不严格要求必须搜寻到终点，而是搜寻到终点附近一定距离范围内的采样点即可，如此，能够更为快速地完成路径规划。在得到满足第二条件的采样点之后，将其与终点一同存储在采样点集合V中，以供后续在路径裁剪中使用。

在本申请实施例中，对于由已有的采样点和终点组成的路径进行裁剪的方式并不做限定，例如，通过启发式搜索算法对路径进行裁剪。

图3为本申请实施例提供的路径规划方法的实现流程示意图，如图3所示，该方法可以包括以下步骤301至步骤307：

步骤301，将待规划路径的起始点作为第一采样点，根据第一采样点所处位置的空旷区域的体积，确定第一采样点对应的随机概率。

在一些实施例中，可以通过执行如下步骤3011至步骤3013来实现步骤301：

步骤3011，确定第一采样点所属的子节点的中心位置与第一采样点所处位置的距离。

在一些实施例中，第一采样点所属的子节点的中心位置，即为在八叉树结构中划分出的叶子节点的中心位置。对于确定第一采样点所属的子节点的中心位置与第一采样点所处位置的距离dist的方式不做限定，例如，可以为确定两个位置之间的欧式距离。

步骤3012，根据第一采样点所处位置的空旷区域的体积和所述距离，确定第一采样点对应的调节因子。

在一些实施例中，在确定出第一采样点所属的子节点的中心位置与第一采样点所处位置的距离dist之后，可以通过判断距离dist与阈值δ之间的关系，确定出第一采样点对应的调节因子α。其中，在dist大于或等于阈值δ时，调节因子α为第一采样点所处位置的空旷区域的体积与dist的比值；在dist小于阈值δ时，调节因子α为第一采样点所处位置的空旷区域的体积与阈值δ的比值。

步骤3013，根据调节因子，确定第一采样点对应的随机概率。

这里，在确定出第一采样点对应的调节因子α后，随机概率P与调节因子α成正比，随机概率P的值在[0，1]范围内。

步骤302，在随机概率大于第一阈值的情况下，在第一采样点朝向待规划路径的终点的延伸方向上生成第一候选点。

这里，在随机概率小于第一阈值的情况下，则在三维栅格地图中，基于第一采样点，随机选择一个方向生成第一候选点。

其中，第一阈值的大小可以自由设定，例如，第一阈值设置为0.7。

步骤303，确定第一采样点和第一候选点组成的第一边的边距是否小于或等于第二阈值；如果是，执行步骤304；否则，执行步骤305。

为了防止随机生成的下一采样点位于之前已经探索过的位置，造成不必要的路径延伸，可以对上一采样点和下一采样点之间的边距进行限制，即，限制上一采样点和下一采样点之间的边距小于或等于第二阈值。

其中，第二阈值的大小可以自由设定，例如，第二阈值设置为0.7。

步骤304，将第一候选点作为第二采样点；

步骤305，在第一边上选取和第一采样点组成的第二边的边距等于第二阈值的第二候选点，将第二候选点作为第二采样点。

需要说明的是，在确定出候选点之后，并不能直接将候选点作为第二采样点存储在采样集合V中。这是因为，在生成候选点时，并不能够保证第一采样点和候选点之间不存在障碍物，如果第一采样点和候选点之间有较大概率不存在障碍物，说明电子设备能够通过该段路径，则可以将该候选点作为第二采样点进行存储；如果第一采样点和候选点之间有较大概率存在障碍物，则说明电子设备不能通过该段路径，应当摒弃掉该候选点，重新生成新的候选点，直至生成的候选点与第一采样点之间有较大概率不存在障碍物为止，将该候选点作为第二采样点。

因此，在一些实施例中，可以通过执行以下如下步骤3051至步骤3055来确定候选点是否为第二采样点：

步骤3051，对第一采样点和候选点组成的边进行插值处理，得到至少一个插值点。

在一些实施例中，可以按照最小分辨率对第一采样点和候选点组成的边进行插值处理，得到至少一个插值点Pi。

步骤3052，获取传感器测量的插值点的第一占据概率；

步骤3053，根据插值点所处位置的高度值，确定插值点的第二占据概率。

可以理解地，在无人机航迹规划场景中，插值点所处位置的高度值越大(即插值点越高)，碰撞到障碍物的可能性就越小。

步骤3054，对第一占据概率和第二占据概率进行融合处理，得到目标占据概率；其中，目标占据概率用于表征插值点所处位置存在障碍物的可能性大小。

在本申请实施例中，在确定插值点所处位置是否存在障碍物时，在传感器测量得到的第一占据概率的基础上，还引入了基于插值点的高度信息得到的第二占据概率，将高度信息与传感器测量数据进行融合，能够有效降低传感器测量错误引发的碰撞风险。

步骤3055，在各个不同的插值点的目标占据概率满足第三条件的情况下，将候选点作为第二采样点。

其中，满足第三条件为目标占据概率小于特征数值，例如，在一些实施例中，满足第三条件为目标占据概率小于0.5。在得到的每个插值点的目标占据概率均小于0.5时，确定该候选点为第二采样点，将其存储在采样集合V中。

步骤306，从第一采样点和第二采样点中选出与待规划路径的终点之间的距离满足第一条件的采样点作为参考点；

步骤307，根据参考点所处位置的空旷区域的体积，生成第三采样点，以及从已有的采样点中选出与所述终点之间的距离满足第一条件的采样点作为新的参考点，直至最终得到的新的参考点与所述终点的距离满足第二条件为止，对由已有的采样点和所述终点组成的路径进行裁剪，得到目标路径。

图4为本申请实施例提供的路径规划方法的实现流程示意图，如图4所示，该方法可以包括以下步骤401至步骤406：

步骤401，将待规划路径的起始点作为第一采样点，根据第一采样点所处位置的空旷区域的体积，生成第二采样点；

步骤402，根据第一采样点和第二采样点所处的位置，确定第二采样点的代价函数值。

其中，代价函数值用于表征第二采样点为组成目标路径中的采样点的概率。

可以理解地，在无人机航迹规划场景中，无人机在爬升时，需要克服重力做功，其耗电量/耗油量相较于平飞和下降会更多，因此，为节省无人机飞行功耗，可以在获取目标路径时，尽可能使无人机在目标路径中的路线为平飞或下降，以节省功耗。

在一些实施例中，可以通过计算采样点的代价函数值，来确定该采样点为组成目标路径中的采样点的概率，采样点的代价函数值越小，则该采样点为目标路径中的采样点的概率就越大。

在一些实施例中，可以通过执行如下步骤4021至步骤4023来实现步骤402：

步骤4021，确定第二采样点和第一采样点的高度差；

步骤4022，在高度差小于或等于第三阈值的情况下，确定高度差对应的权重为第一权重；在高度差大于第三阈值的情况下，确定高度差对应的权重为第二权重；其中，第二权重小于第一权重。

在一些实施例中，第三阈值为0。也即，当第二采样点的高度小于或等于第一采样点的高度(地理位置上呈现为路径平移或下降)时，确定对应的权重为第一权重；当第二采样点的高度大于第一采样点的高度(地理位置上呈现为路径爬升)时，确定对应的权重为第二权重。第二权重小于第一权重，例如，取第一权重为大于1的数值，如2；取第二权重为小于1的数值，如0.5。

步骤4023，根据高度差对应的权重，确定第二采样点的代价函数值。

步骤403，从第一采样点和第二采样点中选出与待规划路径的终点之间的距离满足第一条件的采样点作为参考点；

步骤404，根据参考点所处位置的空旷区域的体积，生成第三采样点；以及根据参考点和第三采样点所处的位置，确定第三采样点的代价函数值。

在确定第三采样点的代价函数值时，确定方式与步骤402中确定第二采样点的代价函数值的方式相同，在此不再赘述。

步骤405，从已有的采样点中选出与终点之间的距离满足第一条件的采样点作为新的参考点，直至最终得到的新的参考点与终点的距离满足第二条件为止；

步骤406，根据每一采样点的代价函数值，对由已有的采样点和终点组成的至少一条路径进行遍历，从中选取出目标路径。

其中，目标路径由代价函数值总值满足第四条件的至少一个采样点和终点组成，至少一个采样点中包括第一采样点。

在一些实施例中，满足条件为代价函数值的总值最小。

在利用RRT算法进行路径规划时，最终会得到至少一条能够将起始点和终点连接起来的路径，在从中选取目标路径时，可以基于每一个采样点的代价函数值，选择出一条代价函数值尽可能小(即路径中爬升的路段较少)的路径作为目标路径。如此，能够使得设备在飞行时尽量不做不必要的爬升运动，以节省耗电量和耗油量。

在一些实施例中，采用RRT算法进行的三维路径规划时，引入的随机概率P为固定值，其并不能最大限度节省计算机资源；且构造的代价函数为欧式距离，未考虑高度在无人机爬升和下降过程中区别，此与实际无人机耗电量/耗油量情形不符；另外其对三维空间中的占据概率求解未进行探索。

基于此，在本申请实施例中，设计了一套基于改进型RRT算法的无人机导航方法，该方法包括：首先利用倾斜摄影获取该区域的三维地图信息并以八叉树格式进行保存；然后利用改进RRT算法获取无人机起点至终点的最优路径，最后对该路径进优化。

在本申请实施例中，通过自适应选取RRT算法中的随机采样概率，能够充分解放计算资源；且令爬升和下降权重不一致，使无人机尽量不做不必要的爬升运动，从而能够节省耗电量/耗油量；另外，由二值贝叶斯滤波求解改为引入高度先验信息，利用贝叶斯概率信息公式可以获取先验信息与传感器测量数据融合后的占据概率，从而避免过度依赖传感器的精度。

本申请实施例提出了一种基于改进型RRT算法的无人机导航方法，如图5所示，该方法包括以下三个实施模块：

模块1，构建离线地图。

利用无人机对该区域进行倾斜摄影，并从摄影模型中提取出点云，再利用点云数据库(Point Cloud Library，PCL)生成作业环境的三维模型地图，然后利用现有Octomap库将其转换为八叉树格式。

模块2，改进型RRT算法。

快速扩展随机树算法(Rapidly-exploring random tree，RRT)是广泛应用在路径规划中的一种抽样规划算法。它通过抽样在已知的地图上建立无向图，进而通过搜索方法寻找相对最优的路径，它是一种概率完备算法：即只要路径存在，且规划的时间足够长，就一定能够确保找到一条路径解。

为解决无人机碰撞风险，在插入新生成的随机点时，需要判断其连接边E是否存在碰撞风险。在一些实施例中，是直接利用传感器探测概率信息，但当传感器数据有误时，此时则会造成错误的风险值计算。在本申请实施例中，创新性地引入高度先验信息，认为高度越高，该节点占据的概率越小，即碰撞到障碍物的风险越低。在本申请实施例中，利用贝叶斯概率信息公式可以获取先验信息与传感器测量数据融合后的占据概率，以降低传感器错误引发的风险。其风险(即目标占据概率)计算公式如下公式1所示：

r(n)＝value*p(n)*p_h(z_n) (公式1)；

其中，高度先验计算公式有很多变种，在本申请中以指数函数进行构造，如下公式2所示：

在上述公式2中，需要满足平滑条件

式中z_n为待检测点(即采样点)的高度，h₀、h₁为参数。value为风险值所对应的权重因子参数，p(n)为待检测点在八叉树中的占据概率(即第一占据概率)，由octomap serve库计算得到，p_h(z_n)为高度先验信息(即第二占据概率)。

由上述公式2可知，如果待检测点的高度小于h0，说明该待检测点所处位置较低，则高度先验信息较大，相应地，碰撞风险也较高；如果待检测点的高度大于h1，说明该待检测点所处位置较高，则高度先验信息较小，碰撞风险也较低。

无人机在实际飞行中，其进行爬升时，由于需要克服重力做功，其耗电量/耗油量相较于平飞、下降更多，为使无人机所耗能量更少，在进行RRT边的代价计算时，将向上运行距离、水平距离、向下距离权重设置不同，其向上＞水平＞向下，使尽量不让无人机向上飞行，从而尽可能降低无人机耗电/耗油。其代价函数计算如下公式3所示：

其中，(x_s,y_s,z_s)为RRT树中离采样点最近的点位置坐标，(x_n,y_n,z_n)为采样点坐标。取v₁大于等于1(即第一权重)，v₂小于1(即第二权重)，具体值由对应无人机耗能量进行调参。

可以理解地，起始点需要不断地向着终点进行生长，但由于存在障碍物，如果让树型一味朝着终点延伸，则可能会因为“撞墙”而失败。因此，在一些实施例中，RRT采取了一种随机采样方法：在每次选择生长方向时，有一定的概率会向着终点延伸，也有一定的概率会随机在地图内选择一个方向延伸一段距离，其距离的最大值为固定值。

该延伸概率显然会影响搜索效果。表现形式为，随机采样指向目标的概率越小，RRT树的分支也就越多，其计算量也就越大，反之则难以发生新的分支，但会导致不能找到路径。

但是设定指向目标的随机概率较小，会使很多场景下造成资源浪费。比如一种极端情景：无人机起点至终点之间并无障碍物，且处于同一平面，此时若向终点延伸的概率为1，则能够很快获取其最优路径。基于此，本申请在RRT算法中引入自适应调节向终点方向的延伸概率，能够根据当前空闲区域大小，动态调节延伸概率，加速RRT算法计算速度。

其中，构造的动态调节因子α的计算公式如下公式4所示，延伸概率p的计算公式如下公式5所示：

p＝f(α) (公式5)；

上述公式中size为距离目标最近的采样点所属八叉树叶子节点的尺寸，即表达当前空闲体积大小(即空旷区域的体积)，dist为八叉树叶子节点的中心位置与该采样点位置之间的三维欧式距离，δ为阈值参数，k为其权重系数，延伸概率为动态调节因子α的函数，需要满足和α成正比，且最大值不超过1，在此取sigma函数。公式4表明当该采样点所属位置空闲区域较大，即当距离目标最近的采样点位置居于较大空闲区域中心附近时，

此时α较大，则RRT算法以较大概率指向目标。

如图6所示，为本申请实施例通过的改进型RRT算法的实现流程示意图，包括以下步骤601至步骤613：

步骤601，获取地图M，起点Ps，终点Pe；

步骤602，建立采样点集V、边集E，并把起点Ps加入V，以构建RRT树图G＝(V，E)；

步骤603，在采样点集V中寻求与终点Pe最近(即第一条件)的点Vn，构建动态调节因子α，获取延伸概率p(即随机概率)；

步骤604，在地图M中按延伸概率p生成随机采样点Pc(即候选点)；

步骤605，在采样点集V中寻求与终点Pe最近的点Vm，连接点Vm和点Pc，记作边E0，获取其长度d；

步骤606，确定边E0的长度d是否大于所设定的最大边长dmax；如果是，执行步骤607；否则，执行步骤608；

步骤607，在E0中获取边长为dmax的点，替代之前的采样点Pc，并获取新的边E0(即Vm指向新的Pc)；

步骤608，计算边E0的代价函数C；

步骤609，在边E0中按最小分辨率进行插值，其插值点记作Pi，获取每一个插值点Pi对应的改进型占据概率(即目标占据概率)；

步骤610，确定是否所有插值点占据概率均小于阈值(即满足第三条件)，如果是，执行步骤611，否则，返回执行步骤604；

步骤611，将Pc加入V，E0加入E，更新树图G；

步骤612，确定采样点Pc与终点Pe之间的距离是否小于阈值(即满足第二条件)；如果是，执行步骤613；否则，返回执行步骤603；

步骤613，得到RRT树图G。

模块3，路径裁剪。

RRT搜索出的路径往往是曲折的(毕竟节点是随机生成的)，故找到一条可行的从起点到终点的路径之后就需要考虑优化路径，化曲为直。这个过程中从叶子节点开始，不断寻找能否无障碍地直接连接到先辈节点，最终得到一条更适合无人机飞行的航线。

在一些实施例中，在计算占据概率时并未引入高度先验信息，本申请采用引入高度先验信息，认为高度超过某个阈值后，随着高度升高，无人机碰到障碍物的概率越小的方案，达到有效规避传感器错误时带来的影响。

在一些实施例中，RRT算法并未引入调节因子α来自适应调节RRT的随机采样概率，以加速搜寻路径速度。本申请中采用利用与目标最近采样点位置所处空闲大小来自适应计算调节因子α的方案，达到动态调节采样概率，使其RRT算法能够更快速的搜寻目标。

在本申请实施例中，(1)引入高度先验信息，认为高度超过某个阈值后，随着高度升高，无人机碰到障碍物的概率越小；然后占据概率求解利用贝叶斯概率公式可以获取高度先验信息与传感器测量数据融合后的占据概率，提高规避传感器数据错误的风险。(2)利用与目标最近采样点位置所处空闲大小来自适应计算调节因子，从而动态调节采样概率，使得在RRT算法中，能够更快速的搜寻目标。(3)构造的代价函数中考虑高度在无人机爬升和下降过程中区别，此与实际无人机耗电量/耗油量情形相符，使无人机尽量不做不必要的爬升运动，从而能够节省耗电量/耗油量。

应当注意，尽管在附图中以特定顺序描述了本申请中方法的各个步骤，但是，这并非要求或者暗示必须按照该特定顺序来执行这些步骤，或是必须执行全部所示的步骤才能实现期望的结果。附加的或备选的，可以省略某些步骤，将多个步骤合并为一个步骤执行，以及/或者将一个步骤分解为多个步骤执行等；或者，将不同实施例中步骤组合为新的技术方案。

基于前述的实施例，本申请实施例提供一种路径规划装置，该装置包括所包括的各模块、以及各模块所包括的各单元，可以通过处理器来实现；当然也可通过具体的逻辑电路实现；在实施的过程中，处理器可以为中央处理器(CPU)、微处理器(MPU)、数字信号处理器(DSP)或现场可编程门阵列(FPGA)等。

图7为本申请实施例路径规划装置的结构示意图，如图7所示，所述装置700包括生成模块701、选择模块702和裁剪模块703，其中：生成模块701，用于将待规划路径的起始点作为第一采样点，根据所述第一采样点在地图中所处位置的空旷区域的体积，确定第一采样点对应的随机概率；基于第一采样点对应的随机概率，生成第二采样点；选择模块702，用于从所述第一采样点和所述第二采样点中选出与所述待规划路径的终点之间的距离满足第一条件的采样点作为参考点；所述生成模块701，还用于根据所述参考点所处位置的空旷区域的体积，生成第三采样点，以及所述选择模块702，还用于从已有的采样点中选出与所述终点之间的距离满足所述第一条件的采样点作为新的参考点，直至最终得到的新的参考点与所述终点的距离满足第二条件为止；裁剪模块703，用于对由已有的采样点和所述终点组成的路径进行裁剪，得到目标路径。

在一些实施例中，所述装置700还包括确定模块，所述确定模块，用于确定所述第一采样点所属的子节点的中心位置与所述第一采样点所处位置的距离；根据所述第一采样点所处位置的空旷区域的体积和所述距离，确定所述第一采样点对应的调节因子；根据所述调节因子，确定所述第一采样点对应的随机概率。

在一些实施例中，所述确定模块，用于根据所述第一采样点所处位置的空旷区域的体积，确定所述第一采样点对应的随机概率；其中，所述随机概率用于表征所述第一采样点的延伸方向朝向所述待规划路径的终点的可能性大小；生成模块701，用于基于所述第一采样点对应的随机概率，生成第二采样点。

在一些实施例中，生成模块701，用于在所述随机概率大于第一阈值的情况下，在所述第一采样点朝向所述待规划路径的终点的延伸方向上生成第一候选点；所述确定模块，还用于确定所述第一采样点和所述第一候选点组成的第一边的边距是否小于或等于第二阈值；所述确定模块，还用于在所述第一边的边距小于或等于所述第二阈值的情况下，将所述第一候选点作为所述第二采样点；选择模块702，还用于在所述第一边的边距大于所述第二阈值的情况下，在所述第一边上选取和所述第一采样点组成的第二边的边距等于所述第二阈值的第二候选点，将所述第二候选点作为所述第二采样点。

在一些实施例中，所述装置700还包括插值模块，所述插值模块，用于对所述第一采样点和所述候选点组成的边进行插值处理，得到至少一个插值点；所述确定模块，还用于确定所述插值点的目标占据概率；其中，所述目标占据概率用于表征所述插值点所处位置存在障碍物的可能性大小；在各个不同的所述插值点的目标占据概率满足第三条件的情况下，将所述候选点作为所述第二采样点。

在一些实施例中，所述装置700还包括获取模块和融合模块，所述获取模块，用于获取传感器测量的所述插值点的第一占据概率；所述确定模块，用于根据所述插值点所处位置的高度值，确定所述插值点的第二占据概率；所述融合模块，用于对所述第一占据概率和所述第二占据概率进行融合处理，得到所述目标占据概率。

在一些实施例中，所述装置700还包括遍历模块，确定模块，用于根据所述第一采样点和所述第二采样点所处的位置，确定所述第二采样点的代价函数值；其中，所述代价函数值用于表征所述第二采样点为组成所述目标路径中的采样点的概率；所述遍历模块，用于根据每一所述采样点的代价函数值，对由已有的采样点和所述终点组成的至少一条路径进行遍历，从中选取出所述目标路径；其中，所述目标路径由代价函数值总值满足第四条件的至少一个采样点和所述终点组成，所述至少一个采样点包括所述第一采样点。

在一些实施例中，确定模块，用于确定所述第二采样点和所述第一采样点的高度差；在所述高度差小于或等于第三阈值的情况下，确定所述高度差对应的权重为第一权重；在所述高度差大于所述第三阈值的情况下，确定所述高度差对应的权重为第二权重；其中，所述第二权重小于所述第一权重；根据所述高度差对应的权重，确定所述第二采样点的代价函数值。

以上装置实施例的描述，与上述方法实施例的描述是类似的，具有同方法实施例相似的有益效果。对于本申请装置实施例中未披露的技术细节，请参照本申请方法实施例的描述而理解。

需要说明的是，本申请实施例中图7所示的路径规划装置对模块的划分是示意性的，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式。另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。也可以采用软件和硬件结合的形式实现。

需要说明的是，本申请实施例中，如果以软件功能模块的形式实现上述的方法，并作为独立的产品销售或使用时，也可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请实施例的技术方案本质上或者说对相关技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得电子设备执行本申请各个实施例所述方法的全部或部分。而前述的存储介质包括：U盘、移动硬盘、只读存储器(Read Only Memory，ROM)、磁碟或者光盘等各种可以存储程序代码的介质。这样，本申请实施例不限制于任何特定的硬件和软件结合。

本申请实施例提供一种电子设备，图8为本申请实施例的电子设备的硬件实体示意图，如图8所示，所述电子设备800包括存储器801和处理器802，所述存储器801存储有可在处理器802上运行的计算机程序，所述处理器802执行所述程序时实现上述实施例中提供的方法中的步骤。

需要说明的是，存储器801配置为存储由处理器802可执行的指令和应用，还可以缓存在处理器802以及电子设备800中各模块待处理或已经处理的数据(例如，图像数据、音频数据、语音通信数据和视频通信数据)，可以通过闪存(FLASH)或随机访问存储器(RandomAccess Memory，RAM)实现。

本申请实施例提供一种计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现上述实施例中提供的方法中的步骤。

本申请实施例提供了一种包含指令的计算机程序产品，当其在计算机上运行时，使得计算机执行上述方法实施例提供的方法中的步骤。

这里需要指出的是：以上存储介质和设备实施例的描述，与上述方法实施例的描述是类似的，具有同方法实施例相似的有益效果。对于本申请存储介质、存储介质和设备实施例中未披露的技术细节，请参照本申请方法实施例的描述而理解。

应理解，说明书通篇中提到的“一个实施例”或“一实施例”或“一些实施例”意味着与实施例有关的特定特征、结构或特性包括在本申请的至少一个实施例中。因此，在整个说明书各处出现的“在一个实施例中”或“在一实施例中”或“在一些实施例中”未必一定指相同的实施例。此外，这些特定的特征、结构或特性可以任意适合的方式结合在一个或多个实施例中。应理解，在本申请的各种实施例中，上述各过程的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本申请实施例的实施过程构成任何限定。上述本申请实施例序号仅仅为了描述，不代表实施例的优劣。上文对各个实施例的描述倾向于强调各个实施例之间的不同之处，其相同或相似之处可以互相参考，为了简洁，本文不再赘述。

本文中术语“和/或”，仅仅是一种描述关联对象的关联关系，表示可以存在三种关系，例如对象A和/或对象B，可以表示：单独存在对象A，同时存在对象A和对象B，单独存在对象B这三种情况。

需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者装置不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括该要素的过程、方法、物品或者设备中还存在另外的相同要素。

在本申请所提供的几个实施例中，应该理解到，所揭露的设备和方法，可以通过其它的方式实现。以上所描述的实施例仅仅是示意性的，例如，所述模块的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，如：多个模块或组件可以结合，或可以集成到另一个系统，或一些特征可以忽略，或不执行。另外，所显示或讨论的各组成部分相互之间的耦合、或直接耦合、或通信连接可以是通过一些接口，设备或模块的间接耦合或通信连接，可以是电性的、机械的或其它形式的。

上述作为分离部件说明的模块可以是、或也可以不是物理上分开的，作为模块显示的部件可以是、或也可以不是物理模块；既可以位于一个地方，也可以分布到多个网络单元上；可以根据实际的需要选择其中的部分或全部模块来实现本实施例方案的目的。

另外，在本申请各实施例中的各功能模块可以全部集成在一个处理单元中，也可以是各模块分别单独作为一个单元，也可以两个或两个以上模块集成在一个单元中；上述集成的模块既可以采用硬件的形式实现，也可以采用硬件加软件功能单元的形式实现。

本领域普通技术人员可以理解：实现上述方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成，前述的程序可以存储于计算机可读取存储介质中，该程序在执行时，执行包括上述方法实施例的步骤；而前述的存储介质包括：移动存储设备、只读存储器(Read Only Memory，ROM)、磁碟或者光盘等各种可以存储程序代码的介质。

或者，本申请上述集成的单元如果以软件功能模块的形式实现并作为独立的产品销售或使用时，也可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请实施例的技术方案本质上或者说对相关技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得电子设备执行本申请各个实施例所述方法的全部或部分。而前述的存储介质包括：移动存储设备、ROM、磁碟或者光盘等各种可以存储程序代码的介质。

本申请所提供的几个方法实施例中所揭露的方法，在不冲突的情况下可以任意组合，得到新的方法实施例。

本申请所提供的几个产品实施例中所揭露的特征，在不冲突的情况下可以任意组合，得到新的产品实施例。

本申请所提供的几个方法或设备实施例中所揭露的特征，在不冲突的情况下可以任意组合，得到新的方法实施例或设备实施例。

以上所述，仅为本申请的实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应以所述权利要求的保护范围为准。

Claims (10)

1.一种路径规划方法，其特征在于，所述方法包括：

将待规划路径的起始点作为第一采样点，根据所述第一采样点在地图中所处位置的空旷区域的体积，确定所述第一采样点对应的随机概率；其中，所述随机概率用于表征所述第一采样点的延伸方向朝向所述待规划路径的终点的可能性大小；

基于所述第一采样点对应的随机概率，生成第二采样点；

从所述第一采样点和所述第二采样点中选出与所述待规划路径的终点之间的距离满足第一条件的采样点作为参考点；

根据所述参考点所处位置的空旷区域的体积，生成第三采样点，以及从已有的采样点中选出与所述终点之间的距离满足所述第一条件的采样点作为新的参考点，直至最终得到的新的参考点与所述终点的距离满足第二条件为止，对由已有的采样点和所述终点组成的路径进行裁剪，得到目标路径。

2.根据权利要求1所述的方法，其特征在于，所述根据所述第一采样点所处位置的空旷区域的体积，确定所述第一采样点对应的随机概率，包括：

确定所述第一采样点所属的子节点的中心位置与所述第一采样点所处位置的距离；

根据所述第一采样点所处位置的空旷区域的体积和所述距离，确定所述第一采样点对应的调节因子；

根据所述调节因子，确定所述第一采样点对应的随机概率。

3.根据权利要求1所述的方法，其特征在于，所述基于所述第一采样点对应的随机概率，生成第二采样点，包括：

在所述随机概率大于第一阈值的情况下，在所述第一采样点朝向所述待规划路径的终点的延伸方向上生成第一候选点；

确定所述第一采样点和所述第一候选点组成的第一边的边距是否小于或等于第二阈值；

在所述第一边的边距小于或等于所述第二阈值的情况下，将所述第一候选点作为所述第二采样点；

在所述第一边的边距大于所述第二阈值的情况下，在所述第一边上选取和所述第一采样点组成的第二边的边距等于所述第二阈值的第二候选点，将所述第二候选点作为所述第二采样点。

4.根据权利要求3所述的方法，其特征在于，候选点包括第一候选点或第二候选点；将所述候选点作为所述第二采样点，包括：

对所述第一采样点和所述候选点组成的边进行插值处理，得到至少一个插值点；

确定所述插值点的目标占据概率；其中，所述目标占据概率用于表征所述插值点所处位置存在障碍物的可能性大小；

在各个不同的所述插值点的目标占据概率满足第三条件的情况下，将所述候选点作为所述第二采样点。

5.根据权利要求4所述的方法，其特征在于，所述确定所述插值点的目标占据概率，包括：

获取传感器测量的所述插值点的第一占据概率；

根据所述插值点所处位置的高度值，确定所述插值点的第二占据概率；

对所述第一占据概率和所述第二占据概率进行融合处理，得到所述目标占据概率。

6.根据权利要求1所述的方法，其特征在于，所述方法还包括：

根据所述第一采样点和所述第二采样点所处的位置，确定所述第二采样点的代价函数值；其中，所述代价函数值用于表征所述第二采样点为组成所述目标路径中的采样点的概率；

相应地，所述对由已有的采样点和所述终点组成的路径进行裁剪，得到目标路径，包括：

根据每一所述采样点的代价函数值，对由已有的采样点和所述终点组成的至少一条路径进行遍历，从中选取出所述目标路径；其中，所述目标路径由代价函数值总值满足第四条件的至少一个采样点和所述终点组成，所述至少一个采样点包括所述第一采样点。

7.根据权利要求6所述的方法，其特征在于，所述根据所述第一采样点和所述第二采样点所处的位置，确定所述第二采样点的代价函数值，包括：

确定所述第二采样点和所述第一采样点的高度差；

在所述高度差小于或等于第三阈值的情况下，确定所述高度差对应的权重为第一权重；

在所述高度差大于所述第三阈值的情况下，确定所述高度差对应的权重为第二权重；其中，所述第二权重小于所述第一权重；

根据所述高度差对应的权重，确定所述第二采样点的代价函数值。

8.一种路径规划装置，其特征在于，包括：

生成模块，用于将待规划路径的起始点作为第一采样点，根据所述第一采样点在地图中所处位置的空旷区域的体积，确定第一采样点对应的随机概率；基于第一采样点对应的随机概率，生成第二采样点；

选择模块，用于从所述第一采样点和所述第二采样点中选出与所述待规划路径的终点之间的距离满足第一条件的采样点作为参考点；

所述生成模块，还用于根据所述参考点所处位置的空旷区域的体积，生成第三采样点，以及所述选择模块，还用于从已有的采样点中选出与所述终点之间的距离满足所述第一条件的采样点作为新的参考点，直至最终得到的新的参考点与所述终点的距离满足第二条件为止；

裁剪模块，用于对由已有的采样点和所述终点组成的路径进行裁剪，得到目标路径。

9.一种电子设备，包括存储器和处理器，所述存储器存储有可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现权利要求1至7任一项所述的方法。

10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该计算机程序被处理器执行时实现如权利要求1至7任一项所述的方法。

Images