CN115950439A - 双向rrt路径规划方法、装置、电子设备及存储介质 - Google Patents

Abstract

本申请属于路径规划技术领域，公开了一种双向RRT路径规划方法、装置、电子设备及存储介质，所述方法包括：根据障碍物的轮廓，把地图划分为障碍物区和多个空白区；分别以起始点和目标点为两棵路径树的首个节点，根据所述障碍物区的轮廓确定采样点所在的空白区，并在所确定的所述空白区内采样，以交替地扩展两棵路径树的节点，直到两棵所述路径树相交，以此得到初始路径；对所述初始路径进行平滑处理得到最终的路径；从而能够提高路径规划效率。

Description

双向RRT路径规划方法、装置、电子设备及存储介质

技术领域

本申请涉及路径规划技术领域，具体而言，涉及一种双向RRT路径规划方法、装置、电子设备及存储介质。

背景技术

路径规划的目的是在充满障碍物的环境中，寻找一条从起点到目标点的无碰撞的最优运动路径，使得移动设备（如移动机器人）可以安全地通过，且花费时间最少。

RRT算法是一种常用的路径规划算法，旨在生成一颗路径树，在地图中随机生成一个采样点，在路径树上找到一个与采样点距离最近的树节点，用连接线连接采样点与树节点，在该连接线上以设定步长获得新的树节点，以此方式进行路径树的扩展，其优势在于无需建模，空间覆盖率高，具有强大的搜索能力，可以对未知区域尽可能的探索，使其得到广泛的应用。但是RRT算法生成采样点的方式随机，缺乏路径导向性，导致出现盲目扩展的问题，进而导致路径规划效率低，无法快速收敛。

发明内容

本申请的目的在于提供一种双向RRT路径规划方法、装置、电子设备及存储介质，能够提高路径规划效率。

第一方面，本申请提供了一种双向RRT路径规划方法，包括步骤：

A1.根据障碍物的轮廓，把地图划分为障碍物区和多个空白区；

A2.分别以起始点和目标点为两棵路径树的首个节点，根据所述障碍物区的轮廓确定采样点所在的空白区，并在所确定的所述空白区内采样，以交替地扩展两棵路径树的节点，直到两棵所述路径树相交，以此得到初始路径；

A3.对所述初始路径进行平滑处理得到最终的路径。

利用障碍物轮廓信息先确定采样空白区（即采样点所在的空白区），再在选定的采样空白区中进行采样以扩展节点，该扩展过程具有路径导向性，避免了盲目扩展的问题，从而能够快速收敛，提高路径规划效率。

优选地，步骤A1包括：

以所述障碍物所占据的区域为所述障碍物区；

以经过所述起始点和所述目标点的直线为X轴，并以一垂直于所述X轴的直线为Y轴，建立平面坐标系；

用各所述障碍物区的外切线和各所述障碍物区的轮廓线，把除所述障碍物区以外的区域划分为多个单连通区域，将每个所述单连通区域作为一个所述空白区；所述障碍物区的外切线穿过所述障碍物区的X坐标极值点且平行于Y轴。

从而各空白区与各障碍物区之间相互独立，没有相互包围关系，有利于加强空白区的导向作用，以便于更快地规定得到避障路径。

优选地，步骤A2包括：

A201.分别以所述起始点和所述目标点为两棵路径树的首个节点；

A202.交替地以两棵所述路径树中的一颗为第一路径树，另一颗为第二路径树，以执行以下步骤，直到两棵所述路径树相交：

B1.以所述第一路径树最新得到的节点为第一节点，以所述第二路径树上离所述第一节点最近的节点为第二节点，获取所述第一节点和所述第二节点的连接线作为第一线段；

B2.获取每个所述障碍物区的Y坐标极大点和Y坐标极小点之间的连接线作为所述障碍物区的第二线段；

B3.从与所述第一线段相交的所述第二线段中，获取与所述第一线段的交点距离所述第二节点最近的所述第二线段的两个端点，作为两个候选端点；

B4.从两个所述候选端点中选取一个作为被选端点，使所述第一节点到所述被选端点的距离和所述被选端点到所述第二节点的距离之和最小；

B5.确定所述第一节点和所述被选端点之间的一个所述空白区为采样空白区；

B6.在所述采样空白区中确定一个扩展点作为所述第一路径树的新节点；

A203.依次连接两棵所述路径树的节点得到所述初始路径。

通过上述方式寻找被选端点，以提供一个从第一节点到被选端点的引导方向，用以引导采样空白区的选择，有利于得到长度较短的避障路径。

优选地，步骤B5包括：

B501.获取所述第一节点和所述被选端点的连接线所经过的所述空白区的集合作为第一空白区集合；

