CN111220157B - 基于区域分割的导航路径规划方法及计算机可读存储介质 - Google Patents
基于区域分割的导航路径规划方法及计算机可读存储介质 Download PDFInfo
- Publication number
- CN111220157B CN111220157B CN202010026092.6A CN202010026092A CN111220157B CN 111220157 B CN111220157 B CN 111220157B CN 202010026092 A CN202010026092 A CN 202010026092A CN 111220157 B CN111220157 B CN 111220157B
- Authority
- CN
- China
- Prior art keywords
- path
- target
- node
- point
- feasible
- 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
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Management, Administration, Business Operations System, And Electronic Commerce (AREA)
Abstract
本发明提供了基于区域分割的导航路径规划方法及计算机可读存储介质,该基于区域分割的导航路径规划方法包括:获取高尔夫球场规划的区域,将规划的区域平均分割成上下两个区域,在每个区域内均采用PRM算法进行路径预规划,然后寻找出目标上可行点、目标下可行点,确定出目标上路径、目标下路径,并将目标上可行点、目标下可行点按照第二预设规则进行连接得到目标中路径,将目标上路径、目标中路径、以及目标下路径的路线作为第一目标路径,第一目标路径上无障碍物,完成高尔夫小车的初步路径规划,在一定程度上减少了规划时间,规划的路径长度变短,路径节点数量减少,提高了收球效率。
Description
技术领域
本发明涉及人工智能技术领域,具体地说,涉及一种基于区域分割的导航路径规划方法及计算机可读存储介质。
背景技术
伴随着贸易和经济全球化的发展,市场竞争愈演愈烈,人们对于效率的要求也越来越高。车辆路径规划问题逐渐引起人们的关注,并且成为人工智能和经济发展过程当中亟待要解决的问题。路径规划是指在某种环境内,按照一定的评价标准,如路径最短或规划时间最少等,寻找一条从起点到终点的无碰撞路径。
PRM算法是通过采样在地图上构建一张完整的无向图,再利用A*等搜索算法在路线图上寻找一条路径,以提高搜索效率。这种算法能在障碍物较少的环境下用相对较少的随机采样点找到一条路径,因此这种算法的路径搜索效率较高,然而当障碍物较多或者采样点太少,分布又不合理时,PRM算法就有可能路径规划失败,因此PRM算法是概率不完备的。
传统RRT算法是一种随机搜索且概率完备的算法,在生成新节点时因为采用随机扩展的策略,导致路径规划速度较慢,规划的路径不规则且存在大量的节点等问题,因此该算法不适合高尔夫小车进行实时路径规划,局限性较多,收球效率较低。
Bi-RRT算法是一种在传统RRT算法基础上改进的算法,该算法一开始同时从起点和终点生成两棵RRT并进行相向生长,这种双向RRT算法具有良好的搜索特性,相比传统的RRT算法,在搜索速度,搜索效率方面有了一定的提高。
为了进一步提高路径规划速度,提出了基于概率的双向RRT算法(也即P-Bi-RRT算法),该算法以固定的偏置概率向目标点方向扩展新节点,减小了算法的随机性,从而使得该算法相对于RRT算法、Bi-RRT算法加快了路径规划速度,但是由于P-Bi-RRT算法在生成新节点时依然采用一边搜索,一边采样并建图的策略,尽管在规划速度上有一定的提升,但依然具有传统RRT算法的大部分缺点,因此不能很好的应用于高尔夫小车路径规划。
现有的高尔夫小车路径规划,仍然存在规划时间长,路径长度长,路径节点数量多,绕远路,收球效率低的问题。
发明内容
本发明的目的在于提出一种基于区域分割的导航路径规划方法及计算机可读存储介质,旨在解决现有技术中,对于高尔夫小车路径规划,仍然存在规划时间长,路径长度长,路径节点数量多,绕远路,收球效率低的问题。
为解决上述技术问题,本发明提供一种基于区域分割的导航路径规划方法,包括:
获取高尔夫球场规划的区域;
将所述规划的区域平均分割成上下两个区域,起点和终点分别位于所述上下两个区域中;
在每个区域内均采用PRM算法进行路径预规划,生成上区域无向路径图、下区域无向路径图;
按照第一预设规则从所述上区域无向路径图、所述下区域无向路径图中分别确定出目标上可行点、目标下可行点;所述目标上可行点是其与起点之间的路径上无障碍物的节点,且是这些节点中路径最短的节点;所述目标下可行点是其与终点之间的路径上无障碍物的节点,且是这些节点中路径最短的节点;将所述目标上可行点与所述起点之间的路径记为目标上路径,所述目标下可行点与所述终点之间的路径记为目标下路径;
将所述目标上可行点、所述目标下可行点按照第二预设规则进行连接得到的路径记为目标中路径,所述目标中路径无障碍物,将所述目标上路径、所述目标中路径、以及所述目标下路径的路线作为第一目标路径。
可选地,所述将所述目标上可行点、所述目标下可行点按照第二预设规则进行连接得到的路径记为目标中路径包括:
先将所述目标上可行点、所述目标下可行点进行直线连接,在连接得到的路径上无障碍物的情况下,将此路径作为所述目标中路径;在连接得到的路径上有障碍物的情况下,将所述目标上可行点、所述目标下可行点采用P-Bi-RRT算法进行路径规划,得到所述目标中路径;
或者,将所述目标上可行点、所述目标下可行点直接采用所述P-Bi-RRT算法进行路径规划,得到所述目标中路径。
可选地,所述按照第一预设规则从所述上区域无向路径图、所述下区域无向路径图中分别确定出目标上可行点、目标下可行点包括:
在所述上区域无向路径图中将所述起点和与所述起点依次相连的节点划分到第一集合中,将所述终点和与所述终点依次相连的节点划分到第二集合中,然后将两个集合中的元素两两匹配,并将匹配过后的两个节点记为一对匹配点;
根据公式Q=wL1+L2计算每对匹配点的Q值,并选择Q值最小的一对匹配点作为最佳匹配点;其中w为权重常数,0<w<1,L1为利用A*算法搜索的一对匹配点分别到该节点所在无向图中起点、该节点所在无向图中终点的最短路径的长度之和,L2为匹配点间的欧式距离;
从Q值最小的一对匹配点开始按照第三预设规则进行障碍物判断,当一对匹配点通过障碍物的判断就停止选择新的一对匹配点,并将通过障碍物判断的一对匹配点分别作为所述目标上可行点、所述目标下可行点。
可选地,所述从Q值最小的一对匹配点开始按照第三预设规则进行障碍物判断包括:以一对匹配点构成一个矩形区域并将其栅格化,有障碍物代表数字0,无障碍物代表数字1,如果该矩形中有一行或者一列全为0,表明此对匹配点在这个矩形区域内无法形成一条完整的通路,将此对匹配点舍弃,并从剩下的匹配点中选择Q值最小的继续进行障碍物判断。
可选地,在所述将所述目标上路径、所述目标中路径、以及所述目标下路径的路线作为第一目标路径之后,还包括以下步骤:按照第四预设规则对所述第一目标路径进行优化,得到第二目标路径。
可选地,所述按照第四预设规则对所述第一目标路径进行优化,得到第二目标路径包括:将所述起点加入第三集合中;依次检查所述起点与所述第一目标路径上其他所有节点的连接线是否与障碍物发生碰撞;若直到所述终点都没有发生碰撞,则将所述起点和所述终点进行直线连接,以此作为所述第二目标路径;若发生碰撞,将第一节点加入所述第三集合中,所述第一节点为第一个碰撞的节点的前一个节点;在所述第一节点和与其后一个节点组成的线段上,从所述第一节点开始以预设步长扩展新节点,每扩展一个节点就判断此节点与所述起点的连接线是否与障碍物发生碰撞,当有碰撞时就停止扩展新节点,并将第二节点加入所述第三集合中,所述第二节点为发生碰撞的节点的前一个节点;然后以所述第二节点为新的起点继续检查后续所有节点的连接情况;最后将所述第三集合中的节点按照加入的先后顺序依次相连,以此作为所述第二目标路径。
可选地,在所述按照第四预设规则对所述第一目标路径进行优化,得到第二目标路径之后,还包括以下步骤:按照第五预设规则对所述第二目标路径进行优化,得到第三目标路径。
可选地,所述按照第五预设规则对所述第二目标路径进行优化,得到第三目标路径包括:将所述起点加入第四集合中;将所述第二目标路径上的节点从所述起点之后按照从小到大的顺序依次进行编号;在所述起点、编号1节点、编号2节点形成的三角形中将两短边的线段以相同的间隔等分,并将两条线段上的等分点两两相连,寻找一条长度最大且与障碍物无碰撞的线段,记为第一线段,所述第一线段的两个端点分别为所述起点、第三节点;并将所述第三节点加入所述第四集合中;在所述第三节点、编号2节点、编号3节点形成的三角形中,按照上述同样的方式寻找一条长度最大且与障碍物无碰撞的线段,记为第二线段,所述第二线段的两个端点分别为所述第三节点、第四节点;并将所述第四节点加入所述第四集合中;以此类推直到所述终点,最后将所述第四集合中的节点按照加入的先后顺序依次相连,以此作为所述第三目标路径。
可选地,还包括:在寻找的长度最大且与障碍物无碰撞的这个线段的两个端点中,若有一个端点是下一个由所述第二目标路径上的节点形成的三角形的端点的情况下,则跳过此端点,以此端点、以及此端点之后的两个节点形成的三角形中寻找一条长度最大且与障碍物无碰撞的线段。
可选地,本实施例还提供一种计算机可读存储介质,所述计算机可读存储介质存储有一个或多个程序,所述一个或多个程序可被一个或多个处理器执行,以实现上述任一项所述的基于区域分割的导航路径规划方法的步骤。
有益效果
本发明提供了一种基于区域分割的导航路径规划方法及计算机可读存储介质,该基于区域分割的导航路径规划方法包括:
获取高尔夫球场规划的区域;
将规划的区域平均分割成上下两个区域,起点和终点分别位于上下两个区域中;
在每个区域内均采用PRM算法进行路径预规划,生成上区域无向路径图、下区域无向路径图;
按照第一预设规则从上区域无向路径图、下区域无向路径图中分别确定出目标上可行点、目标下可行点;目标上可行点是其与起点之间的路径上无障碍物的节点,且是这些节点中路径最短的节点;目标下可行点是其与终点之间的路径上无障碍物的节点,且是这些节点中路径最短的节点;将目标上可行点与起点之间的路径记为目标上路径,目标下可行点与终点之间的路径记为目标下路径;
将目标上可行点、目标下可行点按照第二预设规则进行连接得到的路径记为目标中路径,目标中路径无障碍物,将目标上路径、目标中路径、以及目标下路径的路线作为第一目标路径。
通过上述方案,将规划的区域平均分割成上下两个区域,在每个区域内均采用PRM算法进行路径预规划,然后寻找出目标上可行点、目标下可行点,确定出目标上路径、目标下路径,并将目标上可行点、目标下可行点按照第二预设规则进行连接得到目标中路径,将目标上路径、目标中路径、以及目标下路径的路线作为第一目标路径,第一目标路径上无障碍物,完成高尔夫小车的初步路径规划,在一定程度上减少了规划时间,规划的路径长度变短,路径节点数量减少,提高了收球效率。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对与本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以如这些附图获得其他的附图。
图1为本发明实施例提供的一种基于区域分割的导航路径规划方法的流程图;
图2为本发明实施例提供的一种规划的第一目标路径的示意图;
图3为本发明实施例提供的一种采用传统优化方法进行路径优化的示意图;
图4为本发明实施例提供的一种第一次路径优化的示意图;
图5为本发明实施例提供的一种第二次路径优化的示意图;
图6为本发明实施例提供的一种地图1融合算法仿真图的示意图;
图7为本发明实施例提供的一种地图2融合算法仿真图的示意图;
图8为本发明实施例提供的一种地图1二次路径优化仿真图的示意图;
图9为本发明实施例提供的一种地图2二次路径优化仿真图的示意图;
图10为本发明实施例提供的一种平均路径长度对比图的示意图;
图11为本发明实施例提供的一种平均节点数量对比图的示意图;
图12为本发明实施例提供的一种四种算法平均节点数量对比图的示意图;
图13为本发明实施例提供的一种四种算法平均规划时间对比图的示意图;
图14为本发明实施例提供的一种四种算法平均路径长度对比图的示意图。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
实施例一
本实施例将提供一种基于区域分割的导航路径规划方法,参见图1,该基于区域分割的导航路径规划方法包括:
S101、获取高尔夫球场规划的区域;
S102、将规划的区域平均分割成上下两个区域,起点和终点分别位于上下两个区域中;
S103、在每个区域内均采用PRM算法进行路径预规划,生成上区域无向路径图、下区域无向路径图;
在上区域内通过PRM算法生成由若干个随机点和起点构成的无向路径图,在下区域内通过PRM算法生成由若干个随机点和终点构成的无向路径图;
采用PRM算法,可以实现用较少的节点来完成上下两个区域路径的预规划;PRM算法能在障碍物较少的环境下用相对较少的随机采样点找到一条路径,这种算法的路径搜索效率较高;
S104、按照第一预设规则从上区域无向路径图、下区域无向路径图中分别确定出目标上可行点、目标下可行点;目标上可行点是其与起点之间的路径上无障碍物的节点,且是这些节点中路径最短的节点;目标下可行点是其与终点之间的路径上无障碍物的节点,且是这些节点中路径最短的节点;将目标上可行点与起点之间的路径记为目标上路径,目标下可行点与终点之间的路径记为目标下路径;
S105、将目标上可行点、目标下可行点按照第二预设规则进行连接得到的路径记为目标中路径,目标中路径无障碍物,将目标上路径、目标中路径、以及目标下路径的路线作为第一目标路径。
通过上述方案,将规划的区域平均分割成上下两个区域,在每个区域内均采用PRM算法进行路径预规划,然后寻找出目标上可行点、目标下可行点,确定出目标上路径、目标下路径,并将目标上可行点、目标下可行点按照第二预设规则进行连接得到目标中路径,将目标上路径、目标中路径、以及目标下路径的路线作为第一目标路径,第一目标路径上无障碍物,完成高尔夫小车的初步路径规划,在一定程度上减少了规划时间,规划的路径长度变短,路径节点数量减少,提高了收球效率。
可选地,S105将目标上可行点、目标下可行点按照第二预设规则进行连接得到的路径记为目标中路径包括:先将目标上可行点、目标下可行点进行直线连接,在连接得到的路径上无障碍物的情况下,将此路径作为目标中路径;也即通过无障碍直连方法完成目标上可行点、目标下可行点之间路径的规划;在连接得到的路径上有障碍物的情况下,将目标上可行点、目标下可行点构成一个矩形搜索区域,然后以目标上可行点、目标下可行点分别为起点和终点,采用P-Bi-RRT算法进行剩余路径的路径规划,得到目标中路径;
或者,将目标上可行点、目标下可行点构成一个矩形搜索区域,然后以目标上可行点、目标下可行点分别为起点和终点,直接采用P-Bi-RRT算法进行剩余路径的路径规划,得到目标中路径。
本实施例在P-Bi-RRT算法的基础上,结合PRM算法规划时间短,节点数少和P-Bi-RRT算法具有概率完备性的特点,提出了对高尔夫小车的路径规划通过采用一种基于区域分割的PRM与P-Bi-RRT的融合算法来实现,以下简称融合算法。本实施例的路径规划采用了基于区域分割的融合算法,是对所规划的区域平均分割成两个区域,在两个区域内分别采用PRM算法实现用较少的节点来完成大部分路径的预规划,然后使用P-Bi-RRT算法或无障碍直连方法完成剩余小部分路径的规划,以此来提高规划速度和减少节点数量。
可选地,S104按照第一预设规则从上区域无向路径图、下区域无向路径图中分别确定出目标上可行点、目标下可行点包括:
在上区域无向路径图中将起点和与起点依次相连的节点划分到第一集合中,将终点和与终点依次相连的节点划分到第二集合中,然后将两个集合中的元素两两匹配,并将匹配过后的两个节点记为一对匹配点;
根据公式Q=wL1+L2计算每对匹配点的Q值,并选择Q值最小的一对匹配点作为最佳匹配点;其中w为权重常数,0<w<1,L1为利用A*算法搜索的一对匹配点分别到该节点所在无向图中起点、该节点所在无向图中终点的最短路径的长度之和,L2为匹配点间的欧式距离;由此可得当L1越小,已规划的路径越短,L2越小,匹配点间的距离越短,以匹配点构成的矩形搜索区域也就越小,规划速度也就越快。由于后续要对规划的路径进行二次优化,所以匹配点的选择更加注重路径规划速度,因此L1的权重w小于L2的权重1,而且当w趋近于0时,路径规划速度越快,反之趋近于1时越慢;
从Q值最小的一对匹配点开始按照第三预设规则进行障碍物判断,当一对匹配点通过障碍物的判断就停止选择新的一对匹配点,并将通过障碍物判断的一对匹配点分别作为目标上可行点、目标下可行点。
由于起点和终点为一对匹配点,且起点和终点这对匹配点可以通过障碍物的判断,因此该融合算法是概率完备的。
可选地,从Q值最小的一对匹配点开始按照第三预设规则进行障碍物判断包括:以一对匹配点构成一个矩形区域并将其栅格化,有障碍物代表数字0,无障碍物代表数字1,如果该矩形中有一行或者一列全为0,表明此对匹配点在这个矩形区域内无法形成一条完整的通路,将此对匹配点舍弃,并从剩下的匹配点中选择Q值最小的继续进行障碍物判断。
以下将举一个例子进行说明:
参见图2,在区域1中的所有节点,除了Xu4和Xu5,其它所有的节点都与起点Xstart依次相连,所以第一集合={Xstart,Xu1,Xu2,Xu3,Xu6,Xu7,Xu8,Xu9};在区域2中,除了Xd4,Xd5,Xd6,Xd9,其它所有的节点都与终点Xend依次相连,所以第二集合={Xd1,Xd2,Xd3,Xd7,Xd8,Xend}。将两个集合中的元素两两匹配,总共构成48个匹配点,从48个匹配点中选择Q值最小的一对匹配点进行障碍物判断,如果该对匹配点未通过障碍物判断,就从剩下的47个匹配点中选择Q值最小的一对匹配点继续进行障碍物判断,当某一对匹配点通过障碍物判断就停止选择新的一对匹配点。以一对匹配点(Xu9,Xd3)为例,利用A*算法获取从Xstart到Xu9、Xend到Xd3的最短路径分别作为目标上路径,目标下路径,如图2中的黑色粗线,计算这两条路径长度之和L1,以及Xd3和Xu9之间的欧氏距离L2,以及L1与L2之和,根据公式Q=wL1+L2求得匹配点(Xu9,Xd3)的Q值最小且通过了障碍物判断,就根据匹配点(Xu9,Xd3)规划剩余路径,如图2中的黑色虚线,最终从起点Xstart到终点Xend的一条完整路径为Xstart-Xu9-Xd3-Xd1-Xend。
可选地,本实施例可以采用传统优化方法对第一目标路径进行优化,传统优化方法是从起点开始检查与后续节点的连接线是否与障碍物发生碰撞,如没有删除此节点,反之以新的节点为起点检查后续节点的连接情况,传统方法路径优化示意图如图3。
可选地,本实施例可以采用新的优化方法对第一目标路径进行优化;可以对第一目标路径进行第一次路径优化,在S105将目标上路径、目标中路径、以及目标下路径的路线作为第一目标路径之后,还包括以下步骤:按照第四预设规则对第一目标路径进行优化,得到第二目标路径。对第一目标路径进行第一次路径优化,第二目标路径相较于第一目标路径,进一步缩短了路径长度,极大地减少了路径节点数量。
可选地,按照第四预设规则对第一目标路径进行优化,得到第二目标路径包括:将起点加入第三集合中;依次检查起点与第一目标路径上其他所有节点的连接线是否与障碍物发生碰撞;若直到终点都没有发生碰撞,则将起点和终点进行直线连接,以此作为第二目标路径;若发生碰撞,将第一节点加入第三集合中,第一节点为第一个碰撞的节点的前一个节点;在第一节点和与其后一个节点组成的线段上,从第一节点开始以预设步长扩展新节点,每扩展一个节点就判断此节点与起点的连接线是否与障碍物发生碰撞,当有碰撞时就停止扩展新节点,并将第二节点加入第三集合中,第二节点为发生碰撞的节点的前一个节点;然后以第二节点为新的起点继续检查后续所有节点的连接情况;最后将第三集合中的节点按照加入的先后顺序依次相连,以此作为第二目标路径。
以下将举一个例子对第一次路径优化进行说明:
参见图4,首先将起点qstart加入到第一次路径优化节点集合第三集合中,然后检查起点与后续所有节点的连接线是否与障碍物发生碰撞,选择最后一个与障碍物没有碰撞的节点q3,同时如果节点q3为终点,将其加入到第三集合中,算法结束,否则在q3节点和q3下一个节点q4组成的线段上,从q3节点开始以一定步长不断扩展新节点qi,每扩展一个节点就判断该节点与起点的连接线是否与障碍物发生碰撞,当有碰撞时就停止扩展新节点,如图3中的节点q9和节点qstart的连接线与障碍物发生碰撞,将q9的上一个节点q8加入到第三集合中,然后以q8为新的起点继续检查后续所有节点的连接情况,最终第一次优化路径为qstart-q8-q11-qend,也即第二目标路径为qstart-q8-q11-qend。
可选地,可以对第一目标路径进行第二次路径优化,在按照第四预设规则对第一目标路径进行优化,得到第二目标路径之后,还包括以下步骤:按照第五预设规则对第二目标路径进行优化,得到第三目标路径。对第一目标路径进行第二次路径优化,第三目标路径相较于第二目标路径,更进一步缩短了路径长度,更极大地减少了路径节点数量。
可选地,按照第五预设规则对第二目标路径进行优化,得到第三目标路径包括:将起点加入第四集合中;将第二目标路径上的节点从起点之后按照从小到大的顺序依次进行编号;在起点、编号1节点、编号2节点形成的三角形中将两短边的线段以相同的间隔等分,并将两条线段上的等分点两两相连,寻找一条长度最大且与障碍物无碰撞的线段,记为第一线段,第一线段的两个端点分别为起点、第三节点;并将第三节点加入第四集合中;在第三节点、编号2节点、编号3节点形成的三角形中,按照上述同样的方式寻找一条长度最大且与障碍物无碰撞的线段,记为第二线段,第二线段的两个端点分别为第三节点、第四节点;并将第四节点加入第四集合中;以此类推直到终点,最后将第四集合中的节点按照加入的先后顺序依次相连,以此作为第三目标路径。
可选地,还包括:在寻找的长度最大且与障碍物无碰撞的这个线段的两个端点中,若有一个端点是下一个由第二目标路径上的节点形成的三角形的端点的情况下,则跳过此端点,以此端点、以及此端点之后的两个节点形成的三角形中寻找一条长度最大且与障碍物无碰撞的线段。
以下将举一个例子对第二次路径优化进行说明:
第二次路径优化是利用三角形两边之和大于第三边的原理来减少路径长度。参见图5,从节点q1开始,在三角形△qstartq1q2中将线段和线段/>以相同的间隔等分,并将两条线段上的等分点两两相连,寻找一条长度最大且与障碍物无碰撞的线段,如线段/>同理在△q8q2q3中寻找的一条长度最大且与障碍物无碰撞的线段为/>由于线段/>的q3端点和下一个△q2q3q4的q3顶点重合,所以跳过q3节点,最后在△q3q4qend中寻找的长度最大且与障碍物无碰撞的线段为/>最终第二次优化路径为qstart-q8-q3-q12-qend,也即第三目标路径为qstart-q8-q3-q12-qend。
从图4图5可以看出,增大扩展步长不仅可以减少等分点数量,进而减少路径优化时间,而且也可以增加与障碍物间的距离,防止高尔夫小车与障碍物发生碰撞。为简述方便,以下将扩展步长和等分间隔统称为路径优化步长。
为验证后处理融合算法的优越性和实用性,本实施例还分别提供仿真和实际场景中的高尔夫小车实验,仿真中选取了两张不同障碍物形状的复杂地图,地图大小为500*500像素,起点和终点分别为(10,10),(490,490)。地图中黑色区域为障碍物,白色区域为无障碍物区域。仿真实验均在CPU为Intel Core i3-3210M,2.5GHz,内存8G的计算机上进行,编程环境为Matlab R2016a。
1.1融合算法结果与分析
图6和图7为融合算法在两张复杂地图上规划的路径,此实验过程中偏置概率0.5,搜索步长20个像素,采样点个数8个,权重w=0.1。
如图6、图7所示,大部分路径(也即黑色实线)用速度较快的PRM算法规划,剩余的小部分路径(也即黑色虚线)采用具有概率完备性的P-Bi-RRT算法或无障碍直连方法完成,因此极大地提高了路径规划速度,减少了节点数量。
1.2二次路径优化结果与分析
图8和图9为二次路径优化的结果,此实验过程中路径优化步长2个像素。图中原始路径为融合算法规划的路径。
从图8、图9中可以看出原始路径规划较为曲折冗长,经过第一次优化后,路径长度和节点数量大幅度减少,但依然存在可以优化的空间,而且当路径经过第二次优化后还可以进一步优化,但是经过大量仿真实验证明:第三次对路径进行优化,减少的长度非常有限,反而增加了较多的时间开销,所以为了实现规划时间与路径长度的均衡,本实施例只进行两次路径优化。
为验证本实施例提出的二次路径优化方法相对于传统优化方法具有一定的优势,本实施例基于地图1进行了50次实验,并从第5次开始计算两种方法优化的路径和原始路径的平均长度、平均节点数量,对比分析图如图10,图11,实验数据见表1。
表1地图1仿真实验数据对比(实验数据为50次平均值)
50次实验 | 原始路径 | 传统优化方法 | 二次路径优化方法 |
路径长度/像素 | 985.263 | 748.778 | 689.965 |
节点数量 | 30 | 20 | 15 |
从表1数据可得,二次路径优化方法(也即图10、图11中的“本文方法优化的路径”)优化的路径相对于传统优化方法优化的路径和原始路径,在平均路径长度方面,分别减少了7.812%,20.213%;在平均节点数量方面,分别减少了20%,50%,证明了二次路径优化方法相对于传统的路径优化方法更具有优势,且对原始路径的优化较为显著。
1.3后处理融合算法结果与分析
为验证后处理融合算法(也即对第一目标路径进行了二次优化)在路径节点数量,规划时间,路径长度方面的优越性,本实施例基于地图1与传统RRT算法,Bi-RRT算法,P-Bi-RRT算法进行了实验对比分析,每种算法运行50次,此实验过程中偏置概率0.5,搜索步长20个像素,采样点个数8个,路径优化步长2个像素,权重w=0.1。对比分析图如图12,图13和图14,实验数据见表2。
表2仿真实验数据对比(实验数据为50次平均值)
50次实验 | RRT | Bi-RRT | P-Bi-RRT | 后处理融合算法 |
路径长度/像素 | 1050.23 | 910.283 | 850.693 | 750.326 |
节点数量 | 69 | 51 | 40 | 15 |
规划时间/s | 26.962 | 6.325 | 4.836 | 1.523 |
从表2数据可得,后处理融合算法相对于传统RRT,Bi-RRT,P-Bi-RRT,在平均路径长度方面,分别减少了28.556%,17.572%,11.798%;在平均节点数量方面,分别减少了78.260%,70.588%,62.5%;在平均规划时间方面,分别减少了94.092%,75.073%,70.789%,证明了后处理融合算法的有效性与优越性。
1.4高尔夫小车实验
测试平台为高尔夫小车,测试环境为高尔夫球场,球场地图大小为4200*4200像素,起点为(10,10),终点为(4190,4190),地图中黑色区域为障碍物,其它为无障碍物区域。为验证后处理融合算法在实际应用时的显著优势,本实施例将后处理融合算法分别与RRT,Bi-RRT,P-Bi-RRT三种算法进行了性能对比,四种算法分别运行50次,并每次让高尔夫小车按照四种算法规划的路径进行巡航,最后计算每种算法50次平均巡航时间,此实验过程中采样点个数25个,扩展步长90个像素,偏置概率0.5,权重w=0.1。为防止高尔夫小车与障碍物相撞,路径优化步长50个像素。50次实验数据见表3。
表3仿真实验数据对比(实验数据为50次平均值)
50次实验 | RRT | Bi-RRT | P-Bi-RRT | 后处理融合算法 |
路径长度/像素 | 8836.23 | 7769.28 | 7363.693 | 6463.326 |
节点数量 | 385 | 195 | 132 | 55 |
规划时间/s | 130.462 | 51.963 | 40.862 | 11.936 |
巡航时间/s | 293.256 | 185.263 | 155.862 | 101.746 |
从表3数据可得,后处理融合算法相对于传统RRT算法,Bi-RRT算法,P-Bi-RRT算法,在平均路径长度方面,分别减少了26.854%,16.809%,12.227%;在平均节点数量方面,分别减少了85.235%,71.794%,58.333%;在平均规划时间方面,分别减少了90.085%,77.029%,72.436%;在平均巡航时间方面,分别减少了64.256%,45.236%,32.569%,因此验证了该算法更加具有实用性和优越性。
针对PRM算法和传统RRT算法及其改进算法Bi-RRT,P-Bi-RRT运用于高尔夫小车的路径规划所存在的问题,本实施例做出了以下两点改进:1、在P-Bi-RRT算法的基础上提出了一种基于区域分割的PRM与P-Bi-RRT的融合算法,提高了路径规划速度,减少了路径节点数量;2、提出了一种二次路径优化方法,缩短了路径长度,进一步减少了路径节点数量。大量仿真和实际场景中的高尔夫小车实验表明:在节点数量,规划时间,路径长度方面,后处理融合算法均优于RRT,Bi-RRT,P-Bi-RRT,经过二次路径优化的融合算法,在规划时间,路径长度,路径节点数量方面都有显著的提高,进一步缩短了路径长度,极大地减少了路径节点数量,从而在实际应用时减少了高尔夫小车的连续转向,解决了绕远路的问题,进一步提高了收球效率;因此该算法更加适合高尔夫小车进行全局路径规划,具有广泛的应用前景。
实施例二
本实施例提供一种终端,终端包括处理器、存储器及通信总线;
通信总线用于实现处理器和存储器之间的连接通信;
处理器用于执行存储器中存储的一个或者多个程序,以实现实施例一中任一项的基于区域分割的导航路径规划方法的步骤。
可选地,本实施例还提供一种计算机可读存储介质,计算机可读存储介质存储有一个或多个程序,一个或多个程序可被一个或多个处理器执行,以实现实施例一中任一项的基于区域分割的导航路径规划方法的步骤。
以上仅是本发明的优选实施方式,应当指出,对于本技术领域的普通技术人员,在不脱离本发明原理的前提下,还可以做出若干改进和补充,这些改进和补充也应视为本发明的保护范围。
Claims (9)
1.一种基于区域分割的导航路径规划方法,其特征在于,包括:
获取高尔夫球场规划的区域;
将所述规划的区域平均分割成上下两个区域,起点和终点分别位于所述上下两个 区域中;
在每个区域内均采用PRM 算法进行路径预规划,生成上区域无向路径图、下区域 无向路径图;
按照第一预设规则从所述上区域无向路径图、所述下区域无向路径图中分别确定 出目标上可行点、 目标下可行点;所述目标上可行点是其与起点之间的路径上无障碍物的节点,且是这些节点中路径最短的节点;所述目标下可行点是其与终点之间的路 径上无障碍物的节点,且是这些节点中路径最短的节点;将所述目标上可行点与所述 起点之间的路径记为目标上路径,所述目标下可行点与所述终点之间的路径记为目标 下路径;
将所述目标上可行点、所述目标下可行点按照第二预设规则进行连接得到的路径 记为目标中路径,所述目标中路径无障碍物,将所述目标上路径、所述目标中路径、 以及所述目标下路径的路线作为第一 目标路径;
所述按照第一预设规则从所述上区域无向路径图、所述下区域无向路径图中分别 确定出目标上可行点、 目标下可行点包括:
在所述上区域无向路径图中将所述起点和与所述起点依次相连的节点划分到第一 集合中,将所述终点和与所述终点依次相连的节点划分到第二集合中,然后将两个集 合中的元素两两匹配,并将匹配过后的两个节点记为一对匹配点;
根据公式 Q=wL 1 +L 2 计算每对匹配点的 Q 值,并选择 Q 值最小的一对匹配点作为最佳匹配点;其中 w 为权重常数,0<w<1,L1 为利用A*算法搜索的一对匹配点分别到该节点所在无向图中起点、该节点所在无向图中终点的最短路径的长度之和,L 2为匹配点间的欧式距离;
从 Q值最小的一对匹配点开始按照第三预设规则进行障碍物判断,当一对匹配点通过障碍物的判断就停止选择新的一对匹配点,并将通过障碍物判断的一对匹配点分 别作为所述目标上可行点、所述目标下可行点。
2.根据权利要求 1 所述的基于区域分割的导航路径规划方法,其特征在于,所述将所述目标上可行点、所述目标下可行点按照第二预设规则进行连接得到的路径记为 目标中路径包括:
先将所述目标上可行点、所述目标下可行点进行直线连接,在连接得到的路径上 无障碍物的情况下,将此路径作为所述目标中路径;在连接得到的路径上有障碍物的 情况下,将所述目标上可行点、所述目标下可行点采用 P-Bi-RRT 算法进行路径规划,得到所述目标中路径;
或者,将所述目标上可行点、所述目标下可行点直接采用所述 P-Bi-RRT 算法进行路径规划,得到所述目标中路径。
3.根据权利要求 2 所述的基于区域分割的导航路径规划方法,其特征在于,所述从 Q值最小的一对匹配点开始按照第三预设规则进行障碍物判断包括:以一对匹配点构成一个矩形区域并将其栅格化,有障碍物代表数字 0,无障碍物代表数字 1,如果该 矩形中有一行或者一列全为 0,表明此对匹配点在这个矩形区域内无法形成一条完整的通路,将此对匹配点舍弃,并从剩下的匹配点中选择 Q 值最小的继续进行障碍物判断。
4.根据权利要求 1 或 2 所述的基于区域分割的导航路径规划方法,其特征在于,在所述将所述目标上路径、所述目标中路径、以及所述目标下路径的路线作为第一 目标路径之后,还包括以下步骤:按照第四预设规则对所述第一 目标路径进行优化,得到第二目标路径。
5.根据权利要求 4 所述的基于区域分割的导航路径规划方法,其特征在于,所述按照第四预设规则对所述第一 目标路径进行优化,得到第二目标路径包括:将所述起点加入第三集合中;依次检查所述起点与所述第一 目标路径上其他所有节点的连接线是否与障碍物发生碰撞;若直到所述终点都没有发生碰撞,则将所述起点和所述终点 进行直线连接,以此作为所述第二目标路径;若发生碰撞,将第一节点加入所述第三 集合中,所述第一节点为第一个碰撞的节点的前一个节点;在所述第一节点和与其后 一个节点组成的线段上,从所述第一节点开始以预设步长扩展新节点,每扩展一个节 点就判断此节点与所述起点的连接线是否与障碍物发生碰撞,当有碰撞时就停止扩展 新节点,并将第二节点加入所述第三集合中,所述第二节点为发生碰撞的节点的前一 个节点;然后以所述第二节点为新的起点继续检查后续所有节点的连接情况;最后将 所述第三集合中的节点按照加入的先后顺序依次相连,以此作为所述第二目标路径。
6.根据权利要求 4 所述的基于区域分割的导航路径规划方法,其特征在于,在所述按照第四预设规则对所述第一 目标路径进行优化,得到第二目标路径之后,还包括以下步骤:按照第五预设规则对所述第二目标路径进行优化,得到第三目标路径。
7.根据权利要求 6 所述的基于区域分割的导航路径规划方法,其特征在于,所述按照第五预设规则对所述第二目标路径进行优化,得到第三目标路径包括:将所述起 点加入第四集合中;将所述第二目标路径上的节点从所述起点之后按照从小到大的顺序依次进行编号;在所述起点、编号 1 节点、编号 2 节点形成的三角形中将两短边的线段以相同的间隔等分,并将两条线段上的等分点两两相连,寻找一条长度最大且与 障碍物无碰撞的线段,记为第一线段,所述第一线段的两个端点分别为所述起点、第 三节点;并将所述第三节点加入所述第四集合中;在所述第三节点、编号 2 节点、编号 3 节点形成的三角形中,按照同样的方式寻找一条长度最大且与障碍物无碰撞的线段,记为第二线段,所述第二线段的两个端点分别为所述第三节点、第四节点; 并将所述第四节点加入所述第四集合中;以此类推直到所述终点,最后将所述第四集 合中的节点按照加入的先后顺序依次相连,以此作为所述第三目标路径。
8.根据权利要求 7 所述的基于区域分割的导航路径规划方法,其特征在于,还包括:在寻找的长度最大且与障碍物无碰撞的这个线段的两个端点中,若有一个端点是 下一个由所述第二目标路径上的节点形成的三角形的端点的情况下,则跳过此端点, 以此端点、以及此端点之后的两个节点形成的三角形中寻找一条长度最大且与障碍物 无碰撞的线段。
9.一种计算机可读存储介质,其特征在于,所述计算机可读存储介质存储有一个 或多个程序,所述一个或多个程序可被一个或多个处理器执行,以实现如权利要求 1至8 中任一项所述的基于区域分割的导航路径规划方法的步骤。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010026092.6A CN111220157B (zh) | 2020-01-10 | 2020-01-10 | 基于区域分割的导航路径规划方法及计算机可读存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010026092.6A CN111220157B (zh) | 2020-01-10 | 2020-01-10 | 基于区域分割的导航路径规划方法及计算机可读存储介质 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111220157A CN111220157A (zh) | 2020-06-02 |
CN111220157B true CN111220157B (zh) | 2023-10-03 |
Family
ID=70829360
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010026092.6A Active CN111220157B (zh) | 2020-01-10 | 2020-01-10 | 基于区域分割的导航路径规划方法及计算机可读存储介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111220157B (zh) |
Families Citing this family (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114234988A (zh) * | 2020-09-09 | 2022-03-25 | 华为技术有限公司 | 一种导航方法、设备及系统 |
US20220163969A1 (en) * | 2020-11-20 | 2022-05-26 | Rapyuta Robotics Co., Ltd. | Systems and methods for optimizing route plans in an operating environment |
CN113465621B (zh) * | 2021-06-22 | 2022-09-20 | 同济大学 | 一种考虑冲突概率的Dijkstra路径规划方法、装置及存储介质 |
CN113551685B (zh) * | 2021-07-30 | 2022-08-26 | 重庆大学 | 用于双时变路网的多偏好路线规划方法 |
CN113932825B (zh) * | 2021-09-30 | 2024-04-09 | 深圳市普渡科技有限公司 | 机器人导航路径宽度获取系统、方法、机器人及存储介质 |
CN113885531A (zh) * | 2021-11-05 | 2022-01-04 | 上海肇观电子科技有限公司 | 用于移动机器人的方法、移动机器人、电路、介质和程序 |
CN114526750B (zh) * | 2022-01-10 | 2024-02-27 | 深圳市灵星雨科技开发有限公司 | 一种路径优化方法及装置 |
CN114442626B (zh) * | 2022-01-24 | 2024-04-26 | 深圳市拓普智造科技有限公司 | 一种全局路径规划方法 |
CN114200965A (zh) * | 2022-02-21 | 2022-03-18 | 中国民航大学 | 一种飞机安全移动路径规划方法 |
CN116777095B (zh) * | 2023-06-30 | 2024-04-12 | 中国南方航空股份有限公司 | 一种航路规划方法、装置、设备及介质 |
CN117109625B (zh) * | 2023-10-20 | 2024-01-16 | 湖南大学 | 一种基于改进prm算法的无人驾驶路径规划方法 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH0914985A (ja) * | 1995-06-30 | 1997-01-17 | Hitachi Ltd | 経路探索方法および装置 |
JP2007240281A (ja) * | 2006-03-07 | 2007-09-20 | Navitime Japan Co Ltd | エリア分割巡回経路探索システム、経路探索サーバ、エリア分割巡回経路探索方法 |
CN101963510A (zh) * | 2010-10-26 | 2011-02-02 | 广东威创视讯科技股份有限公司 | 一种随机路线图的快速路径规划方法及增强方法 |
CN106643733A (zh) * | 2016-12-30 | 2017-05-10 | 北京理工大学 | 面向多导航站接力导航的运动体路径规划方法 |
CN110084393A (zh) * | 2018-01-26 | 2019-08-02 | 北京搜狗科技发展有限公司 | 一种路径信息的处理方法、装置及电子设备 |
-
2020
- 2020-01-10 CN CN202010026092.6A patent/CN111220157B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH0914985A (ja) * | 1995-06-30 | 1997-01-17 | Hitachi Ltd | 経路探索方法および装置 |
JP2007240281A (ja) * | 2006-03-07 | 2007-09-20 | Navitime Japan Co Ltd | エリア分割巡回経路探索システム、経路探索サーバ、エリア分割巡回経路探索方法 |
CN101963510A (zh) * | 2010-10-26 | 2011-02-02 | 广东威创视讯科技股份有限公司 | 一种随机路线图的快速路径规划方法及增强方法 |
CN106643733A (zh) * | 2016-12-30 | 2017-05-10 | 北京理工大学 | 面向多导航站接力导航的运动体路径规划方法 |
CN110084393A (zh) * | 2018-01-26 | 2019-08-02 | 北京搜狗科技发展有限公司 | 一种路径信息的处理方法、装置及电子设备 |
Non-Patent Citations (1)
Title |
---|
王俭 ; 赵鹤鸣 ; 肖金球 ; .基于区域优化分割的机器人全覆盖路径规划.计算机工程与应用.2006,(22),全文. * |
Also Published As
Publication number | Publication date |
---|---|
CN111220157A (zh) | 2020-06-02 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111220157B (zh) | 基于区域分割的导航路径规划方法及计算机可读存储介质 | |
US11182624B2 (en) | Method, system and memory for constructing transverse topological relationship of lanes in high-definition map | |
CN109579854B (zh) | 基于快速扩展随机树的无人车避障方法 | |
CN108762270B (zh) | 变概率双向快速搜索随机树改进路径规划算法 | |
CN112455445A (zh) | 自动驾驶的变道决策方法、装置和车辆 | |
CN113359775B (zh) | 一种动态变采样区域rrt无人车路径规划方法 | |
Xu et al. | A fast path planning algorithm fusing prm and p-bi-rrt | |
CN112683290A (zh) | 一种车辆轨迹规划方法、电子设备及计算机可读存储介质 | |
KR20210121595A (ko) | 최적 경로 탐색 장치 및 이의 동작 방법 | |
CN113124891B (zh) | 一种行驶路径规划方法及相关装置 | |
CN111337047B (zh) | 基于多任务点约束的非结构化道路宏观路径规划方法 | |
CN113701777B (zh) | 基于空间向量的高精地图车道关联轨迹线自动生成方法 | |
CN114705196A (zh) | 一种用于机器人的自适应启发式全局路径规划方法与系统 | |
CN115248042A (zh) | 一种清扫路径的规划方法及装置 | |
CN117553818A (zh) | 一种基于改进a*算法的路径规划方法及系统 | |
CN116414139B (zh) | 基于A-Star算法的移动机器人复杂路径规划方法 | |
CN116817947B (zh) | 一种基于变步长机制的任意时路径规划方法 | |
CN109855640B (zh) | 一种基于自由空间与人工蜂群算法的路径规划方法 | |
CN115223389B (zh) | 一种基于动态路段代价的停车引导路径规划方法 | |
CN113341976B (zh) | 基于锚定效应的新能源汽车混合蚁群路径规划方法 | |
Zhang et al. | An improved dynamic step size RRT algorithm in complex environments | |
CN115741686A (zh) | 一种基于变概率约束采样的机器人路径规划方法 | |
Bai et al. | Multi-density clustering based hierarchical path planning | |
CN115639827B (zh) | 一种结构化环境下机器人路径规划方法和系统 | |
CN118089738A (zh) | 适用于煤矿井下复杂巷道场景的快速路径规划方法、装置 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |