CN115060281B - 一种基于voronoi图的全局路径引导点生成规划方法 - Google Patents

Abstract

本发明公开了一种基于voronoi图的全局路径引导点生成规划方法。为了克服现有技术全局路径生成操作复杂，计算量大，容易受到动态障碍物或者事先不存在的静态障碍物的干扰问题；本发明包括以下步骤：S1：构建工作区域环境地图，初步处理后形成栅格地图；S2：提取voronoi点，剔除障碍物在同侧的相邻voronoi点；S3：提取引导点；S4：导出引导点信息；S5：移动机器人加载引导点的配置文件，接收到目标点坐标；根据自身位置和目标点位置，按最近距离原则找到起点和终点的引导点序号，生成路径。用引导点来代替路径信息，不用输出整条路径，计算速度快。跟踪引导点能让机器人不用按路径严格走，给局部规划的避障留了余地，松耦合，运动更安全。

Description

一种基于voronoi图的全局路径引导点生成规划方法

技术领域

本发明涉及一种移动机器人导航领域，尤其涉及一种基于voronoi图的全局路径引导点生成规划方法。

背景技术

移动机器人导航中常将规划分为全局路径规划和局部路径规划。全局规划器规划是从目标的到终点的一条路径，局部规划器循着全局路径，根据实际的场景做局部的规划，如避让行人、绕开障碍物等。

一般全局规划的技术方案是，由slam建好的栅格地图给全局规划器，全局规划使用搜索算法（比如A*，动态A*，概率路图法，跳点法等）再结合一些约束条件，找到一条符合条件的全局路径，一般希望找的路径光滑，且耗时最短。例如，一种在中国专利文献上公开的“一种移动机器人新型骨架提取的Voronoi路径规划算法”，其公告号CN111323037B，包括如下步骤：步骤一、首先对移动机器人已知先验信息的三色栅格地图进行二值化处理，在对其进行腐蚀、膨胀处理；步骤二、对处理后的二值地图经进行骨架提取，得到Voronoi图；步骤三、用原始的三色地图进行机器人定位，在骨架地图上进行全局路径搜索，对得到的全局路径进行改进的三次样条平滑处理，得到优化后的全局路径。根据本发明提出的算法提取出来的Voronoi骨架没有大量毛边，整体的路径规划更加高效。

但是上述方法生成的全局路径，实际应用操作复杂。首先找到这样的一条全局路径计算量比较大，且实际场景中易出现动态障碍物或者事先不存在的静态障碍物，以至于跟踪这条全局路径进行循迹就没有那么必要。只需要一些关键的全局路径点，包含道路的特征信息如道路宽度、道路弯曲程度等，就可以替换全局路径点。

在实际导航过程中，机器人跟踪这些引导点，由局部规划器实现避障与跟踪引导点的程度，实现全局规划器与局部规划器的松耦合，即机器人因为避障偏离引导点也没关系后面再引导回来即可。

发明内容

本发明主要解决现有技术全局路径生成操作复杂，计算量大，容易受到动态障碍物或者事先不存在的静态障碍物的干扰问题；提供一种基于voronoi图的全局路径引导点生成规划方法，对激光扫描获得的区域进行预处理，用处理后的地图生成voronoi图，提取voronoi中的引导点并进行处理生成全局规划的路径引导点，利用路径引导点实现移动机器人在区域地图导航时的全局路径规划。

本发明的上述技术问题主要是通过下述技术方案得以解决的：

一种基于voronoi图的全局路径引导点生成规划方法，包括以下步骤：

S1：使用激光雷达结合SLAM构建工作区域环境地图，初步处理后形成栅格地图；

S2：根据voronoi生成规则，提取voronoi点，剔除障碍物在同侧的相邻voronoi点；

S3：遍历voronoi点，选取交点；连接相邻交点，根据交点间连线与障碍物关系提取引导点；

S4：导出引导点信息，编辑引导点信息；

S5：移动机器人加载引导点的配置文件，接收到目标点坐标；根据自身位置和目标点位置，按最近距离原则找到起点和终点的引导点序号，生成路径。