B502.获取与所述第一节点所在的所述空白区邻接的所述空白区的集合作为第二空白区集合；

B503.获取所述第一空白区集合和所述第二空白区集合的交集作为所述采样空白区。

实际上从第一节点到被选端点的移动路线是从第一节点朝第二节点移动过程中的一段最理想的路径，通过在该第一节点和所述被选端点的连接线所经过的空白区中选取采样空白区，有利于使第一节点与拓展得到的节点之间的路径接近该理想路径，从而有利于最终得到较短的避障路径。

优选地，步骤B6包括：

B601.在所述采样空白区中随机生成多个采样点；

B602.选取距离所述第一节点和所述被选端点的连接线最近的所述采样点为参考点，并获取所述第一节点沿指向所述参考点的方向移动预设步长后得到点作为候选扩展点；

B603.对所述候选扩展点进行碰撞检测；

B604.若碰撞检测合格，则以所述候选扩展点为有效扩展点，并把所述有效扩展点作为所述第一路径树的新节点；若碰撞检测不合格，则重复步骤B601-步骤B604。

通过上述方式一方面可以保证得到的路径能够可靠避障，另一方面由于以距离第一节点和被选端点的连接线最近的采样点为参考点，可使第一节点到扩展点之间的路径更加贴近理想路径，从而更有利于最终得到较短的避障路径。

优选地，两棵所述路径树相交是指两棵所述路径树上最接近的两个节点之间的距离小于预设的距离阈值。

优选地，步骤A3包括：

对所述初始路径进行B样条平滑处理得到最终的路径。

第二方面，本申请提供了一种双向RRT路径规划装置，包括：

划分模块，用于根据障碍物的轮廓，把地图划分为障碍物区和多个空白区；

规划模块，用于分别以起始点和目标点为两棵路径树的首个节点，根据所述障碍物区的轮廓确定采样点所在的空白区，并在所确定的所述空白区内采样，以交替地扩展两棵路径树的节点，直到两棵所述路径树相交，以此得到初始路径；

平滑处理模块，用于对所述初始路径进行平滑处理得到最终的路径。

利用障碍物轮廓信息先确定采样空白区（即采样点所在的空白区），再在选定的采样空白区中进行采样以扩展节点，该扩展过程具有路径导向性，避免了盲目扩展的问题，从而能够快速收敛，提高路径规划效率。

第三方面，本申请提供了一种电子设备，包括处理器和存储器，所述存储器存储有所述处理器可执行的计算机程序，所述处理器执行所述计算机程序时，运行如前文所述双向RRT路径规划方法中的步骤。

第四方面，一种存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时运行如前文所述双向RRT路径规划方法中的步骤。

有益效果：本申请提供的双向RRT路径规划方法、装置、电子设备及存储介质，利用障碍物轮廓信息先确定采样空白区，再在选定的采样空白区中进行采样以扩展节点，该扩展过程具有路径导向性，避免了盲目扩展的问题，从而能够快速收敛，提高路径规划效率。

附图说明

图1为本申请实施例提供的双向RRT路径规划方法的流程图。

图2为本申请实施例提供的双向RRT路径规划装置的结构示意图。

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

图4为示例性的空白区划分的原理图。

图5为示例性的选取采样空白区的原理图。

具体实施方式

下面将结合本申请实施例中附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。

应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。同时，在本申请的描述中，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

请参照图1，图1是本申请一些实施例中的一种双向RRT路径规划方法，包括步骤：

A1.根据障碍物的轮廓，把地图划分为障碍物区和多个空白区；

A2.分别以起始点和目标点为两棵路径树的首个节点，根据障碍物区的轮廓确定采样点所在的空白区，并在所确定的空白区内采样，以交替地扩展两棵路径树的节点，直到两棵路径树相交，以此得到初始路径；

A3.对初始路径进行平滑处理得到最终的路径。

利用障碍物轮廓信息先确定采样空白区（即采样点所在的空白区），再在选定的采样空白区中进行采样以扩展节点，该扩展过程具有路径导向性，避免了盲目扩展的问题，从而能够快速收敛，提高路径规划效率。

在本实施例中，步骤A1包括：

A101.以障碍物所占据的区域（即障碍物轮廓线所包围的区域）为障碍物区；

A102.以经过起始点和目标点的直线为X轴，并以一垂直于X轴的直线为Y轴，建立平面坐标系；

A103.用各障碍物区的外切线和各障碍物区的轮廓线，把除障碍物区以外的区域划分为多个单连通区域（即内部没有孔的连通区域），将每个单连通区域作为一个空白区；障碍物区的外切线穿过障碍物区的X坐标极值点且平行于Y轴。

