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

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


背景技术：

2.移动机器人导航中常将规划分为全局路径规划和局部路径规划。全局规划器规划是从目标的到终点的一条路径，局部规划器循着全局路径，根据实际的场景做局部的规划，如避让行人、绕开障碍物等。
3.一般全局规划的技术方案是，由slam建好的栅格地图给全局规划器，全局规划使用搜索算法（比如a*，动态a*，概率路图法，跳点法等）再结合一些约束条件，找到一条符合条件的全局路径，一般希望找的路径光滑，且耗时最短。例如，一种在中国专利文献上公开的“一种移动机器人新型骨架提取的voronoi路径规划算法”，其公告号cn111323037b，包括如下步骤：步骤一、首先对移动机器人已知先验信息的三色栅格地图进行二值化处理，在对其进行腐蚀、膨胀处理；步骤二、对处理后的二值地图经进行骨架提取，得到voronoi图；步骤三、用原始的三色地图进行机器人定位，在骨架地图上进行全局路径搜索，对得到的全局路径进行改进的三次样条平滑处理，得到优化后的全局路径。根据本发明提出的算法提取出来的voronoi骨架没有大量毛边，整体的路径规划更加高效。
4.但是上述方法生成的全局路径，实际应用操作复杂。首先找到这样的一条全局路径计算量比较大，且实际场景中易出现动态障碍物或者事先不存在的静态障碍物，以至于跟踪这条全局路径进行循迹就没有那么必要。只需要一些关键的全局路径点，包含道路的特征信息如道路宽度、道路弯曲程度等，就可以替换全局路径点。
5.在实际导航过程中，机器人跟踪这些引导点，由局部规划器实现避障与跟踪引导点的程度，实现全局规划器与局部规划器的松耦合，即机器人因为避障偏离引导点也没关系后面再引导回来即可。


技术实现要素：