本方案增加了同侧障碍物判断，去除了一些杂点的影响或是分叉路径，使得在栅格地图上整个voronoi图很清晰，与在连续性空间中voronoi图相一致。提取的voronoi图的引导点，很好地表示了地图的拓扑信息，用这些引导点进行规划高效快速。计算快速，不用输出整条路径，用引导点来代替路径信息；跟踪引导点能让机器人近可能走中间，不用按路径严格走，给局部规划的避障留了余地，是松耦合，运动更安全。

作为优选，若一个点到距离最近的两个障碍物的距离相等，则该点为voronoi点。

作为优选，剔除障碍物在同侧的voronoi点的过程为：

选择任意节点P与相邻节点PN；

令节点P到节点P最近障碍物的连线为P1，节点P到相邻节点PN最近障碍物的连线为P2；相邻节点PN到相邻节点PN最近障碍物的连线为PN1，相邻节点PN到节点P最近障碍物的连线为PN2；

计算P1与P2之间的夹角或计算PN1与PN2之间的夹角；若夹角小于额定阈值θ，则对进行voronoi点剔除。

判断两个点的障碍物是否在同侧，避免pn与p的障碍物在同侧，导致有多个周边点属于voronoi。

作为优选，计算节点P到节点P最近障碍物的距离D_P1；计算节点P到相邻节点PN最近障碍物的距离D_P2；计算相邻节点PN到相邻节点PN最近障碍物的距离D_PN1；计算相邻节点PN到节点P最近障碍物的距离D_PN2；

若D_P2≤D_PN2且D_P2≥D_P1，则节点P属于voronoi点；

若D_PN2≤D_P2且D_PN2≥D_PN1，则相邻节点PN属于voronoi点。

增加了同侧障碍物判断，去除了一些杂点的影响或是分叉路径，使得在栅格地图上整个voronoi图很清晰，与在连续性空间中voronoi图相一致。

作为优选，遍历所有voronoi点，选择存在三个及以上相邻节点的voronoi点为交点；

从一交点出发，向相邻的交点搜索，遍历交点间所有voronoi点；

以机器人实际宽度为线段宽度，连接起始交点与当前voronoi点，若线段与障碍物相交，则判断发生碰撞，选取前一voronoi点为插入点；

提取交点与插入点作为引导点。

用引导点来代替路径信息，不用输出整条路径，计算速度快。

作为优选，若相邻的两个引导点之间的距离小于预设阈值栅格，则保留其中一个引导点。

最后得到的引导点都处于道路中央，且该位置附近有明显路况变化。跟踪引导点能让机器人近可能走中间，不用按路径严格走，给局部规划的避障留了余地，是松耦合，运动更安全。

作为优选，所述的导出的引导点信息包括引导点与相邻两引导点的夹角、引导点之间的朝向信息和最近障碍物的坐标。

引导点包含丰富道路信息，可以根据道路属性设置通行方向，便于机器人导航决策。

本发明的有益效果是：

1. 用引导点来代替路径信息，不用输出整条路径，计算速度快。

2. 引导点包含丰富道路信息，可以根据道路属性设置通行方向，便于机器人导航决策。

3. 跟踪引导点能让机器人尽可能走中间，不用按路径严格走，给局部规划的避障留了余地，是松耦合，运动更安全。

4. 增加了同侧障碍物判断，去除了一些杂点的影响或是分叉路径，使得在栅格地图上整个voronoi图很清晰，与在连续性空间中voronoi图相一致。

附图说明

图1是本发明的全局路径引导点生成规划方法流程图。

图2是本发明的初步处理后的环境地图。

图3是本发明栅格地图提取voronoi点的示意图。

图4是本发明的voronoi点可视化示意图。

图5是本发明引导点信息的描述示意图。

图6是发明的基于引导点的全局规划的路径示意图。

图5中1~7为引导点的序号。

具体实施方式

下面通过实施例，并结合附图，对本发明的技术方案作进一步具体的说明。

实施例：