从而各空白区与各障碍物区之间相互独立，没有相互包围关系，有利于加强空白区的导向作用，以便于更快地规定得到避障路径。

其中，障碍物区的X坐标极值点包括X坐标极大点和X坐标极小点，其中，X坐标极大点是指障碍物区的点中X坐标值最大的点，X坐标极小点是指障碍物区的点中X坐标值最小的点，X坐标极值点一般为障碍物区的轮廓线上的点。

例如图4中，黑色区域为障碍物区，每个障碍物区两侧均有一条平行于Y轴的虚线，该虚线即为外切线，从而，该外切线和各障碍物区的轮廓线把地图区域划分出7个空白区分别为： S1、S2、S3、S4、S5、S6和S7。

具体地，步骤A2包括：

A201.分别以起始点和目标点为两棵路径树的首个节点；

A202.交替地以两棵路径树中的一颗为第一路径树，另一颗为第二路径树，以执行以下步骤，直到两棵路径树相交：

B1.以第一路径树最新得到的节点为第一节点，以第二路径树上离第一节点最近的节点为第二节点，获取第一节点和第二节点的连接线作为第一线段；

B2.获取每个障碍物区的Y坐标极大点和Y坐标极小点之间的连接线作为障碍物区的第二线段；

B3.从与第一线段相交的第二线段中，获取与第一线段的交点距离第二节点最近的第二线段的两个端点，作为两个候选端点；

B4.从两个候选端点中选取一个作为被选端点，使第一节点到被选端点的距离和被选端点到第二节点的距离之和最小；

B5.确定第一节点和被选端点之间的一个空白区为采样空白区；

B6.在采样空白区中确定一个扩展点作为第一路径树的新节点；

A203.依次连接两棵路径树的节点得到初始路径。

通过上述方式寻找被选端点，以提供一个从第一节点到被选端点的引导方向，用以引导采样空白区的选择，有利于得到长度较短的避障路径。

其中，起始点和目标点均为预设点，分别为要规划的路径的起点和终点。

为了便于说明，把以起始点为首个节点的路径树称为左路径树，把以目标点为首个节点的路径树称为右路径树。在步骤A202中，例如，先以起始点为第一节点，此时的第二节点为目标点，第一路径树为左路径树，第二路径树为右路径树，第一次循环中新扩展得到的新节点为节点P1，该节点P1为左路径树的最新节点；在第二次循环中，第一节点为目标点，第二节点为左路径树中离目标点最近的节点，从而新扩展得到的新节点为节点P2，该节点P2为右路径树的最新节点；在第三次循环中，第一节点为节点P1，第二节点为右路径树上离节点P1最近的节点，从而新扩展得到的新节点为节点P3，该节点P3为左路径树的最新节点；在第四次循环中，第一节点为节点P2，第二节点为左路径树上离节点P2最近的节点，从而新扩展得到的新节点为节点P4，该节点P4为右路径树的最新节点；依此循环，直到两棵路径树相交。

其中，障碍物区的Y坐标极大点是指障碍物区的点中Y坐标值最大的点，Y坐标极小点是指障碍物区的点中Y坐标值最小的点，该Y坐标极大点和Y坐标极小点一般为障碍物区的轮廓线上的点。在步骤B2中，若一个障碍物区的Y坐标极大点有多个，则可随机选取其中一个作为有效的Y坐标极大点，也可选取其中一个作为有效的Y坐标极大点使得第一节点到该有效的Y坐标极大点的距离和该有效的Y坐标极大点到第二节点的距离之和最小（例如，假设有三个Y坐标极大点分别为点X1、点X2和点X3，起始点为点P0，目标点为点Pz，若线段P0X1和线段X1Pz的总长为L1，线段P0X2和线段X2Pz的总长为L2，线段P0X3和线段X3Pz的总长为L3，且

，则以点X1为有效的Y坐标极大点），从而更有利于规划出更短的路径；同理，若一个障碍物区的Y坐标极小点有多个，则可随机选取其中一个作为有效的Y坐标极小点，也可选取其中一个作为有效的Y坐标极小点使得第一节点到该有效的Y坐标极小点的距离和该有效的Y坐标极小点到第二节点的距离之和最小，从而更有利于规划出更短的路径。

例如，图5中，显示了第一次循环在步骤B3中确定的两个候选端点，其中，仅菱形的障碍物区的第二线段（其上角点M1与下角点M2之间的连线）与第一线段（起始点P0和目标点Pz之间的连线）相交，即与第一线段的交点距离第二节点最近的第二线段也为该菱形的障碍物区的第二线段，从而两个候选端点为该上角点M1和下角点M2。