6.本发明主要解决现有技术全局路径生成操作复杂，计算量大，容易受到动态障碍物或者事先不存在的静态障碍物的干扰问题；提供一种基于voronoi图的全局路径引导点生成规划方法，对激光扫描获得的区域进行预处理，用处理后的地图生成voronoi图，提取voronoi中的引导点并进行处理生成全局规划的路径引导点，利用路径引导点实现移动机器人在区域地图导航时的全局路径规划。
7.本发明的上述技术问题主要是通过下述技术方案得以解决的：一种基于voronoi图的全局路径引导点生成规划方法，包括以下步骤：s1：使用激光雷达结合slam构建工作区域环境地图，初步处理后形成栅格地图；s2：根据voronoi生成规则，提取voronoi点，剔除障碍物在同侧的相邻voronoi点；s3：遍历voronoi点，选取交点；连接相邻交点，根据交点间连线与障碍物关系提取
引导点；s4：导出引导点信息，编辑引导点信息；s5：移动机器人加载引导点的配置文件，接收到目标点坐标；根据自身位置和目标点位置，按最近距离原则找到起点和终点的引导点序号，生成路径。
8.本方案增加了同侧障碍物判断，去除了一些杂点的影响或是分叉路径，使得在栅格地图上整个voronoi图很清晰，与在连续性空间中voronoi图相一致。提取的voronoi图的引导点，很好地表示了地图的拓扑信息，用这些引导点进行规划高效快速。计算快速，不用输出整条路径，用引导点来代替路径信息；跟踪引导点能让机器人近可能走中间，不用按路径严格走，给局部规划的避障留了余地，是松耦合，运动更安全。
9.作为优选，若一个点到距离最近的两个障碍物的距离相等，则该点为voronoi点。
10.作为优选，剔除障碍物在同侧的voronoi点的过程为：选择任意节点p与相邻节点pn；令节点p到节点p最近障碍物的连线为p1，节点p到相邻节点pn最近障碍物的连线为p2；相邻节点pn到相邻节点pn最近障碍物的连线为pn1，相邻节点pn到节点p最近障碍物的连线为pn2；计算p1与p2之间的夹角或计算pn1与pn2之间的夹角；若夹角小于额定阈值θ，则对进行voronoi点剔除。
11.判断两个点的障碍物是否在同侧，避免pn与p的障碍物在同侧，导致有多个周边点属于voronoi。
12.作为优选，计算节点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点。
13.增加了同侧障碍物判断，去除了一些杂点的影响或是分叉路径，使得在栅格地图上整个voronoi图很清晰，与在连续性空间中voronoi图相一致。
14.作为优选，遍历所有voronoi点，选择存在三个及以上相邻节点的voronoi点为交点；从一交点出发，向相邻的交点搜索，遍历交点间所有voronoi点；以机器人实际宽度为线段宽度，连接起始交点与当前voronoi点，若线段与障碍物相交，则判断发生碰撞，选取前一voronoi点为插入点；提取交点与插入点作为引导点。
15.用引导点来代替路径信息，不用输出整条路径，计算速度快。
16.作为优选，若相邻的两个引导点之间的距离小于预设阈值栅格，则保留其中一个引导点。
17.最后得到的引导点都处于道路中央，且该位置附近有明显路况变化。跟踪引导点能让机器人近可能走中间，不用按路径严格走，给局部规划的避障留了余地，是松耦合，运动更安全。
18.作为优选，所述的导出的引导点信息包括引导点与相邻两引导点的夹角、引导点
之间的朝向信息和最近障碍物的坐标。
19.引导点包含丰富道路信息，可以根据道路属性设置通行方向，便于机器人导航决策。
20.本发明的有益效果是：1. 用引导点来代替路径信息，不用输出整条路径，计算速度快。
21.2. 引导点包含丰富道路信息，可以根据道路属性设置通行方向，便于机器人导航决策。
22.3. 跟踪引导点能让机器人尽可能走中间，不用按路径严格走，给局部规划的避障留了余地，是松耦合，运动更安全。
23.4. 增加了同侧障碍物判断，去除了一些杂点的影响或是分叉路径，使得在栅格地图上整个voronoi图很清晰，与在连续性空间中voronoi图相一致。
附图说明
24.图1是本发明的全局路径引导点生成规划方法流程图。
25.图2是本发明的初步处理后的环境地图。
26.图3是本发明栅格地图提取voronoi点的示意图。
27.图4是本发明的voronoi点可视化示意图。
28.图5是本发明引导点信息的描述示意图。
29.图6是发明的基于引导点的全局规划的路径示意图。
30.图5中1~7为引导点的序号。
具体实施方式
31.下面通过实施例，并结合附图，对本发明的技术方案作进一步具体的说明。
32.实施例：本实施例的一种基于voronoi图的全局路径引导点生成规划方法，如图1所示，包括以下步骤：s1：使用激光雷达结合slam构建工作区域环境地图，初步处理后形成栅格地图。
33.如图2所示，使用激光雷达结合slam（同时定位与建图）技术构建机器人需要工作区域的环境地图，对地图进行初步处理，人为标记行驶道路、障碍物等，限制机器活动区域。
34.slam建图得到的是粗糙的环境地图，有些地方是柱子，有些地方是道路，但是建图可能仅体现了柱子的一部分，或是激光仅扫到了道路的一个轮廓，有些地方是残缺的。
35.对于机器人导航来说，安全是最重要的，有必要人为根据地图绘制障碍物、道路边界线等特征。在本实施例中，初步处理是人为的标识一些物体。
36.将初步处理后的环境地图栅格化，形成栅格地图。
37.s2：根据voronoi生成规则，提取voronoi点，剔除障碍物在同侧的相邻voronoi点。
38.在连续空间一个点属于voronoi那么它到两个障碍物的距离相等，在栅格地图中，则变成了离散空间，需要一些判断条件辅助。
39.voronoi生成规则为，若一个点到距离最近的两个障碍物的距离相等，则该点为voronoi点。
40.令每个点存储它最近障碍物的点坐标，考察任意点当p和p的邻居节点pn，判断这两个点的障碍物是否在同侧，避免pn与p的障碍物在同侧，导致有多个周边点属于voronoi。
41.剔除障碍物在同侧的voronoi点的过程为：选择任意节点p与相邻节点pn；令节点p到节点p最近障碍物的连线为p1，节点p到相邻节点pn最近障碍物的连线为p2；相邻节点pn到相邻节点pn最近障碍物的连线为pn1，相邻节点pn到节点p最近障碍物的连线为pn2。
42.使用余弦定理，计算p1与p2之间的夹角或计算pn1与pn2之间的夹角；若夹角小于额定的夹角阈值θ，则对这种情况进行voronoi点剔除。
43.计算节点p到节点p最近障碍物的距离d_p1；计算节点p到相邻节点pn最近障碍物的距离d_p2；计算相邻节点pn到相邻节点pn最近障碍物的距离d_pn1；计算相邻节点pn到节点p最近障碍物的距离d_pn2。
44.若d_p2≤d_pn2且d_p2≥d_p1，则节点p属于voronoi点；若d_pn2≤d_p2且d_pn2≥d_pn1，则相邻节点pn属于voronoi点。
45.增加了同侧障碍物判断，去除了一些杂点的影响或是分叉路径，使得在栅格地图上整个voronoi图很清晰，与在连续性空间中voronoi图相一致。
46.例如，图3中的格子p（十字线格子），它到最近障碍物（黑色实心格子）距离是1.41格。p上方邻居格子pn_1（反斜线格子）到最近障碍物的距离是1格，p下方的格子pn_2（斜线格子）到最近障碍物的距离是2格。
47.p到p最近障碍物与p到pn_1最近障碍物的夹角是45
°
，若设夹角阈值θ为90
°
，小于阈值判断为同侧，剔除pn_1。而p到p最近障碍物与p到pn_2最近障碍物的夹角是145
°
，大于阈值判读为异侧，保留pn_2。
48.计算p到pn_2最近障碍物的距离是3格，pn_2到p最近障碍物的距离也是2格，同时满足p到pn_2最近障碍物的距离大于等于p到自身最近障碍物的距离1.41格，所以p属于voronoi。
49.处理后，将属于voronoi的点显示出来，如图 4所示，黑色栅格表示不可通行的区域，星形表示属于voronoi栅格，形成类似骨架的线条，反映了道路的拓扑形态。星型栅格组成的连线的特点在于，在直线道路，星型栅格的邻居栅格有两个，在道路有弯曲处，存在不同的交叉，导致其邻居点的个数存在两个以上。
50.s3：遍历voronoi点，选取交点；连接相邻交点，根据交点间连线与障碍物关系提取引导点。
51.遍历所有voronoi点，选择存在三个及以上相邻节点的voronoi点为交点，在图3中以圆形栅格表示。
52.从一交点出发，向相邻的交点搜索，遍历交点间所有voronoi点。
53.以机器人实际宽度为线段宽度，连接起始交点与当前voronoi点，若线段与障碍物相交，则判断发生碰撞，选取前一voronoi点为插入点。
54.为提取引导点，考虑移动机器人的实际宽度，从一端的交点出发，向另一端的交点搜索，遍历该段线段中所有的点，连接起始端的交点和当前遍历的点，连线宽度为移动机器人的宽度，若当前遍历点与起始端交点的连线与障碍物发生碰撞，则取发生碰撞前的一个
点为插入点，在图4中以三角型表示 。
55.提取交点与插入点作为引导点。以此保证任意具有连接关系的点，它们的连线上都没有障碍物，插点后的连线如图4中的线段所示，圆形交点和三角形插点共同组成引导点。
56.用引导点来代替路径信息，不用输出整条路径，计算速度快。
57.若相邻的两个引导点之间的距离小于预设阈值栅格，则保留其中一个引导点。对过近的关键进行剔除，判断依据为若两个引导点间隔小于一定阈值栅格，如5个栅格，只保留其中一个。最后得到的引导点都处于道路中央，且该位置附近有明显路况变化。
58.最后得到的引导点都处于道路中央，且该位置附近有明显路况变化。跟踪引导点能让机器人近可能走中间，不用按路径严格走，给局部规划的避障留了余地，是松耦合，运动更安全。
59.s4：导出引导点信息，编辑引导点信息。
60.导出的引导点信息包括引导点与相邻两引导点的夹角、引导点之间的朝向信息和最近障碍物的坐标。
61.引导点包含丰富道路信息，可以根据道路属性设置通行方向，便于机器人导航决策。
62.计算引导点与其周边两个引导点夹角，用来表示道路的弯曲程度；计算点与点之间朝向信息，用向量形式表示，用来表示道路的通行方向；计算每个点周边最近障碍物的坐标，用来表示道路的通行宽度。
63.将这些信息都导出为文件形式，作为导航的配置文件。如图5示例所示，数字1-7表示引导点的序号，引导点1包含的信息包括向量v_1-7、 v_1-2，夹角theta_2-1-7，到最近障碍物的距离l_1。
64.其中，向量v_1-7、 v_1-2分别为从引导点1指向引导点7和从引导点1指向引导点2的向量，表示道路的方向信息，即从引导点1可以到引导点7，从引导点1可以到引导点2；夹角theta_2-1-7表示从引导点2经过引导点1到引导点7道路的弯曲程度；l_1表示引导点1到道路边缘最近的距离，以代表道路宽度。
65.引导点以反映道路的信息，用以引导机器人，在使用时记为引导点。
66.根据人为的需要对第四步导出的引导点进行编辑，如删减点、增加点、修改朝向信息等，为用户可编辑的文件，完成关于引导点的构建。
67.引导点具有连接关系，包含原始地图的关键信息，是图的一种表达方式。在实际移动机器人导航过程中，使用引导点进行规划可以快速规划出一组点串。
68.s5：移动机器人加载引导点的配置文件，接收到目标点坐标；根据自身位置和目标点位置，按最近距离原则找到起点和终点的引导点序号，生成路径。
69.移动机器人加载引导点的配置文件，接收到目标点坐标。根据自身位置，和目标点位置，按最近距离原则找到起点和终点的引导点序号。
70.根据起始引导点、终止引导点和相邻两点之间的距离，找到距离最近的引导点序列，使用方法不限于a*、跳点法、概率路图法等搜索算法。根据找到的引导点序列，并对距离较远的两点之间按3m间隔进行插点，生成路径点。机器人沿着路径方向，选择路径点进行跟踪，直到到达目标点。
71.如图6 所示，机器人处于人工标记处理后的环境地图，根据目标点和自身位置，找到最邻近的起始引导点1和终止引导点4。根据引导点1和引导点4生成距离最短的引导点序列1-2-3-4。对于引导点序列中间隔较远的点，按3m距离插点，生成一串从移动机器人附近到目标点的一组路径点序列。导航过程中，机器人只需逐点跟踪引导点直到终点。在跟踪过程中，机器人根据引导点存储的信息，如道路宽度、道路弯曲程度，在道路比较空旷的场景运行速度快一点，如引导点到道路边缘距离l大于2m，机器人按最大速度不超过2m/s运动，l小于2m，机器人按最大速度不超过1.5m/s运动；在拐弯曲折道路场景运行速度慢一点，如引导点theta角小于90度，限制机器人速度上限为原来的一半，theta角大于90度按正常速度运动。
72.本方案增加了同侧障碍物判断，去除了一些杂点的影响或是分叉路径，使得在栅格地图上整个voronoi图很清晰，与在连续性空间中voronoi图相一致。提取的voronoi图的引导点，很好地表示了地图的拓扑信息，用这些引导点进行规划高效快速。计算快速，不用输出整条路径，用引导点来代替路径信息；跟踪引导点能让机器人近可能走中间，不用按路径严格走，给局部规划的避障留了余地，是松耦合，运动更安全。
73.应理解，实施例仅用于说明本发明而不用于限制本发明的范围。此外应理解，在阅读了本发明讲授的内容之后，本领域技术人员可以对本发明作各种改动或修改，这些等价形式同样落于本技术所附权利要求书所限定的范围。