[go: up one dir, main page]
More Web Proxy on the site http://driver.im/

CN114415665B - 一种用于避障路径规划的算法 - Google Patents

一种用于避障路径规划的算法 Download PDF

Info

Publication number
CN114415665B
CN114415665B CN202111549089.3A CN202111549089A CN114415665B CN 114415665 B CN114415665 B CN 114415665B CN 202111549089 A CN202111549089 A CN 202111549089A CN 114415665 B CN114415665 B CN 114415665B
Authority
CN
China
Prior art keywords
path
point
bounding box
points
obstacle
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202111549089.3A
Other languages
English (en)
Other versions
CN114415665A (zh
Inventor
周童
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Xinjiang Bosheran Intelligent Agricultural Machinery Co ltd
Original Assignee
Xinjiang Bosheran Intelligent Agricultural Machinery Co ltd
Filing date
Publication date
Application filed by Xinjiang Bosheran Intelligent Agricultural Machinery Co ltd filed Critical Xinjiang Bosheran Intelligent Agricultural Machinery Co ltd
Priority to CN202111549089.3A priority Critical patent/CN114415665B/zh
Publication of CN114415665A publication Critical patent/CN114415665A/zh
Application granted granted Critical
Publication of CN114415665B publication Critical patent/CN114415665B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Abstract

本发明展示了一种用于避障路径规划的算法,通过采用包围盒技术和等效膨胀,将所述障碍物和所述机器人进行简化,生成路径点集并生成对应的路径段,对所述路径段和所述包围盒进行干涉检测,如果存在干涉则通过所述包围盒获取新的路径点,生成新的路径段,直至路径段与障碍物不存在干涉,该算法的运算量更少。

Description

一种用于避障路径规划的算法
技术领域
本发明涉及一种用于避障路径规划的算法,属于路径规划算法技术领域。
背景技术
路径规划是机器人领域中的一个重要环节,其目的是在存在障碍物的情况下,找到一条从起点到终点的无碰撞并且符合其他约束的路径。
现今广泛使用的路径规划算法有:D*算法,A*算法、快速随机树(RRT)算法及其变种、概率路线图(PRM)算法及其变种、遗传算法、蚁群算法、人工势场法等等,但上述路径规划算法的运算量较大或者搜索时间较长。
发明内容
本发明所要解决的技术问题在于提供一种用于避障路径规划的算法,该算法的运算量更少。
解决上述技术问题,本发明采用如下技术方案:
一种用于避障路径规划的算法,包括机器人和障碍物,所述机器人和障碍物所在空间为任务空间,所述算法步骤如下:
S1、获取并输入信息:获取并输入机器人运动的起点坐标和终点坐标,以及各个障碍物的顶点的坐标;
S2、简化障碍物和机器人:将机器人简化为点,为各个障碍物构建包围盒,并使包围盒进行膨胀,所述包围盒膨胀的宽度等于所述机器人的半径;
S3、生成路径点集和初始路径:生成路径点集,所述路径点集为最终用于构成路径的路径点集合,将起点坐标和终点坐标加入所述路径点集内,根据所述路径点集内的路径点生成路径段;
S4、干涉检测:对所述路径段和所述包围盒进行干涉检测;
S5、路径重规划:当所述路径段与所述包围盒存在干涉,所述路径段所在直线将包围盒切分为两个部分,分别计算两个部分所包含的顶点到路径段所在直线的欧氏距离之和,选取欧氏距离之和更小的那一部分所包含的顶点作为候选路径点,根据候选路径点到起点的距离,由小到大依次添加在终点坐标之前;
S6、将新加入的路径点连接路径点集中除临近点外的其他点,判断新的路径段是否会与所述包围盒发生干涉,是则放弃该连接,否则以该连接作为新的路径段,并将两点间的其他路径点移除,同时更新路径;将终点直接连接路径点集中除临近点外的其他点,判断新的路径段是否会与包围盒发生干涉,是则放弃该连接,否则以该连接作为新的路径段,并将两点间的其他路径点移除;
S8、判断全部路径是否与包围盒存在干涉,是则重复步骤5;否则生成无碰撞路径;
S9、路径平滑:对最终生成的无碰撞路径进行平滑处理。
采用本发明的有益效果是:
本发明中通过采用包围盒技术和等效膨胀,将所述障碍物和所述机器人进行简化,大幅度降低了计算量,同时也为重构路径提供路径点,而采用顶点来重新构件所述机器人的路径,可以使整个计算过程更加简单便捷,也能够使最终生产的路径尽可能的短,从而大幅度缩短所述机器人的行径时间,同时也减少了规划的时间,使本发明更适用于实时要求高或者硬件配置运算性能较差的使用场景,同时也可以和其他规划算法进行结合使用,以此来达到更快更优的路径规划效果。
作为优选,所述包围盒采用AABB包围盒或OBB包围盒。
作为优选,所述S2中将所述任务空间均分为多个区,对所述包围盒进行编号并记录其所处的区。
作为优选,当所述机器人的行动路线在同一平面时,所述任务空间分为4个分区;当所述机器人的行动路线为三维时,所述任务空间分为8个分区。
作为优选,所述S4中记录所述路径段所在的分区,对所述分区内的包围盒与路径段进行干涉检测。
作为优选,所述S5中当所述路径段与所述包围盒存在干涉时,计算出所述路径段与包围盒的干涉点坐标,分别计算干涉点坐标距离路径点集中除终点外最后一个点的欧式距离,选取最小的距离对应的干涉点。
作为优选,当所述包围盒存在重叠时,将重叠的包围盒进行相加的布尔运算,视为一个包围盒。
作为优选,当起点或终点位于所述包围盒范围内时,将该障碍物的顶点或包围盒的顶点作为所述新的起点或终点。
作为优选,当起点或终点位于所述包围盒范围内时,所述路径段所在直线将包围盒切分为两个部分,分别计算两个部分所包含的包围盒的顶点到路径段所在直线的欧氏距离之和,选取欧氏距离之和更小的那一部分所包含的包围盒顶点以及在包围盒边沿上的障碍物顶点作为候选路径点,分别将候选路径点与起点或终点连接为线段并对包围盒内障碍物进行干涉检测,将检测结果为无干涉的候选路径点P作为候选逃逸点,在这些候选逃逸点中选取与终点或起点的欧氏距离更小的作为逃逸点,将所述逃逸点作为新的起点或终点。
本发明的其他特点和优点将会在下面的具体实施方式、附图中详细的揭露。
附图说明
下面结合附图对本发明做进一步的说明:
图1为本发明实施例的流程简图;
图2为本发明实施例的模拟图一;
图3为本发明实施例的模拟图二;
图4为本发明实施例的模拟图三;
图5为本发明实施例的模拟图四;
图6为本发明实施例的模拟图五;
图7为本发明实施例的模拟图六;
图8为本发明实施例的模拟图七;
图9为本发明实施例的模拟图八;
图10为本发明实施例的模拟图九;
图11为本发明实施例的模拟图十。
具体实施方式
下面结合本发明实施例的附图对本发明实施例的技术方案进行解释和说明,但下述实施例仅为本发明的优选实施例,并非全部。基于实施方式中的实施例,本领域技术人员在没有做出创造性劳动的前提下所获得其他实施例,都属于本发明的保护范围。
在本发明的描述中,需要理解的是,术语“中心”、“纵向”、“横向”、“长度”、“宽度”、“厚度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“顺时针”、“逆时针”等指示的方位或位置关系为基于附图所示的方位或位置关系,仅是为了便于描述本发明和简化描述,而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作,因此不能理解为对本发明的限制。
此外,术语“第一”、“第二”仅用于描述目的,而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此,限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。在本发明的描述中, “多个”的含义是两个或两个以上,除非另有明确的限定。
在本发明中,除非另有明确的规定和限定,术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解,例如,可以是固定连接,也可以是可拆卸连接,或一体地连接;可以是机械连接,也可以是电连接;可以是直接相连,也可以通过中间媒介间接相连,可以是两个元件内部的连通。对于本领域的普通技术人员而言,可以根据具体情况理解上述术语在本发明中的具体含义。
如图1-11所示,本实施例展示了一种用于避障路径规划的算法,包括机器人和障碍物,所述机器人和障碍物所在空间为任务空间,所述算法步骤如下:
S1、获取并输入信息:获取并输入机器人运动的起点坐标和终点坐标,以及各个障碍物的顶点的坐标;
S2、简化障碍物和机器人:将机器人简化为点,为各个障碍物构建包围盒,并使包围盒进行膨胀,所述包围盒膨胀的宽度等于所述机器人的半径;
S3、生成路径点集和初始路径:生成路径点集,所述路径点集为最终用于构成路径的路径点集合,将起点坐标和终点坐标加入所述路径点集内,根据所述路径点集内的路径点生成路径段;
S4、干涉检测:对所述路径段和所述包围盒进行干涉检测;
S5、路径重规划:当所述路径段与所述包围盒存在干涉,所述路径段所在直线将包围盒切分为两个部分,分别计算两个部分所包含的顶点到路径段所在直线的欧氏距离之和,选取欧氏距离之和更小的那一部分所包含的顶点作为候选路径点,根据候选路径点到起点的距离,由小到大依次添加在终点坐标之前;
S6、路径优化:将新加入的路径点连接路径点集中除临近点外的其他点,判断新的路径段是否会与所述包围盒发生干涉,是则放弃该连接,否则以该连接作为新的路径段,并将两点间的其他路径点移除,同时更新路径;将终点直接连接路径点集中除临近点外的其他点,判断新的路径段是否会与包围盒发生干涉,是则放弃该连接,否则以该连接作为新的路径段,并将两点间的其他路径点移除;
S8、判断全部路径是否与包围盒存在干涉,是则重复步骤5;否则生成无碰撞路径;
S9、路径平滑:对最终生成的无碰撞路径进行平滑处理。
其中所述包围盒采用AABB包围盒或者OBB包围盒,本实施例中所述S2中将所述任务空间均分为多个区,对所述包围盒进行编号并记录其所处的区,当所述机器人的行动路线在同一平面时,所述任务空间分为4个分区;当所述机器人的行动路线为三维时,所述任务空间分为8个分区,在所述S4中记录所述路径段所在的分区,对所述分区内的包围盒与路径段进行干涉检测。
本实施例所述S5中当所述路径段与所述包围盒存在干涉时,计算出所述路径段与包围盒的干涉点坐标,分别计算干涉点坐标距离路径点集中除终点外最后一个点的欧式距离,选取最小的距离对应的干涉点。
当所述包围盒存在重叠时,将重叠的包围盒进行相加的布尔运算,视为一个包围盒,当起点或终点位于所述包围盒范围内时,所述路径段所在直线将包围盒切分为两个部分,分别计算两个部分所包含的包围盒的顶点到路径段所在直线的欧氏距离之和,选取欧氏距离之和更小的那一部分所包含的包围盒顶点以及在包围盒边沿上的障碍物顶点作为候选路径点,分别将候选路径点与起点或终点连接为线段并对包围盒内障碍物进行干涉检测,将检测结果为无干涉的候选路径点P作为候选逃逸点,在这些候选逃逸点中选取与终点或起点的欧氏距离更小的作为逃逸点,将所述逃逸点作为新的起点或终点。
如图2-11所示,本实施例以9*11的二维任务空间为例,所述任务空间内存在5个不同形状的障碍物,所述机器人的起点坐标为(1,1),终点坐标为(9,8),本实施例的路径规划过程如下:
步骤1:如图2所示,获取并输入信息,所述机器人的起点坐标为(1,1),终点坐标为(9,8),同时5个障碍物的顶点坐标也已知;
步骤2:如图3所示,为各个障碍物构件AABB包围盒,可以理解本实施例中采用的AABB包围盒简化障碍物,例如对于obstacle-1,求其X、Y方向上所有顶点中的最大、最小值Xmax、Xmi、Ymax、Ymi,并将X、Y值两两组合作为包围盒的顶点,当然在其他实施例中也可以采用OBB包围盒来简化所述障碍物;
步骤3:如图4所示,将机器人简化为点,同时把该机器人的半径等效于所有包围盒的膨胀宽度,本实施例中所述机器人简化后的机器人的半径为0.2,则所述包围盒的宽度等效膨胀0.2;
步骤4:如图5所示,本实施例将任务空间等分为4个分区,其中右上为1分区,左上为2分区,左下为3分区以及右下为4分区,并记录每个障碍物所在的分区;
步骤5:如图6所示,生成路径点集,将起点坐标和终点坐标加入到所述路径点集,并生成路径段,由于此时路径点集中只有起点和终点两个点,故所述路径段为连接起点和终点的直线;
步骤6:对路径段进行干涉检测,由于所述路径段经过的分区为1、2、3分区,故可以排除4分的包围盒,仅对剩余的包围盒进行干涉检测,可以减少干涉检测的计算量,从图6可知,所述路径段与障碍物obstacle-1和obstacle-2存在干涉,此时计算出所述路径段与包围盒的干涉点坐标,分别计算干涉点坐标距离路径点集中除终点外最后一个点,即距离起点的欧式距离,选取最小的距离对应的干涉点为P;
步骤7:所述干涉点P对应包围盒有ABCD四个顶点,路径段将其分割为AB和CD两个部分,分别计算两部分的两点到路径段的欧式距离之和。经计算,AB两点到路径段的欧式距离之和最小,故选取AB两点作为新的候选路径点;
根据候选路径点A、B到起点的距离,由小到大依次进行如下操作:
对于点A:路径点集中除终点外的最后一个点为起点,所述起点为O,连接起点O和点A构成路径段并对其进行干涉检测,路径段OA无干涉,将点A加入路径点集。
对点B:路径点集中除终点外的最后一个点为点A,连接点A和点B构成路径段并进行干涉检测,路径段AB无干涉,将点B加入路径点集。候选路径点判定完毕,之后更新路径点集,并生成新的路径段如图7所示;
步骤8:执行路径优化,此时无需优化;
步骤9:判断端部路径是否扔与包围盒存在干涉,从图7可知,此时B与终点的路径段与障碍物obstacle-2存在干涉,重复执行步骤6、7和8,
如图8所示,此时路径段将其分割为EF和GH两个部分,分别计算两部分的两点到路径段的欧式距离之和。经计算,EF两点到路径段的欧式距离之和最小,故选取EF两点作为新的候选路径点。
根据候选路径点E、F到起点的距离,由小到大依次进行如下操作:
对于点E:路径点集中除终点外的最后一个点为B,连接点B和点E构成路径段并对其进行干涉检测,路径段BE无干涉,将点E加入路径点集。
对点F:路径点集中除终点外的最后一个点为点E,连接点E和点F构成路径段并进行干涉检测,路径段EF无干涉,将点F加入路径点集。执行路径优化,将点F和路径点集中的O、A、B分别连接成线段,进行干涉检测,都有干涉放弃连接。将终点和路径点集中的O、A、B分别连接成线段,进行干涉检测,都有干涉放弃连接。将终点和路径点集中E点连接成线段,进行干涉检测,无干涉,此时删除路径点F,路径点集优化为O、A、B、E、终点;
如图9所示,执行路径优化,将点F和路径点集中的O、A、B分别连接成路径段,进行干涉检测,都有干涉放弃连接。将终点和路径点集中的O、A、B分别连接成路径段,进行干涉检测,都有干涉放弃连接;将终点和路径点集中E点连接成路径段,进行干涉检测,无干涉,此时删除路径点F,路径点集优化为O、A、B、E、终点;
步骤10:对最终生成的路径进行平滑处理。此时点A、B、E处转角不利于循迹运动,需要使用曲线对所有的路径点处进行重新规划,使得转角平滑,处理完后生成最终路径如,如图10和11所示。
本实施例中通过采用包围盒技术和等效膨胀,将所述障碍物和所述机器人进行简化,大幅度降低了计算量,同时也为重构路径提供路径点,而采用顶点来重新构件所述机器人的路径,可以使整个计算过程更加简单便捷,也能够使最终生产的路径尽可能的短,从而大幅度缩短所述机器人的行径时间,同时也减少了规划的时间,使本实施例更适用于实时要求高或者硬件配置运算性能较差的使用场景,同时也可以和其他规划算法进行结合使用,以此来达到更快更优的路径规划效果。
以上所述,仅为本发明的具体实施方式,但本发明的保护范围并不局限于此,熟悉该本领域的技术人员应该明白本发明包括但不限于附图和上面具体实施方式中描述的内容。任何不偏离本发明的功能和结构原理的修改都将包括在权利要求书的范围中。

Claims (8)

1.一种用于避障路径规划的算法,其特征在于:包括机器人和障碍物,所述机器人和障碍物所在空间为任务空间,所述算法步骤如下:
S1、获取并输入信息:获取并输入机器人运动的起点坐标和终点坐标,以及各个障碍物的顶点的坐标;
S2、简化障碍物和机器人:将机器人简化为点,为各个障碍物构建包围盒,所述包围盒采用AABB包围盒或OBB包围盒,并使包围盒进行膨胀,所述包围盒膨胀的宽度等于所述机器人的半径;
S3、生成路径点集和初始路径:生成路径点集,所述路径点集为最终用于构成路径的路径点集合,将起点坐标和终点坐标加入所述路径点集内,根据所述路径点集内的路径点生成路径段;
S4、干涉检测:对所述路径段和所述包围盒进行干涉检测;
S5、路径重规划:当所述路径段与所述包围盒存在干涉,所述路径段所在直线将包围盒切分为两个部分,分别计算两个部分所包含的顶点到路径段所在直线的欧氏距离之和,选取欧氏距离之和更小的那一部分所包含的顶点作为候选路径点,根据候选路径点到起点的距离,由小到大依次添加在终点坐标之前;
S6、将新加入的路径点连接路径点集中除临近点外的其他点,判断新的路径段是否会与所述包围盒发生干涉,是则放弃该连接,否则以该连接作为新的路径段,并将两点间的其他路径点移除,同时更新路径;将终点直接连接路径点集中除临近点外的其他点,判断新的路径段是否会与包围盒发生干涉,是则放弃该连接,否则以该连接作为新的路径段,并将两点间的其他路径点移除;
S8、判断全部路径是否与包围盒存在干涉,是则重复步骤5;否则生成无碰撞路径;
S9、路径平滑:对最终生成的无碰撞路径进行平滑处理。
2.根据权利要求1所述的一种用于避障路径规划的算法,其特征在于:所述S2中将所述任务空间均分为多个区,对所述包围盒进行编号并记录其所处的区。
3.根据权利要求2所述的一种用于避障路径规划的算法,其特征在于:当所述机器人的行动路线在同一平面时,所述任务空间分为4个分区;当所述机器人的行动路线为三维时,所述任务空间分为8个分区。
4.根据权利要求2所述的一种用于避障路径规划的算法,其特征在于:所述S4中记录所述路径段所在的分区,对所述分区内的包围盒与路径段进行干涉检测。
5.根据权利要求1所述的一种用于避障路径规划的算法,其特征在于:所述S5中当所述路径段与所述包围盒存在干涉时,计算出所述路径段与包围盒的干涉点坐标,分别计算干涉点坐标距离路径点集中除终点外最后一个点的欧式距离,选取最小的距离对应的干涉点。
6.根据权利要求1所述的一种用于避障路径规划的算法,其特征在于:当所述包围盒存在重叠时,将重叠的包围盒进行相加的布尔运算,视为一个包围盒。
7.根据权利要求1所述的一种用于避障路径规划的算法,其特征在于:当起点或终点位于所述包围盒范围内时,将所述障碍物的顶点或包围盒的顶点作为所述新的起点或终点。
8.根据权利要求7所述的一种用于避障路径规划的算法,其特征在于:当起点或终点位于所述包围盒范围内时,所述路径段所在直线将包围盒切分为两个部分,分别计算两个部分所包含的包围盒的顶点到路径段所在直线的欧氏距离之和,选取欧氏距离之和更小的那一部分所包含的包围盒顶点以及在包围盒边沿上的障碍物顶点作为候选路径点,分别将候选路径点与起点或终点连接为线段并对包围盒内障碍物进行干涉检测,将检测结果为无干涉的候选路径点P作为候选逃逸点,在这些候选逃逸点中选取与终点或起点的欧氏距离更小的作为逃逸点,将所述逃逸点作为新的起点或终点。
CN202111549089.3A 2021-12-17 一种用于避障路径规划的算法 Active CN114415665B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111549089.3A CN114415665B (zh) 2021-12-17 一种用于避障路径规划的算法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111549089.3A CN114415665B (zh) 2021-12-17 一种用于避障路径规划的算法

Publications (2)

Publication Number Publication Date
CN114415665A CN114415665A (zh) 2022-04-29
CN114415665B true CN114415665B (zh) 2024-11-12

Family

ID=

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110763247A (zh) * 2019-10-21 2020-02-07 上海海事大学 基于可视图和贪心算法结合的机器人路径规划方法
CN110887484A (zh) * 2019-10-14 2020-03-17 重庆邮电大学 基于改进遗传算法的移动机器人路径规划方法及存储介质

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110887484A (zh) * 2019-10-14 2020-03-17 重庆邮电大学 基于改进遗传算法的移动机器人路径规划方法及存储介质
CN110763247A (zh) * 2019-10-21 2020-02-07 上海海事大学 基于可视图和贪心算法结合的机器人路径规划方法

Similar Documents

Publication Publication Date Title
CN110823241B (zh) 基于可通行区域骨架提取的机器人路径规划方法及系统
CN112034836B (zh) 一种改进a*算法的移动机器人路径规划方法
CN110231824B (zh) 基于直线偏离度方法的智能体路径规划方法
CN108444490B (zh) 基于可视图和a*算法深度融合的机器人路径规划方法
CN107121142A (zh) 移动机器人的拓扑地图创建方法及导航方法
KR101037379B1 (ko) 거리센서로부터 얻은 주변환경의 거리정보를 바탕으로 한 이동로봇탐사시스템 및 이를 이용한 탐사방법
US8471224B2 (en) Method for determining paths of particle beams through 3D tissue volumes
CN110909961B (zh) 基于bim的室内路径查询方法及装置
CN111562787A (zh) 机器人全覆盖路径规划区域划分方法、装置、介质及设备
CN112987749A (zh) 一种智能割草机器人混合路径规划方法
CN115268448A (zh) 一种基于冲突搜索和速度障碍的多机器人路径规划方法
CN114415665B (zh) 一种用于避障路径规划的算法
CN115047880A (zh) 一种未知动态环境下机器人智能路径规划方法
CN112656307B (zh) 一种清洁方法及清洁机器人
CN112612267B (zh) 自动驾驶的路径规划方法和装置
CN111427341B (zh) 一种基于概率地图的机器人最短预期时间目标搜索方法
Soni et al. Multi-robot unknown area exploration using frontier trees
CN114754777A (zh) 一种基于地理坐标系的无人割草车的全路径覆盖规划方法
Gong et al. Multi-agent deterministic graph mapping via robot rendezvous
CN114415665A (zh) 一种用于避障路径规划的算法
Wang et al. A partitioning-based approach for robot path planning problems
CN109977455B (zh) 一种适用于带地形障碍三维空间的蚁群优化路径构建方法
CN115420296B (zh) 基于多分辨率拓扑地图的路径搜索方法及系统
CN116560360A (zh) 面向复杂动态场景的医用护理机器人实时动态路径规划方法及系统
CN115586769A (zh) 移动机器人路径规划方法和系统

Legal Events

Date Code Title Description
PB01 Publication
SE01 Entry into force of request for substantive examination
GR01 Patent grant