在图5中，线段P0M1和线段M1Pz的总长比线段P0M2和线段M2Pz的总长小，因此，步骤B4中选取上角点M1为作为被选端点。

在一些优选实施方式中，步骤B5包括：

B501.获取第一节点和被选端点的连接线所经过的空白区的集合作为第一空白区集合；

B502.获取与第一节点所在的空白区邻接的空白区的集合作为第二空白区集合；

B503.获取第一空白区集合和第二空白区集合的交集作为采样空白区。

实际上从第一节点到被选端点的移动路线是从第一节点朝第二节点移动过程中的一段最理想的路径，通过在该第一节点和被选端点的连接线所经过的空白区中选取采样空白区，有利于使第一节点与拓展得到的节点之间的路径接近该理想路径，从而有利于最终得到较短的避障路径。

继续以图5为例，第一节点和被选端点的连接线（即线段P0M1）所经过的空白区包括S1、S3、S4和S5，因此第一空白区集合

为

，第一节点所在的空白区为S1，与S1的邻接的空白区包括S2和S3，因此第二空白区集合

为

，第一空白区集合和第二空白区集合的交集

为

，从而，采样空白区为S3。需要说明的是，交集

中的空白区有且仅有一个，并非图5的情况为特例。

在一些优选实施方式中，步骤B6包括：

B601.在采样空白区中随机生成多个采样点；

B602.选取距离第一节点和被选端点的连接线最近的采样点为参考点，并获取第一节点沿指向参考点的方向移动预设步长后得到点作为候选扩展点；

B603.对候选扩展点进行碰撞检测；

B604.若碰撞检测合格，则以候选扩展点为有效扩展点，并把有效扩展点作为第一路径树的新节点；若碰撞检测不合格，则重复步骤B601-步骤B604。

通过上述方式一方面可以保证得到的路径能够可靠避障，另一方面由于以距离第一节点和被选端点的连接线最近的采样点为参考点，可使第一节点到扩展点之间的路径更加贴近理想路径，从而更有利于最终得到较短的避障路径。

步骤B601中，可随机生成预设数量的采样点，其中，预设数量可以根据实际需要设置，例如为5个，但不限于此。

其中，步骤B602中的预设步长可根据实际需要设置。

继续以图5为例，步骤B602中，假设离第一节点和被选端点的连接线P0M1最近的采样点为点M3，则以点M3为参考点，第一节点沿 P0→M3的方向移动预设步长后得到的点为点P1，则该点P1为候选扩展点。

在一些实施例中，步骤B603包括：把以移动设备的碰撞半径（根据移动设备的尺寸预先设置）为半径的圆形区域的中心设置在候选扩展点处，并检测该圆形区域是否与至少一个障碍物区有交点，若是，则判定碰撞检测不合格，否则，判定碰撞检测合格。

在另一些实施例中，步骤B603包括：把以移动设备的轮廓朝外扩展预设碰撞距离（可根据实际需要设置）后得到的碰撞区域的几何中心设置在候选扩展点处，并检测该碰撞区域是否与至少一个障碍物区有交点，若是，则判定碰撞检测不合格，否则，判定碰撞检测合格。

在实际应用中，可能没有一条第二线段与第一线段相交，表示第一线段不与所有障碍物区相交，此时，可直接以第二节点作为被选端点。从而，在一些实施方式中，步骤B2之后和步骤B3之前，还包括：

B7.判断第一线段是否与至少一条第二线段相交，若是，则转至步骤B3（即依次执行步骤B1、B2、B7、B3、B4、B5、B6），若否，则以第二节点作为被选端点，并转至步骤B5（即依次执行步骤B1、B2、B7、B5、B6）。

优选地，两棵路径树相交是指两棵路径树上最接近的两个节点之间的距离小于预设的距离阈值。即，只要两棵路径树上接近的两个节点之间的距离小于预设的距离阈值，就判定两棵路径树相交，从而停止进行节点的扩展。

其中，步骤A3中，可用现有的任意一种平滑处理方法对初始路径进行平滑处理，此处不对其进行限定。例如，在一些实施方式中，步骤A3包括：

对初始路径进行B样条平滑处理得到最终的路径。

由上可知，该双向RRT路径规划方法，根据障碍物的轮廓，把地图划分为障碍物区和多个空白区；分别以起始点和目标点为两棵路径树的首个节点，根据障碍物区的轮廓确定采样点所在的空白区，并在所确定的空白区内采样，以交替地扩展两棵路径树的节点，直到两棵路径树相交，以此得到初始路径；对初始路径进行平滑处理得到最终的路径；从而能够提高路径规划效率。具体地，具有以下优点：