本实施例的一种基于voronoi图的全局路径引导点生成规划方法，如图1所示，包括以下步骤：

S1：使用激光雷达结合SLAM构建工作区域环境地图，初步处理后形成栅格地图。

如图2所示，使用激光雷达结合SLAM（同时定位与建图）技术构建机器人需要工作区域的环境地图，对地图进行初步处理，人为标记行驶道路、障碍物等，限制机器活动区域。

SLAM建图得到的是粗糙的环境地图，有些地方是柱子，有些地方是道路，但是建图可能仅体现了柱子的一部分，或是激光仅扫到了道路的一个轮廓，有些地方是残缺的。

对于机器人导航来说，安全是最重要的，有必要人为根据地图绘制障碍物、道路边界线等特征。在本实施例中，初步处理是人为的标识一些物体。

将初步处理后的环境地图栅格化，形成栅格地图。

S2：根据voronoi生成规则，提取voronoi点，剔除障碍物在同侧的相邻voronoi点。

在连续空间一个点属于voronoi那么它到两个障碍物的距离相等，在栅格地图中，则变成了离散空间，需要一些判断条件辅助。

voronoi生成规则为，若一个点到距离最近的两个障碍物的距离相等，则该点为voronoi点。

令每个点存储它最近障碍物的点坐标，考察任意点当P和P的邻居节点PN，判断这两个点的障碍物是否在同侧，避免PN与P的障碍物在同侧，导致有多个周边点属于voronoi。

剔除障碍物在同侧的voronoi点的过程为：

选择任意节点P与相邻节点PN；

令节点P到节点P最近障碍物的连线为P1，节点P到相邻节点PN最近障碍物的连线为P2；相邻节点PN到相邻节点PN最近障碍物的连线为PN1，相邻节点PN到节点P最近障碍物的连线为PN2。

使用余弦定理，计算P1与P2之间的夹角或计算PN1与PN2之间的夹角；若夹角小于额定的夹角阈值θ，则对这种情况进行voronoi点剔除。

计算节点P到节点P最近障碍物的距离D_P1；计算节点P到相邻节点PN最近障碍物的距离D_P2；计算相邻节点PN到相邻节点PN最近障碍物的距离D_PN1；计算相邻节点PN到节点P最近障碍物的距离D_PN2。

若D_P2≤D_PN2且D_P2≥D_P1，则节点P属于voronoi点；

若D_PN2≤D_P2且D_PN2≥D_PN1，则相邻节点PN属于voronoi点。

增加了同侧障碍物判断，去除了一些杂点的影响或是分叉路径，使得在栅格地图上整个voronoi图很清晰，与在连续性空间中voronoi图相一致。

例如，图3中的格子p（十字线格子），它到最近障碍物（黑色实心格子）距离是1.41格。p上方邻居格子pn_1（反斜线格子）到最近障碍物的距离是1格，p下方的格子pn_2（斜线格子）到最近障碍物的距离是2格。

p到p最近障碍物与p到pn_1最近障碍物的夹角是45°，若设夹角阈值θ为90°，小于阈值判断为同侧，剔除pn_1。而p到p最近障碍物与p到pn_2最近障碍物的夹角是145°，大于阈值判读为异侧，保留pn_2。

计算p到pn_2最近障碍物的距离是3格，pn_2到p最近障碍物的距离也是2格，同时满足p到pn_2最近障碍物的距离大于等于p到自身最近障碍物的距离1.41格，所以p属于voronoi。

处理后，将属于voronoi的点显示出来，如图 4所示，黑色栅格表示不可通行的区域，星形表示属于voronoi栅格，形成类似骨架的线条，反映了道路的拓扑形态。星型栅格组成的连线的特点在于，在直线道路，星型栅格的邻居栅格有两个，在道路有弯曲处，存在不同的交叉，导致其邻居点的个数存在两个以上。

S3：遍历voronoi点，选取交点；连接相邻交点，根据交点间连线与障碍物关系提取引导点。

遍历所有voronoi点，选择存在三个及以上相邻节点的voronoi点为交点，在图3中以圆形栅格表示。

