本发明涉及路径规划领域，具体涉及一种基于voronoi骨架的移动机器人融合路径规划方法。
背景技术：
1、路径规划是移动机器人完成导航任务的关键环节。移动机器人通过环境建模实现对工作空间的地图表示，根据地图完成全局路径规划。在机器人导航过程中，利用自身传感器实时感知环境障碍进行避障局部路径规划。目前，应用广泛的全局路径规划算法有基于栅格地图的a*算法与dijkstra算法，基于拓扑地图的voronoi图算法等。局部路径规划算法有人工势场法(artificial potential field,apf)、动态窗口法(dynamic windowapproach,dwa)等。
2、栅格地图下使用a*或dijkstra算法进行路径规划时，通过搜索栅格能快速规划出一条可行路径，但是路径拐点多，易靠近障碍物，不利于机器人在复杂环境下运行。拓扑地图下使用voronoi图法可规划出安全路径，但是拐点多、路径冗长，导致机器人导航效率低。以上全局路径融入动态窗口法使其可以避免对障碍物的碰撞，提高机器人运行安全性。
3、现有技术公开了一种基于改进的a*融合dwa算法的移动机器人路径规划方法(公开号：cn116817956a)，在栅格地图上利用a*算法进行路径规划，使用折叠优化算法优化得到优化路径，并将优化路径节点输入到dwa算法作为中间目标点，使局部路径遵循优化路径。若在复杂多障碍物环境下，优化路径无法保障远离障碍物，同时dwa算法仅使用路径节点信息，融合路径无法保障精炼性。
技术实现思路
1、为解决现有技术中存在的技术缺陷，本发明提出了一种基于voronoi骨架的移动机器人融合路径规划方法，通过将环境栅格地图转为骨架图，基于骨架图生成全局优化先验路径，融合全局引导型dwa算法进行实时局部规划，解决了现有技术中在栅格地图下路径规划存在路径拐点多、运行不平滑、安全性不高等问题。
2、本发明通过以下技术方案实现：
3、基于voronoi骨架的移动机器人融合路径规划方法，包含步骤：
4、步骤1：构建栅格地图：使用激光slam方法构建移动机器人作业环境栅格地图；
5、步骤2：voronoi骨架图构建：通过栅格-骨架图构造方法，在栅格地图上通过种子点集提取、delaunay三角剖分得到三角网格图；在三角网格图上通过对偶voronoi构造得到voronoi图；针对voronoi图创建邻接矩阵，通过邻接矩阵变换对障碍物栅格相关的voronoi顶点和边进行处理，并删除冗余顶点，得到移动机器人作业环境的骨架图；
6、步骤3：全局先验路径规划：在移动机器人运行前，根据骨架图规划-关键顶点优化两阶段规划方法，基于骨架顶点使用a*算法进行全局路径规划得到初步全局路径；然后根据关键顶点提取方法对初步全局路径中的关键顶点进行选取，依次连接得到一条全局优化先验路径；
7、步骤4：局部路径动态目标点规划：根据机器人与先验全局路径之间的相对位置关系，依次以全局先验路径中的拐点作为局部路径规划的动态目标点，进而将全局先验路径与实时局部路径进行融合；
8、步骤5：局部无碰撞路径规划：通过优化dwa算法的速度采样空间，在评价函数中引入全局引导型函数项、终点引导型函数项构建全局引导型dwa算法；将其作为局部路径规划算法，以步骤4规划的动态目标点为算法输入目标，通过循环执行的局部无碰撞路径规划直至移动机器人到达最终目标。
9、进一步地，所述栅格-骨架图构造方法，包含以下步骤：
10、步骤1.1：delaunay三角剖分：提取栅格地图中障碍物栅格、建图边界栅格的中心点坐标，构建一组种子点集；以种子点集中的点为三角网格图中边的起始点，通过delaunay三角剖分方法生成唯一的delaunay三角网格图，所述三角网格图中所有边互不相交；
11、步骤1.2：对偶voronoi构造：依次遍历delaunay三角网格图中的所有delaunay三角形，若两个delaunay三角形具有公共边，则以所述两个delaunay三角形的外接圆圆心为voronoi点，所述两个voronoi点连线为voronoi边，通过对偶voronoi构造方法将delaunay三角网格图转换至voronoi图；
12、步骤1.3：voronoi图邻接矩阵构造：依次遍历voronoi图的所有顶点，若顶点i和顶点j之间具有连接关系，则邻接矩阵avor(i,j)＝disij,其中disij为顶点i和顶点j之间的欧式距离；若顶点i和顶点j之间没有连接关系，则邻接矩阵avor(i,j)＝0，从而构建大小为n×n的对称邻接矩阵，n为voronoi图顶点数；
13、步骤1.4：邻接矩阵变换：通过判断voronoi顶点、voronoi边与障碍物栅格之间的位置关系，对步骤1.3生成的邻接矩阵进行变换处理，剔除障碍物内部voronoi顶点，剔除穿过障碍物的voronoi边，剔除冗余voronoi顶点，得到voronoi图的骨架图。
14、进一步地，所述所述邻接矩阵变换，包含以下步骤：
15、步骤2.1：剔除障碍物内部voronoi顶点：将顶点坐标(xvi,yvi)按照式(1)进行转换得到转换后坐标(xg_vi,yg_vi)，式中floor()为向下取整函数，m_xl、m_yl分别为环境地图x、y方向的最小值，resolution为栅格分辨率；如果该转换后坐标为“0”，则其对应的栅格为障碍物栅格，在邻接矩阵中删除该顶点对应的行和列；若该转换后坐标为“1”，则不为障碍物栅格，不做处理；
16、
17、步骤2.2：剔除穿过障碍物的voronoi边：构建包含voronoi边的最小栅格矩阵m，在最小栅格矩阵m内计算障碍物占用栅格中心到voronoi边的最小距离dobs；dobs计算方式如式(2)所示，p1、p2为voronoi边的两个端点坐标，pi为占用栅格的中心点坐标，det()为求取矩阵行列式函数，norm()为求取向量欧式距离函数；
18、
19、如果倍栅格边长，则认为该边穿过障碍物，在邻接矩阵中将该边对应矩阵元素值置0；否则不做处理；
20、步骤2.3：剔除直线内部冗余voronoi顶点：遍历voronoi图中所有顶点，若voronoi顶点vi的度为2，则取该点相邻的两个voronoi顶点，判断所述三点是否在同一直线上，若三点在同一直线上，则voronoi顶点vi为冗余顶点，在邻接矩阵中删除该顶点对应的行和列；若三点不在同一直线上，则不做处理；
21、步骤2.4：剔除相同冗余voronoi顶点：遍历voronoi图中所有顶点，若在同一位置有多个相同的voronoi顶点，则保留该位置处voronoi顶点的连接关系，剔除重复的voronoi顶点，在邻接矩阵中删除该顶点对应的行和列。
22、进一步地，所述骨架图规划-关键顶点优化两阶段规划方法包含以下步骤：
23、步骤3.1：路径两端骨架顶点搜索：通过最近点搜索得到距离起点s、终点e欧式距离最小的骨架顶点vs、ve；
24、步骤3.2：基于骨架顶点全局规划：采用a*路径规划算法，以骨架顶点为基本搜索单元，骨架边为搜索单元之间的探索关系，骨架边的长度为两顶点之间的实际代价g(n)，启发式搜索从vs到ve的路径，并将起点、终点加入路径中，得到初步规划路径apath；
25、步骤3.3：关键顶点提取优化路径：针对初步规划路径apath，根据关键顶点提取方法提取规划路径中的关键顶点、剔除冗余顶点，得到全局优化先验路径。
26、进一步地，所述关键顶点提取方法包含以下步骤：
27、步骤4.1：初始化：初始化全局路径关键顶点集tpath为空，初始化相邻顶点序号j＝1，从起点s开始，将起点s加入到全局路径关键顶点集tpath中；
28、步骤4.2：终止条件判断：从初步规划路径apath中找到起点s的下一相邻顶点vj，当vj是终点时，将终点加入到全局路径关键顶点集tpath中，进入步骤4.6；当vj不是终点时，进入步骤4.3；
29、步骤4.3：避障需求判断：以点s和vj为对角顶点构成矩形区域ro，若矩形区域ro内存在障碍物，则存在空间避障需求，进入步骤4.5；若矩形区域ro内不存在障碍物，则认为当前顶点为冗余顶点，进入步骤4.4；
30、步骤4.4：相邻顶点遍历：j＝j+1，在初步规划路径apath中寻找新的相邻顶点vj，返回步骤4.3；
31、步骤4.5：关键顶点提取：若障碍物距离路径的最小距离dmin≤距离阈值dth，提取关键顶点，将点vj-1作为起始点s，返回步骤4.2；反之，j＝j+1，返回步骤4.3；
32、步骤4.6：全局优化路径生成：将全局路径关键顶点集tpath中的关键顶点依次连接构成全局优化路径。
33、进一步地，所述全局引导型dwa算法包含以下步骤：
34、步骤5.1：速度矢量空间设置：根据机器人自身以及周围环境状态，通过设置机器人参数、机器人驱动力约束获得机器人速度矢量(v,ω)的采样空间，如式(3)所示：
35、
36、其中，vmax和vmin表示机器人参数约束的速度极限；ωmax和ωmin表示机器人参数约束的角速度极限；vc、ωc是机器人当前速度和角速度；amax、αmax分别是因机器人有限驱动力而造成的线加速度和角加速度极限；
37、步骤5.2：矢量空间速度采样：在速度矢量空间中，设定线速度和角速度的步长，按顺序原则依次选取线速度和角速度的速度矢量(v,ω)，所述顺序原则包括：(1)从线速度和角速度的最小值开始，按照步长依次增加的顺序，直至到达线速度和角速度的最大值；(2)从线速度和角速度的最大值开始，按照步长依次减小的顺序，直至到达线速度和角速度的最小值；
38、步骤5.3：机器人轨迹预测：根据机器人运动学模型，针对线速度和角速度的速度矢量(v,ω)，预测机器人在未来时间段的运行轨迹traj；
39、步骤5.4：预测轨迹评价：计算预测轨迹traj的航向得分heading(v,ω)、障碍物距离得分occdist(v,ω)、速度得分velocity(v,ω)、全局引导项得分vorpath(v,ω)和终点收敛得分goaldist(v,ω)，将所述各项得分进行数值归一化处理，并进行加权求和，作为预测轨迹的评价得分，如式(4)所示：
40、
41、式中，k为平滑系数；α、β、γ、σ、η为各子函数的加权系数；
42、步骤5.5：速度矢量选取：针对速度矢量空间中所有速度矢量(v,ω)采样的预测轨迹，选取得分最高的预测轨迹tr所对应的速度矢量作为局部路径规划的结果。
43、进一步地，所述全局引导项函数项vorpath(v,ω)如式(5)所示：
44、
45、其中，p1、p2是全局路径中两个相邻节点，p为预测轨迹末端坐标，det()为求取矩阵行列式函数，norm()为求取向量欧几里得范数函数。
46、进一步地，所述终点收敛项goaldist(v,ω)如式(6)所示：
47、
48、式中，dmin(traj,goal)为预测轨迹与目标的最短距离，d(r,goal)为机器人与目标之间的欧式距离，dsensor为传感器的障碍物探测距离。
49、与现有技术相比，本发明至少具有下述的有益效果或优点：
50、本发明所述的基于voronoi骨架的移动机器人融合路径规划方法设计了栅格地图-骨架图转换方法，通过delaunay三角剖分、对偶voronoi图构造以及无向图邻接矩阵变换处理，构建环境栅格地图的骨架图。通过骨架图规划-关键顶点优化两阶段规划方法规划出的全局路径保留voronoi算法远离障碍物特性和效规划效率高等优势的同时，减少了路径长度和拐点数量，使机器人运行路径更加平滑和安全。
51、同时，在局部路径规划层面，针对传统dwa算法运行效率低、易陷入局部最优、缺乏对全局路径的使用等问题，优化了速度采样空间，增加全局引导型评价函数项、终点收敛评价函数项，在保留对障碍物进行规避特点的前提下，提高机器人运行效率和对全局路径的使用，同时促使移动机器人在目标点附近快速抵达目标点。
1.一种基于voronoi骨架的移动机器人融合路径规划方法，其特征在于包括离线阶段和在线阶段过程，
2.根据权利要求1所述的一种基于voronoi骨架的移动机器人融合路径规划方法，其特征在于，所述步骤2中栅格-骨架图构造方法包括以下步骤：
3.根据权利要求2所述的一种基于voronoi骨架的移动机器人融合路径规划方法，其特征在于，所述邻接矩阵变换包含以下步骤：
4.根据权利要求1所述的一种基于voronoi骨架的移动机器人融合路径规划方法，其特征在于，所述骨架图规划-关键顶点优化两阶段规划方法包含以下步骤：
5.根据权利要求4所述的一种基于voronoi骨架的移动机器人融合路径规划方法，其特征在于，所述关键顶点提取方法包含以下步骤：
6.根据权利要求1所述的一种基于voronoi骨架的移动机器人融合路径规划方法，其特征在于所述全局引导型dwa算法包含以下步骤：
7.根据权利要求6所述的一种基于voronoi骨架的移动机器人融合路径规划方法，其特征在于所述全局引导项得分vorpath(v,ω)如式(5)所示：
8.根据权利要求6所述的一种基于voronoi骨架的移动机器人融合路径规划方法，其特征在于所述终点收敛得分goaldist(v,ω)如式(6)所示：