1.自动化程度高，规划过程无需人为干预；

2.在选定的空白区内进行采样，解决了传统RRT算法随机采样（全图随机采样）而导致的盲目扩展问题，提高了采样效率，加快规划速度；

3.利用障碍物轮廓信息选取采样空白区，计算简单，运行效率高，提高了方法的实时性；

4.针对不同场景，可通过调节预设步长的大小以加快搜索效率，应用范围广；

5.对得到的初始路径进行B样条平滑处理，得到一条光滑的全局路径，更有利于移动设备运动。

参考图2，本申请提供了一种双向RRT路径规划装置，包括：

划分模块1，用于根据障碍物的轮廓，把地图划分为障碍物区和多个空白区；

规划模块2，用于分别以起始点和目标点为两棵路径树的首个节点，根据障碍物区的轮廓确定采样点所在的空白区，并在所确定的空白区内采样，以交替地扩展两棵路径树的节点，直到两棵路径树相交，以此得到初始路径；

平滑处理模块3，用于对初始路径进行平滑处理得到最终的路径。

利用障碍物轮廓信息先确定采样空白区（即采样点所在的空白区），再在选定的采样空白区中进行采样以扩展节点，该扩展过程具有路径导向性，避免了盲目扩展的问题，从而能够快速收敛，提高路径规划效率。

在本实施例中，划分模块1在根据障碍物的轮廓，把地图划分为障碍物区和多个空白区的时候，执行：

以障碍物所占据的区域（即障碍物轮廓线所包围的区域）为障碍物区；

以经过起始点和目标点的直线为X轴，并以一垂直于X轴的直线为Y轴，建立平面坐标系；

用各障碍物区的外切线和各障碍物区的轮廓线，把除障碍物区以外的区域划分为多个单连通区域（即内部没有孔的连通区域），将每个单连通区域作为一个空白区；障碍物区的外切线穿过障碍物区的X坐标极值点且平行于Y轴。

从而各空白区与各障碍物区之间相互独立，没有相互包围关系，有利于加强空白区的导向作用，以便于更快地规定得到避障路径。

其中，障碍物区的X坐标极值点包括X坐标极大点和X坐标极小点，其中，X坐标极大点是指障碍物区的点中X坐标值最大的点，X坐标极小点是指障碍物区的点中X坐标值最小的点，X坐标极值点一般为障碍物区的轮廓线上的点。

例如图4中，黑色区域为障碍物区，每个障碍物区两侧均有一条平行于Y轴的虚线，该虚线即为外切线，从而，该外切线和各障碍物区的轮廓线把地图区域划分出7个空白区分别为： S1、S2、S3、S4、S5、S6和S7。

具体地，规划模块2在分别以起始点和目标点为两棵路径树的首个节点，根据障碍物区的轮廓确定采样点所在的空白区，并在所确定的空白区内采样，以交替地扩展两棵路径树的节点，直到两棵路径树相交，以此得到初始路径的时候，执行：

分别以起始点和目标点为两棵路径树的首个节点；

交替地以两棵路径树中的一颗为第一路径树，另一颗为第二路径树，以执行以下步骤，直到两棵路径树相交：

B1.以第一路径树最新得到的节点为第一节点，以第二路径树上离第一节点最近的节点为第二节点，获取第一节点和第二节点的连接线作为第一线段；

B2.获取每个障碍物区的Y坐标极大点和Y坐标极小点之间的连接线作为障碍物区的第二线段；

B3.从与第一线段相交的第二线段中，获取与第一线段的交点距离第二节点最近的第二线段的两个端点，作为两个候选端点；

B4.从两个候选端点中选取一个作为被选端点，使第一节点到被选端点的距离和被选端点到第二节点的距离之和最小；

B5.确定第一节点和被选端点之间的一个空白区为采样空白区；

B6.在采样空白区中确定一个扩展点作为第一路径树的新节点；

依次连接两棵路径树的节点得到初始路径。

通过上述方式寻找被选端点，以提供一个从第一节点到被选端点的引导方向，用以引导采样空白区的选择，有利于得到长度较短的避障路径。

其中，起始点和目标点均为预设点，分别为要规划的路径的起点和终点。

