一种基于二分法和路径融合的全局避障路径规划方法

1.本发明属于机器人路径规划技术领域，特别是涉及一种基于二分法和路径融合的全局避障路径规划方法。


背景技术：

2.机器人的避障路径规划问题一直是机器人智能控制的核心问题之一，随着越来越多的机器人应用在生活和工作中，对机器人避障路径规划方法的研究显得更加重要，移动机器人对物品进行投递或者分类，以及工业机器人对物品进行分拣，都涉及机器人的全局避障路径规划，即在已知空间环境中规划出一条从起始点至目标点的无碰撞路径，且要求总路径长度最小；
3.针对已知空间环境下的机器人避障路径规划，现有技术也提出了一些解决方法，例如快速搜索随机树算法(rrt)，rrt规定从起始点开始，在空间中随机采样，并找到路径树上与采样点最接近且能与它无障碍连接的点，连接这个点与采样点，将采样点加入路径树，直至终点附近区域被探索到，然后对终点在路径树进行回溯，可以得到一条可行路径，但rrt计算效率低，并且这种方法无法保证得到的路径是最优的；基于rrt的改进方法rrt*，它在rrt的基础上做了改进，主要是在rrt的基础上加入了重新选择父节点和重布线的操作，虽然能够得到较优路径，但其搜索效率还是不能满足实时性的要求；基于rrt的另一种改进方法rrt_connect，它在rrt的基础上引入了双树扩展环节，分别以起始点和目标点为根节点生成两棵树进行双向扩展，当两棵树建立连接时可认为路径规划成功，rrt_connect加入了启发式步骤，加快了搜索速度，对于狭窄通道也具有较好的效果，虽然rrt_connect的搜索速度相比于rrt已经有了很大的提升，但随机搜索算法的整体搜索速度还是很慢，不能达到实时性要求；
4.因此，有必要对现有技术进行改进，以解决上述技术问题。


技术实现要素：