从一交点出发，向相邻的交点搜索，遍历交点间所有voronoi点。

以机器人实际宽度为线段宽度，连接起始交点与当前voronoi点，若线段与障碍物相交，则判断发生碰撞，选取前一voronoi点为插入点。

为提取引导点，考虑移动机器人的实际宽度，从一端的交点出发，向另一端的交点搜索，遍历该段线段中所有的点，连接起始端的交点和当前遍历的点，连线宽度为移动机器人的宽度，若当前遍历点与起始端交点的连线与障碍物发生碰撞，则取发生碰撞前的一个点为插入点，在图4中以三角型表示 。

提取交点与插入点作为引导点。以此保证任意具有连接关系的点，它们的连线上都没有障碍物，插点后的连线如图4中的线段所示，圆形交点和三角形插点共同组成引导点。

用引导点来代替路径信息，不用输出整条路径，计算速度快。

若相邻的两个引导点之间的距离小于预设阈值栅格，则保留其中一个引导点。对过近的关键进行剔除，判断依据为若两个引导点间隔小于一定阈值栅格，如5个栅格，只保留其中一个。最后得到的引导点都处于道路中央，且该位置附近有明显路况变化。

最后得到的引导点都处于道路中央，且该位置附近有明显路况变化。跟踪引导点能让机器人近可能走中间，不用按路径严格走，给局部规划的避障留了余地，是松耦合，运动更安全。

S4：导出引导点信息，编辑引导点信息。

导出的引导点信息包括引导点与相邻两引导点的夹角、引导点之间的朝向信息和最近障碍物的坐标。

引导点包含丰富道路信息，可以根据道路属性设置通行方向，便于机器人导航决策。

计算引导点与其周边两个引导点夹角，用来表示道路的弯曲程度；计算点与点之间朝向信息，用向量形式表示，用来表示道路的通行方向；计算每个点周边最近障碍物的坐标，用来表示道路的通行宽度。

将这些信息都导出为文件形式，作为导航的配置文件。如图5示例所示，数字1-7表示引导点的序号，引导点1包含的信息包括向量v_1-7、 v_1-2，夹角theta_2-1-7，到最近障碍物的距离L_1。

其中，向量v_1-7、 v_1-2分别为从引导点1指向引导点7和从引导点1指向引导点2的向量，表示道路的方向信息，即从引导点1可以到引导点7，从引导点1可以到引导点2；夹角theta_2-1-7表示从引导点2经过引导点1到引导点7道路的弯曲程度；L_1表示引导点1到道路边缘最近的距离，以代表道路宽度。

引导点以反映道路的信息，用以引导机器人，在使用时记为引导点。

根据人为的需要对第四步导出的引导点进行编辑，如删减点、增加点、修改朝向信息等，为用户可编辑的文件，完成关于引导点的构建。

引导点具有连接关系，包含原始地图的关键信息，是图的一种表达方式。在实际移动机器人导航过程中，使用引导点进行规划可以快速规划出一组点串。

S5：移动机器人加载引导点的配置文件，接收到目标点坐标；根据自身位置和目标点位置，按最近距离原则找到起点和终点的引导点序号，生成路径。

移动机器人加载引导点的配置文件，接收到目标点坐标。根据自身位置，和目标点位置，按最近距离原则找到起点和终点的引导点序号。

根据起始引导点、终止引导点和相邻两点之间的距离，找到距离最近的引导点序列，使用方法不限于A*、跳点法、概率路图法等搜索算法。根据找到的引导点序列，并对距离较远的两点之间按3m间隔进行插点，生成路径点。机器人沿着路径方向，选择路径点进行跟踪，直到到达目标点。