为了便于说明，把以起始点为首个节点的路径树称为左路径树，把以目标点为首个节点的路径树称为右路径树。例如，先以起始点为第一节点，此时的第二节点为目标点，第一路径树为左路径树，第二路径树为右路径树，第一次循环中新扩展得到的新节点为节点P1，该节点P1为左路径树的最新节点；在第二次循环中，第一节点为目标点，第二节点为左路径树中离目标点最近的节点，从而新扩展得到的新节点为节点P2，该节点P2为右路径树的最新节点；在第三次循环中，第一节点为节点P1，第二节点为右路径树上离节点P1最近的节点，从而新扩展得到的新节点为节点P3，该节点P3为左路径树的最新节点；在第四次循环中，第一节点为节点P2，第二节点为左路径树上离节点P2最近的节点，从而新扩展得到的新节点为节点P4，该节点P4为右路径树的最新节点；依此循环，直到两棵路径树相交。

其中，障碍物区的Y坐标极大点是指障碍物区的点中Y坐标值最大的点，Y坐标极小点是指障碍物区的点中Y坐标值最小的点，该Y坐标极大点和Y坐标极小点一般为障碍物区的轮廓线上的点。在步骤B2中，若一个障碍物区的Y坐标极大点有多个，则可随机选取其中一个作为有效的Y坐标极大点，也可选取其中一个作为有效的Y坐标极大点使得第一节点到该有效的Y坐标极大点的距离和该有效的Y坐标极大点到第二节点的距离之和最小（例如，假设有三个Y坐标极大点分别为点X1、点X2和点X3，起始点为点P0，目标点为点Pz，若线段P0X1和线段X1Pz的总长为L1，线段P0X2和线段X2Pz的总长为L2，线段P0X3和线段X3Pz的总长为L3，且

，则以点X1为有效的Y坐标极大点），从而更有利于规划出更短的路径；同理，若一个障碍物区的Y坐标极小点有多个，则可随机选取其中一个作为有效的Y坐标极小点，也可选取其中一个作为有效的Y坐标极小点使得第一节点到该有效的Y坐标极小点的距离和该有效的Y坐标极小点到第二节点的距离之和最小，从而更有利于规划出更短的路径。

例如，图5中，显示了第一次循环在步骤B3中确定的两个候选端点，其中，仅菱形的障碍物区的第二线段（其上角点M1与下角点M2之间的连线）与第一线段（起始点P0和目标点Pz之间的连线）相交，即与第一线段的交点距离第二节点最近的第二线段也为该菱形的障碍物区的第二线段，从而两个候选端点为该上角点M1和下角点M2。

在图5中，线段P0M1和线段M1Pz的总长比线段P0M2和线段M2Pz的总长小，因此，步骤B4中选取上角点M1为作为被选端点。

在一些优选实施方式中，步骤B5包括：

B501.获取第一节点和被选端点的连接线所经过的空白区的集合作为第一空白区集合；

B502.获取与第一节点所在的空白区邻接的空白区的集合作为第二空白区集合；

B503.获取第一空白区集合和第二空白区集合的交集作为采样空白区。

实际上从第一节点到被选端点的移动路线是从第一节点朝第二节点移动过程中的一段最理想的路径，通过在该第一节点和被选端点的连接线所经过的空白区中选取采样空白区，有利于使第一节点与拓展得到的节点之间的路径接近该理想路径，从而有利于最终得到较短的避障路径。

继续以图5为例，第一节点和被选端点的连接线（即线段P0M1）所经过的空白区包括S1、S3、S4和S5，因此第一空白区集合

为

，第一节点所在的空白区为S1，与S1的邻接的空白区包括S2和S3，因此第二空白区集合

为

，第一空白区集合和第二空白区集合的交集

为

，从而，采样空白区为S3。需要说明的是，交集

中的空白区有且仅有一个，并非图5的情况为特例。

在一些优选实施方式中，步骤B6包括：

B601.在采样空白区中随机生成多个采样点；

B602.选取距离第一节点和被选端点的连接线最近的采样点为参考点，并获取第一节点沿指向参考点的方向移动预设步长后得到点作为候选扩展点；

B603.对候选扩展点进行碰撞检测；

B604.若碰撞检测合格，则以候选扩展点为有效扩展点，并把有效扩展点作为第一路径树的新节点；若碰撞检测不合格，则重复步骤B601-步骤B604。

通过上述方式一方面可以保证得到的路径能够可靠避障，另一方面由于以距离第一节点和被选端点的连接线最近的采样点为参考点，可使第一节点到扩展点之间的路径更加贴近理想路径，从而更有利于最终得到较短的避障路径。

步骤B601中，可随机生成预设数量的采样点，其中，预设数量可以根据实际需要设置，例如为5个，但不限于此。

其中，步骤B602中的预设步长可根据实际需要设置。

继续以图5为例，步骤B602中，假设离第一节点和被选端点的连接线P0M1最近的采样点为点M3，则以点M3为参考点，第一节点沿 P0→M3的方向移动预设步长后得到的点为点P1，则该点P1为候选扩展点。