5.本发明的目的在于提供一种能规划出最短路径且能显著提高搜索效率的基于二分法和路径融合的全局避障路径规划方法，解决了现有的机器人用全局避障路径规划方法在实际的使用中存在无法规划最短路径和路径搜索效率低下的问题。
6.为解决上述技术问题，本发明是通过以下技术方案实现的：
7.本发明为一种基于二分法和路径融合的全局避障路径规划方法，规划方法具体包括以下步骤：
8.s1：初始化起始点start和目标点goal，根据已知空间环境建立二维空间模型；
9.s2：建立碰撞检测模型，在start至goal的直线段上生成采样点，判断start至goal是否存在直接路径；
10.s3：若存在直接路径，则将start和goal作为路径节点加入路径，并将该路径作为最佳路径，路径规划结束；
11.s4：若不存在直接路径，则基于二分法，计算得到start至goal的所有可能路径；
12.s5：计算每条路径的路径长度，选择任意一条路径标记为长度最小路径，依次和其他路径的长度进行对比，若存在更小的路径长度则自动替换，最终可以得到最小路径长度的路径；
13.s6：使用两端夹逼方法，对s5中得到的最小路径长度的路径进行路径融合，去除路径上的冗余路径节点，得到最佳路径。
14.进一步地，步骤s1中，在建立二维空间模型时，用任意形状的多边形表示二维空间中的障碍物，且用于表示障碍物的多边形之间没有交集。
15.进一步地，步骤s2中，在建立碰撞检测模型时，以多边形的外接矩形进行碰撞检测，同时设定安全距离，安全距离为机器人半径的110％～130％。
16.进一步地，移动机器人的半径为所属球体半径，工业机器人的半径为末端执行器所属圆柱体的底面半径。
17.进一步地，步骤s3中，在判断起始点start至目标点goal是否存在直接路径时，从start开始，每间隔一段距离n，在start至goal的直线段上生成一个采样点。
18.进一步地，对于移动机器人，n＝安全距离
×
(10％～30％)；对于工业机器人，n＝安全距离
×
(30％～50％)。
19.进一步地，步骤s4中，基于二分法计算start至goal的所有存在路径的方法包括以下具体步骤：
20.a.根据采样点得到start至goal发生碰撞的首段碰撞路径；
21.b.以表示障碍物的多边形外接矩形的特征对首段碰撞路径进行碰撞类型检测；
22.c.根据碰撞类型并基于二分法，计算得到一条或者多条路径，所得路径为完整的或/和不完整的；
23.d.对于完整的路径，直接将其保存为start至goal的可能路径；对于不完整的路径，将该路径的最后一个节点设置为二分法的起点，再次基于二分法得到该不完整路径的可能路径，重复操作直至所有的路径都为完整路径，最终得到start至goal的所有可能路径。
24.进一步地，二分法的操作步骤如下：
25.a.根据碰撞类型，将首段碰撞路径的中间点按指定方向从障碍物位置移出，使中间点至障碍物表面的距离为安全距离的3～5倍，令midpoint表示第一次基于二分法产生的中间点；
26.b.对于起点至midpoint的左半段路径，根据碰撞检测模型，若起点至midpoint存在直接路径，则结束左半段路径的路径规划，否则，继续按照步骤a找新的中间点，直至左半段路径不发生碰撞；
27.c.对于midpoint至终点的右半段路径，根据碰撞检测模型，若midpoint至终点存在直接路径，则结束右半段路径的路径规划，否则，继续按照步骤a找新的中间点，直至右半段路径不发生碰撞。
28.进一步地，两端夹逼的方法具体为：在路径两端设置头尾指针，头指针向goal移动，尾指针向start移动，若头指针指向的路径节点至goal存在直接路径，则将该路径节点加入最佳路径，将该节点设为goal，若尾指针指向的路径节点至start之间存在直接路径，
则将该路径节点加入最佳路径，将该节点设为start，重复这个过程，最终可以得到无冗余节点的最佳路径。
29.本发明具有以下有益效果：
30.基于二分法，在已知空间环境中进行避障路径规划，大大减少了路径生成时间，提升了算法的搜索效率，并且生成路径的路径长度最短，进一步地，利用两端夹逼的路径融合方法，去除了该路径上的冗余节点，得到最佳路径。
附图说明
31.为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
32.图1为本发明的总流程图；
33.图2为本发明的碰撞检测模型示意图；
34.图3为本发明的碰撞类型检测示意图；
35.图4为本发明的中间点移出障碍物示意图；
36.图5为本发明的二分法流程图；
37.图6为本发明的路径融合结果示意图。
具体实施方式
38.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。
39.请参阅图1所示，本发明为一种基于二分法和路径融合的全局避障路径规划方法，规划方法具体包括以下步骤：
40.s1：初始化起始点start和目标点goal，根据已知空间环境建立二维空间模型；
41.在建立二维空间模型时，用任意形状的多边形表示二维空间中的障碍物，且用于表示障碍物的多边形之间没有交集。
42.s2：建立碰撞检测模型，在start至goal的直线段上生成采样点，判断start至goal是否存在直接路径；
43.在建立碰撞检测模型时，以多边形的外接矩形进行碰撞检测，同时设定安全距离，安全距离为机器人半径的110％～130％；
44.其中，移动机器人的半径为所属球体半径，工业机器人的半径为末端执行器所属圆柱体的底面半径；
45.如图2所示，图2中列举了一些不同形状的外接矩形，碰撞检测模型、碰撞类型检测以及后面的步骤都是基于这些外接矩形进行的。
46.s3：若存在直接路径，则将start和goal作为路径节点加入路径，并将该路径作为最佳路径，路径规划结束；
47.在判断起始点start至目标点goal是否存在直接路径时，从start开始，每间隔一段距离n，在start至goal的直线段上生成一个采样点；
48.对于移动机器人，n＝安全距离
×
(10％～30％)；对于工业机器人，n＝安全距离
×
(30％～50％)。
49.s4：若不存在直接路径，则基于二分法，计算得到start至goal的所有可能路径；
50.基于二分法计算start至goal的所有存在路径的方法包括以下具体步骤：
51.a.根据采样点得到start至goal发生碰撞的首段碰撞路径，可用数组obspoint记录第一段发生碰撞的采样点集合；
52.b.以表示障碍物的多边形外接矩形的特征对首段碰撞路径进行碰撞类型检测，即根据首段碰撞路径的第一个碰撞点obspoint(1)和最后一个碰撞点obspoint(end)，进行碰撞类型检测，得到首段碰撞路径的碰撞类型，并根据碰撞类型确定中间点；
53.具体的，如图3-4所示，当碰撞类型为1时，中间点选择图4中的类型1位置，当碰撞类型为2时，中间点选择图4中的类型2位置；
54.c.根据碰撞类型并基于二分法，计算得到一条或者多条路径，所得路径为完整的或/和不完整的，将这些路径保持在route_gen数组中；
55.二分法的操作步骤如下，参阅图5所示：
56.a.根据碰撞类型，将首段碰撞路径的中间点按指定方向从障碍物位置移出，使中间点至障碍物表面的距离为安全距离的3～5倍，令midpoint表示第一次基于二分法产生的中间点；
57.令距离障碍物的距离为growth，则growth＝n*r，其中，r为安全距离，n为安全距离的倍数，从而得出如图4的中间点移出障碍物示图；
58.再令midpoint_temp_left作为左半段路径基于二分法生成的中间点，midpoint_temp_right作为右半段路径基于二分法生成的中间点，令它们的初始值等于midpoint；
59.即：midpoint_temp_left＝midpoint_temp_right＝midpoint；
60.b.对于起点至midpoint的左半段路径，根据碰撞0检测模型，若起点至midpoint存在直接路径，则结束左半段路径的路径规划，否则，继续按照步骤a找新的中间点，直至左半段路径不发生碰撞；
61.即，将start加入左半段路径数组path_left，此时start作为左半段路径起点，midpoint作为左半段路径终点，对start至midpoint进行碰撞检测，若存在直接路径，然后将midpoint加入path_left，此时左半段路径规划结束，否则，对start至midpoint进行碰撞检测，可以得到首段碰撞路径，碰撞点集为obspoint，根据obspoint的第一个点和最后一个点进行碰撞类型检测，根据碰撞类型，按照指定方向将start至midpoint的首段碰撞路径的中点midpoint_temp_left移出障碍物；
62.令start＝midpoint_temp_left，重复这个过程，直至左半段路径没有碰撞，可以得到最终的左半段路径数组path_left；
63.c.对于midpoint至终点的右半段路径，根据碰撞检测模型，若midpoint至终点存在直接路径，则结束右半段路径的路径规划，否则，继续按照步骤a找新的中间点，直至右半段路径不发生碰撞；
64.即，将midpoint加入右半段路径数组path_right，此时midpoint作为左半段路径起点，goal作为右半段路径终点，对start至midpoint进行碰撞检测，若存在直接路径，然后将goal加入path_right，此时右半段路径规划结束，否则，对midpoint至goal进行碰撞检
测，可以得到首段碰撞路径，碰撞点集为obspoint，根据obspoint的第一个点和最后一个点进行碰撞类型检测；
65.在这里可能会出现两种情况；
66.情况1：若在二分法的右半段路径循环过程中，根据碰撞类型检测检测到的碰撞物和前一次检测到的碰撞物不一致，则退出循环，此时，二分法计算得到的路径是不完整路径，即该路径的最后一个节点不是goal；
67.情况2：若在二分法的右半段路径循环过程中，根据碰撞类型检测检测到的碰撞物和前一次检测到的碰撞物一致，则根据碰撞类型，按照指定方向将start至midpoint的首段碰撞路径的中点midpoint_temp_right移出障碍物，此时令midpoint＝midpoint_temp_right，重复这个过程，直至右半段路径没有碰撞，可以得到最终的右半段路径数组path_right。
68.d.对于route_gen数组中完整的路径，直接将其保存为start至goal的可能路径，用route_complete数组记录；对于route_gen中不完整的路径，将该路径的最后一个节点设置为二分法的起点，再次基于二分法得到该不完整路径的可能路径，存入route_gen，重复操作直至所有的路径都为完整路径，最终得到start至goal的所有可能路径。
69.s5：计算每条路径的路径长度，选择任意一条路径标记为长度最小路径，依次和其他路径的长度进行对比，若存在更小的路径长度则自动替换，最终可以得到最小路径长度的路径；
70.即，对于得到的route_complete数组，依次计算数组中每条路径的路径长度dis，dis的计算公式如下：
[0071][0072]
其中，(xi,yi)和(x
i+1
,y
i+1
)分别表示一条路径的前后两个路径节点，i＝1,2,3...，对于每一条路径，dis初始化为0，计算完route_complete数组中所有路径的路径，并最终通过对比替换得到最小路径长度的路径。
[0073]
s6：使用两端夹逼方法，对s5中得到的最小路径长度的路径进行路径融合，去除路径上的冗余路径节点，得到最佳路径；
[0074]
两端夹逼的方法具体为：在路径两端设置头尾指针，头指针向goal移动，尾指针向start移动，若头指针指向的路径节点至goal存在直接路径，则将该路径节点加入最佳路径，将该节点设为goal，若尾指针指向的路径节点至start之间存在直接路径，则将该路径节点加入最佳路径，将该节点设为start，重复这个过程，最终可以得到无冗余节点的最佳路径；
[0075]
即，设定cur_left和cur_right分别指向起始点start和目标点goal，首先判断start至cur_right指向的路径节点是否存在直接路径，若存在直接路径，则将cur_right指向的路径节点加入最佳路径，将start置为cur_right指向的路径节点，否则，cur_right向前移动一次；然后判断cur_left指向的路径节点至goal是否存在直接路径，若存在直接路径，则将cur_left指向的路径节点加入最佳路径，将goal置为cur_left指向的路径节点，否则，cur_left向后移动一次，重复这个过程，直至start和goal为同一点，最后，将初始的起始点start和目标点goal加入最佳路径，作为最佳路径的起点和终点，得到无冗余节点的最佳路径，路径融合结果如图6所示。
[0076]
以上仅为本发明的优选实施例，并不限制本发明，任何对前述各实施例所记载的技术方案进行修改，对其中部分技术特征进行等同替换，所作的任何修改、等同替换、改进，均属于在本发明的保护范围。