如图6 所示，机器人处于人工标记处理后的环境地图，根据目标点和自身位置，找到最邻近的起始引导点1和终止引导点4。根据引导点1和引导点4生成距离最短的引导点序列1-2-3-4。对于引导点序列中间隔较远的点，按3m距离插点，生成一串从移动机器人附近到目标点的一组路径点序列。导航过程中，机器人只需逐点跟踪引导点直到终点。在跟踪过程中，机器人根据引导点存储的信息，如道路宽度、道路弯曲程度，在道路比较空旷的场景运行速度快一点，如引导点到道路边缘距离L大于2m，机器人按最大速度不超过2m/s运动，L小于2m，机器人按最大速度不超过1.5m/s运动；在拐弯曲折道路场景运行速度慢一点，如引导点theta角小于90度，限制机器人速度上限为原来的一半，theta角大于90度按正常速度运动。

本方案增加了同侧障碍物判断，去除了一些杂点的影响或是分叉路径，使得在栅格地图上整个voronoi图很清晰，与在连续性空间中voronoi图相一致。提取的voronoi图的引导点，很好地表示了地图的拓扑信息，用这些引导点进行规划高效快速。计算快速，不用输出整条路径，用引导点来代替路径信息；跟踪引导点能让机器人近可能走中间，不用按路径严格走，给局部规划的避障留了余地，是松耦合，运动更安全。

应理解，实施例仅用于说明本发明而不用于限制本发明的范围。此外应理解，在阅读了本发明讲授的内容之后，本领域技术人员可以对本发明作各种改动或修改，这些等价形式同样落于本申请所附权利要求书所限定的范围。

Claims (5)

1.一种基于voronoi图的全局路径引导点生成规划方法，其特征在于，包括以下步骤：

S1：使用激光雷达结合SLAM构建工作区域环境地图，初步处理后形成栅格地图；

S2：根据voronoi生成规则，提取voronoi点，剔除障碍物在同侧的相邻voronoi点；

S3：遍历voronoi点，选取交点；连接相邻交点，根据交点间连线与障碍物关系提取引导点；

S4：导出引导点信息，编辑引导点信息；

S5：移动机器人加载引导点的配置文件，接收到目标点坐标；根据自身位置和目标点位置，按最近距离原则找到起点和终点的引导点序号，生成路径；

若一个点到距离最近的两个障碍物的距离相等，则该点为voronoi点；

剔除障碍物在同侧的voronoi点的过程为：

选择任意节点P与相邻节点PN；

令节点P到节点P最近障碍物的连线为P1，节点P到相邻节点PN最近障碍物的连线为P2；相邻节点PN到相邻节点PN最近障碍物的连线为PN1，相邻节点PN到节点P最近障碍物的连线为PN2；

计算P1与P2之间的夹角或计算PN1与PN2之间的夹角；若夹角小于额定的夹角阈值θ，则进行voronoi点剔除。

2.根据权利要求1所述的一种基于voronoi图的全局路径引导点生成规划方法，其特征在于，计算节点P到节点P最近障碍物的距离D_P1；计算节点P到相邻节点PN最近障碍物的距离D_P2；计算相邻节点PN到相邻节点PN最近障碍物的距离D_PN1；计算相邻节点PN到节点P最近障碍物的距离D_PN2；

若D_P2≤D_PN2且D_P2≥D_P1，则节点P属于voronoi点；

若D_PN2≤D_P2且D_PN2≥D_PN1，则相邻节点PN属于voronoi点。

3.根据权利要求1所述的一种基于voronoi图的全局路径引导点生成规划方法，其特征在于，

遍历所有voronoi点，选择存在三个及以上相邻节点的voronoi点为交点；

从一交点出发，向相邻的交点搜索，遍历交点间所有voronoi点；

以机器人实际宽度为线段宽度，连接起始交点与当前voronoi点，若线段与障碍物相交，则判断发生碰撞，选取前一voronoi点为插入点；

提取交点与插入点作为引导点。

4.根据权利要求1或3所述的一种基于voronoi图的全局路径引导点生成规划方法，其特征在于，若相邻的两个引导点之间的距离小于预设阈值栅格，则保留其中一个引导点。

5.根据权利要求1所述的一种基于voronoi图的全局路径引导点生成规划方法，其特征在于，所述的导出的引导点信息包括引导点与相邻两引导点的夹角、引导点之间的朝向信息和最近障碍物的坐标。

Images