在一些实施例中，步骤B603包括：把以移动设备的碰撞半径（根据移动设备的尺寸预先设置）为半径的圆形区域的中心设置在候选扩展点处，并检测该圆形区域是否与至少一个障碍物区有交点，若是，则判定碰撞检测不合格，否则，判定碰撞检测合格。

在另一些实施例中，步骤B603包括：把以移动设备的轮廓朝外扩展预设碰撞距离（可根据实际需要设置）后得到的碰撞区域的几何中心设置在候选扩展点处，并检测该碰撞区域是否与至少一个障碍物区有交点，若是，则判定碰撞检测不合格，否则，判定碰撞检测合格。

在实际应用中，可能没有一条第二线段与第一线段相交，表示第一线段不与所有障碍物区相交，此时，可直接以第二节点作为被选端点。从而，在一些实施方式中，步骤B2之后和步骤B3之前，还包括：

B7.判断第一线段是否与至少一条第二线段相交，若是，则转至步骤B3（即依次执行步骤B1、B2、B7、B3、B4、B5、B6），若否，则以第二节点作为被选端点，并转至步骤B5（即依次执行步骤B1、B2、B7、B5、B6）。

优选地，两棵路径树相交是指两棵路径树上最接近的两个节点之间的距离小于预设的距离阈值。即，只要两棵路径树上接近的两个节点之间的距离小于预设的距离阈值，就判定两棵路径树相交，从而停止进行节点的扩展。

其中，可用现有的任意一种平滑处理方法对初始路径进行平滑处理，此处不对其进行限定。例如，在一些实施方式中，平滑处理模块3在对初始路径进行平滑处理得到最终的路径的时候，执行：

对初始路径进行B样条平滑处理得到最终的路径。

由上可知，该双向RRT路径规划装置，根据障碍物的轮廓，把地图划分为障碍物区和多个空白区；分别以起始点和目标点为两棵路径树的首个节点，根据障碍物区的轮廓确定采样点所在的空白区，并在所确定的空白区内采样，以交替地扩展两棵路径树的节点，直到两棵路径树相交，以此得到初始路径；对初始路径进行平滑处理得到最终的路径；从而能够提高路径规划效率。具体地，具有以下优点：

1.自动化程度高，规划过程无需人为干预；

2.在选定的空白区内进行采样，解决了传统RRT算法随机采样（全图随机采样）而导致的盲目扩展问题，提高了采样效率，加快规划速度；

3.利用障碍物轮廓信息选取采样空白区，计算简单，运行效率高，提高了方法的实时性；

4.针对不同场景，可通过调节预设步长的大小以加快搜索效率，应用范围广；

5.对得到的初始路径进行B样条平滑处理，得到一条光滑的全局路径，更有利于移动设备运动。

请参照图3，图3为本申请实施例提供的一种电子设备的结构示意图，本申请提供一种电子设备，包括：处理器301和存储器302，处理器301和存储器302通过通信总线303和/或其他形式的连接机构（未标出）互连并相互通讯，存储器302存储有处理器301可执行的计算机程序，当电子设备运行时，处理器301执行该计算机程序，以执行上述实施例的任一可选的实现方式中的双向RRT路径规划方法，以实现以下功能：根据障碍物的轮廓，把地图划分为障碍物区和多个空白区；分别以起始点和目标点为两棵路径树的首个节点，根据障碍物区的轮廓确定采样点所在的空白区，并在所确定的空白区内采样，以交替地扩展两棵路径树的节点，直到两棵路径树相交，以此得到初始路径。

本申请实施例提供一种存储介质，其上存储有计算机程序，计算机程序被处理器执行时，执行上述实施例的任一可选的实现方式中的双向RRT路径规划方法，以实现以下功能：根据障碍物的轮廓，把地图划分为障碍物区和多个空白区；分别以起始点和目标点为两棵路径树的首个节点，根据障碍物区的轮廓确定采样点所在的空白区，并在所确定的空白区内采样，以交替地扩展两棵路径树的节点，直到两棵路径树相交，以此得到初始路径。其中，存储介质可以由任何类型的易失性或非易失性存储设备或者它们的组合实现，如静态随机存取存储器（Static Random Access Memory, 简称SRAM），电可擦除可编程只读存储器（Electrically Erasable Programmable Read-Only Memory, 简称EEPROM），可擦除可编程只读存储器（Erasable Programmable Read Only Memory, 简称EPROM），可编程只读存储器（Programmable Red-Only Memory, 简称PROM），只读存储器（Read-Only Memory,简称ROM），磁存储器，快闪存储器，磁盘或光盘。

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

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

再者，在本申请各个实施例中的各功能模块可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或两个以上模块集成形成一个独立的部分。

在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。

以上所述仅为本申请的实施例而已，并不用于限制本申请的保护范围，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。

Claims (10)

1.一种双向RRT路径规划方法，其特征在于，包括步骤：

A1.根据障碍物的轮廓，把地图划分为障碍物区和多个空白区；

A2.分别以起始点和目标点为两棵路径树的首个节点，根据所述障碍物区的轮廓确定采样点所在的空白区，并在所确定的所述空白区内采样，以交替地扩展两棵路径树的节点，直到两棵所述路径树相交，以此得到初始路径；

A3.对所述初始路径进行平滑处理得到最终的路径。

2.根据权利要求1所述的双向RRT路径规划方法，其特征在于，步骤A1包括：

以所述障碍物所占据的区域为所述障碍物区；

以经过所述起始点和所述目标点的直线为X轴，并以一垂直于所述X轴的直线为Y轴，建立平面坐标系；

用各所述障碍物区的外切线和各所述障碍物区的轮廓线，把除所述障碍物区以外的区域划分为多个单连通区域，将每个所述单连通区域作为一个所述空白区；所述障碍物区的外切线穿过所述障碍物区的X坐标极值点且平行于Y轴。

3.根据权利要求2所述的双向RRT路径规划方法，其特征在于，步骤A2包括：

A201.分别以所述起始点和所述目标点为两棵路径树的首个节点；

A202.交替地以两棵所述路径树中的一颗为第一路径树，另一颗为第二路径树，以执行以下步骤，直到两棵所述路径树相交：

B1.以所述第一路径树最新得到的节点为第一节点，以所述第二路径树上离所述第一节点最近的节点为第二节点，获取所述第一节点和所述第二节点的连接线作为第一线段；

B2.获取每个所述障碍物区的Y坐标极大点和Y坐标极小点之间的连接线作为所述障碍物区的第二线段；

B3.从与所述第一线段相交的所述第二线段中，获取与所述第一线段的交点距离所述第二节点最近的所述第二线段的两个端点，作为两个候选端点；

B4.从两个所述候选端点中选取一个作为被选端点，使所述第一节点到所述被选端点的距离和所述被选端点到所述第二节点的距离之和最小；

B5.确定所述第一节点和所述被选端点之间的一个所述空白区为采样空白区；

B6.在所述采样空白区中确定一个扩展点作为所述第一路径树的新节点；

A203.依次连接两棵所述路径树的节点得到所述初始路径。

4.根据权利要求3所述的双向RRT路径规划方法，其特征在于，步骤B5包括：

B501.获取所述第一节点和所述被选端点的连接线所经过的所述空白区的集合作为第一空白区集合；

B502.获取与所述第一节点所在的所述空白区邻接的所述空白区的集合作为第二空白区集合；

B503.获取所述第一空白区集合和所述第二空白区集合的交集作为所述采样空白区。

5.根据权利要求3所述的双向RRT路径规划方法，其特征在于，步骤B6包括：

B601.在所述采样空白区中随机生成多个采样点；

B602.选取距离所述第一节点和所述被选端点的连接线最近的所述采样点为参考点，并获取所述第一节点沿指向所述参考点的方向移动预设步长后得到点作为候选扩展点；

B603.对所述候选扩展点进行碰撞检测；

B604.若碰撞检测合格，则以所述候选扩展点为有效扩展点，并把所述有效扩展点作为所述第一路径树的新节点；若碰撞检测不合格，则重复步骤B601-步骤B604。

6.根据权利要求2所述的双向RRT路径规划方法，其特征在于，两棵所述路径树相交是指两棵所述路径树上最接近的两个节点之间的距离小于预设的距离阈值。

7.根据权利要求1所述的双向RRT路径规划方法，其特征在于，步骤A3包括：

对所述初始路径进行B样条平滑处理得到最终的路径。

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

划分模块，用于根据障碍物的轮廓，把地图划分为障碍物区和多个空白区；

规划模块，用于分别以起始点和目标点为两棵路径树的首个节点，根据所述障碍物区的轮廓确定采样点所在的空白区，并在所确定的所述空白区内采样，以交替地扩展两棵路径树的节点，直到两棵所述路径树相交，以此得到初始路径；

平滑处理模块，用于对所述初始路径进行平滑处理得到最终的路径。

9.一种电子设备，其特征在于，包括处理器和存储器，所述存储器存储有所述处理器可执行的计算机程序，所述处理器执行所述计算机程序时，运行如权利要求1-7任一项所述双向RRT路径规划方法中的步骤。

10.一种存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时运行如权利要求1-7任一项所述双向RRT路径规划方法中的步骤。

Images