CN114526750B - 一种路径优化方法及装置 - Google Patents
一种路径优化方法及装置 Download PDFInfo
- Publication number
- CN114526750B CN114526750B CN202210024272.XA CN202210024272A CN114526750B CN 114526750 B CN114526750 B CN 114526750B CN 202210024272 A CN202210024272 A CN 202210024272A CN 114526750 B CN114526750 B CN 114526750B
- Authority
- CN
- China
- Prior art keywords
- node
- motion
- path
- local path
- line segment
- 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
- 238000000034 method Methods 0.000 title claims abstract description 67
- 238000005457 optimization Methods 0.000 title claims abstract description 33
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 21
- 230000008569 process Effects 0.000 claims description 22
- 238000009499 grossing Methods 0.000 claims description 14
- 238000012545 processing Methods 0.000 claims description 5
- 238000005070 sampling Methods 0.000 description 16
- 238000010586 diagram Methods 0.000 description 12
- 238000003860 storage Methods 0.000 description 8
- 238000004364 calculation method Methods 0.000 description 4
- 238000004590 computer program Methods 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 4
- 230000007306 turnover Effects 0.000 description 4
- 230000004888 barrier function Effects 0.000 description 3
- 238000001514 detection method Methods 0.000 description 3
- 238000000354 decomposition reaction Methods 0.000 description 2
- 230000001360 synchronised effect Effects 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 238000009826 distribution Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000010845 search algorithm Methods 0.000 description 1
- 238000004904 shortening Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes
-
- 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/34—Route searching; Route guidance
- G01C21/3407—Route searching; Route guidance specially adapted for specific applications
- G01C21/343—Calculating itineraries, i.e. routes leading from a starting point to a series of categorical destinations using a global route restraint, round trips, touristic trips
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
- Y02D30/00—Reducing energy consumption in communication networks
- Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks
Abstract
本申请提供了一种路径优化方法及装置,首先获取基于PRM算法生成的全局路径,将全局路径节点集合中的节点以首节点为起点,相邻三个节点为一组划分为局部路径节点集合,对当前局部路径节点集合进行节点更新后,以当前局部路径节点集合中运动节点对应的节点作为起点,生成新的局部路径节点集合,对新的局部路径节点集合进行节点更新,以此类推,直至直至最新的局部路径节点集合中包括尾节点。通过对局部路径节点结合进行节点更新,使得局部路径节点集合中的节点按顺序连接的路径缩短,并依次对之后局部路径节点集合进行节点更新,使得节点更新后的全局路径节点集合中的节点按顺序连接的路径较更新前有较大地缩短。
Description
技术领域
本申请涉及机器人路径规划领域,特别涉及为一种路径优化方法及装置。
背景技术
路径规划是运动规划的主要研究内容之一。运动规划由路径规划和轨迹规划组成,连接起点位置和终点位置的序列点或曲线称之为路径,构成路径的策略称之为路径规划。
路径规划作为机器人完成各种任务的基础,一直是研究的热点。研究人员提出了许多规划方法:如人工势场法、单元分解法、随机路标图(PRM)法、快速搜索树(RRT)法等。传统的人工势场、单元分解法需要对空间中的障碍物进行精确建模,当环境中的障碍物较为复杂时,将导致规划算法计算量较大。基于随机采样技术的PRM法可以有效解决高维空间和复杂约束中的路径规划问题。
然而PRM算法在构建路标图时,对路标节点的采样完全基于随机采样技术,使得路径搜索具有随机性,且当采样点数量不同或者步长不同时,会生成不同的路径,这样容易导致最终搜索的路径较长。
发明内容
本发明的目的在于提供一种路径优化方法及装置,能够解决基于PRM算法搜寻的路径较长的问题。
为了实现上述发明目的,本发明提供如下技术方案:
第一方面,提供一种路径优化方法,包括以下步骤:
全局路径获取步骤:获取全局路径,所述全局路径是基于PRM算法得到的,且所述全局路径由多个节点依次连接而成;
局部路径获取步骤:在全局路径节点集合中,从首节点开始以依次相邻的三个节点为一组得到局部路径节点集合,所述局部路径节点集合包括依次相邻的第一节点、第二节点和第三节点;
节点更新步骤:在第一节点和第二节点的连线上设置一运动节点,在运动节点从第一节点向第二节点移动的过程中,检测到运动节点与第三节点的连线不经过障碍物时,若运动节点不位于第一节点,在全局路径节点集合中将第二节点对应的位置更新为运动节点的位置;
循环步骤:在全局路径节点集合中从上一组局部路径节点集合的运动节点对应的节点开始以依次相邻的三个节点为一组得到新的局部路径节点集合,对所述新的局部路径节点集合执行节点更新步骤的操作,以对全局路径节点集合进行更新;以此类推,直至最新的局部路径节点集合中包括尾节点。
第二方面,提供一种路径优化装置,包括:
全局路径获取模块,用于获取全局路径,所述全局路径是基于PRM算法得到的,且所述全局路径由多个节点依次连接而成;
局部路径获取模块,用于在全局路径节点集合中,从首节点开始以依次相邻的三个节点为一组得到局部路径节点集合,所述局部路径节点集合包括依次相邻的第一节点、第二节点和第三节点;
节点更新模块,用于在第一节点和第二节点的连线上设置一运动节点,在运动节点从第一节点向第二节点移动的过程中,检测到运动节点与第三节点的连线不经过障碍物时,若运动节点不位于第一节点,在全局路径节点集合中将第二节点对应的位置更新为运动节点的位置;
循环模块,用于在全局路径节点集合中从上一组局部路径节点集合的运动节点对应的节点开始以依次相邻的三个节点为一组得到新的局部路径节点集合,对所述新的局部路径节点集合循环执行节点更新模块,以对全局路径节点集合进行更新;以此类推,直至最新的局部路径节点集合中包括尾节点。
一种路径优化方法及装置,首先获取基于PRM算法生成的全局路径,将全局路径节点集合中的节点以首节点为起点,相邻三个节点为一组划分为局部路径节点集合,对当前局部路径节点集合进行节点更新后,以当前局部路径节点集合中运动节点对应的节点作为起点,生成新的局部路径节点集合,对新的局部路径节点集合进行节点更新,以此类推,直至直至最新的局部路径节点集合中包括尾节点。通过对局部路径节点结合进行节点更新,使得局部路径节点集合中的节点按顺序连接的路径缩短,并依次对之后局部路径节点集合进行节点更新,使得节点更新后的全局路径节点集合中的节点按顺序连接的路径较更新前有较大地缩短。
附图说明
为了更清楚地说明本申请实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本申请的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
其中:
图1为一个实施例中路径优化方法的流程图;
图2为一个实施例中路径优化方法的示意图;
图3为一个实施例中路径优化方法的示意图;
图4为一个实施例中路径优化方法的示意图;
图5为一个实施例中路径优化方法的示意图;
图6为一个实施例中路径优化方法的示意图;
图7为一个实施例中路径优化方法的流程图;
图8为一个实施例中路径优化方法的示意图;
图9为一个实施例中路径优化方法的示意图;
图10为一个实施例中路径优化装置的结构框图;
图11为一个实施例中计算机设备的内部结构图。
具体实施方式
应当理解,此处所描述的具体实施例仅仅用以解释本申请,并不用于限定本申请。
下面将结合本申请的实施例中的附图,对本申请的实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本申请的一部分实施例,而不是全部的实施例。基于本申请中的实施例,本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例,都属于本申请保护的范围。
需要说明的是,本申请的说明书和权利要求书及上述附图中的术语“包括”、“包含”和“具有”以及它们任何变形,意图在于覆盖不排他的包含。例如包含了一系列步骤或单元的过程、方法、终端、产品或设备没有限定于已列出的步骤或单元,而是可选地还包括没有列出的步骤或单元,或可选地还包括对于这些过程、方法、产品或设备固有的其他步骤或单元。在本申请的权利要求书、说明书以及说明书附图中的术语,诸如“第一”和“第二”等之类的关系术语仅仅用来将一个实体/操作/对象与另一个实体/操作/对象区分开来,而不一定要求或者暗示这些实体/操作/对象之间存在任何这种实际的关系或者顺序。
在本文中提及“实施例”意味着,结合实施例描述的特定特征、结构或特性可以包含在本申请的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例,也不是与其他实施例互斥的独立的或备选的实施例。本领域技术人员显式地和隐式地理解的是,本文所描述的实施例可以与其他实施例相结合。
在机器人的路径规划问题中,常常采用概率路线图(PRM)算法进行路径规划。基于随机采样技术的PRM(Probabilistic Road Map)法可以有效避免对位姿空间中的障碍物进行精确建模,能够有效解决高维空间和复杂约束中的路径规划问题。但是由于PRM算法在生成路径时,对节点的采样完全基于随机采样技术,使得路径搜索具有随机性,且当采样点数量不同或者步长不同时,会生成不同的路径,这样容易导致最终搜索的路径较长。因此,可以对PRM算法生成的路径进行优化,缩短其路径,使得机器人运动的路径更短。
如图1所示,提出了一种路径优化方法,所述方法包括以下步骤:
全局路径获取步骤S101:终端获取全局路径,所述全局路径是基于PRM算法得到的,且所述全局路径由多个节点依次连接而成。
其中,PRM是一种基于图搜索的方法,它将连续空间转换成离散空间,再利用A*(A-Star)等搜索算法在路线图上寻找路径,以提高搜索效率。这种方法能用相对少的随机采样点来找到一个解,对多数问题而言,相对少的样本足以覆盖大部分可行的空间,并且找到路径的概率为1(随着采样数增加,P(找到一条路径)指数的趋向于1)。显然,当采样点太少,或者分布不合理时,PRM算法是不完备的,但是随着采用点的增加,也可以达到完备。所以PRM是概率完备且不最优的,因此,需要对PRM算法生成的路径进行优化。在本申请中,终端首先获取基于PRM算法生成的全局路径,全局路径是由多个节点依次连接而成的完整的路径。全局路径可以是终端自己生成的,也可以是导入终端的。终端包括计算机或平板电脑。
局部路径获取步骤S102:终端在全局路径节点集合中,从首节点开始以依次相邻的三个节点为一组得到局部路径节点集合,所述局部路径节点集合包括依次相邻的第一节点、第二节点和第三节点。
其中,全局路径节点集合是全局路径中所有节点的集合。全局路径的第一个节点为首节点,终端首先以首节点开始以依次相邻的三个节点为一组得到局部路径节点集合,局部路径节点集合是以三个相邻节点为一组组成的节点的集合,局部路径由局部路径节点集合中的节点依次连接而成。例如:全局路径节点集合依次包括A、B、C、D、E、F、G等节点,A节点为首节点,以A节点开始,将A节点和相邻的B节点、C节点为一组得到局部路径节点集合,这里的局部路径节点集合由A节点、B节点、C节点组成,这里的A节点被定义为第一节点、B节点被定义为第二节点、C节点被定义为第三节点。将全局路径节点集合依次划分为一个个局部路径节点集合,对局部路径节点集合进行后续优化处理,和直接对所有节点进行优化处理相比,本申请的方案大大减少了数据计算量,减轻了计算压力,减少了计算时间。
节点更新步骤S103:终端在第一节点和第二节点的连线上设置一运动节点,在运动节点从第一节点向第二节点移动的过程中,检测到运动节点与第三节点的连线不经过障碍物时,若运动节点不位于第一节点,在全局路径节点集合中将第二节点对应的位置更新为运动节点的位置。
其中,运动节点以第一节点为起始点,向第二运动节点移动,运动节点在移动时与第三节点连接形成连线,终端对运动节点与第三节点的连线进行碰撞检测,通过对连线进行等距采样,从中挑选出若干个相互等距的采样点,从而逐个检测其是否处于障碍物的位置。当终端检测到运动节点与第三节点的连线不经过障碍物,且运动节点不位于第一节点时,在全局路径节点集合中将第二节点对应的位置更新为运动节点的位置。例如:如图2所示,A节点为第一节点,B节点为第二节点、C节点为第三节点、D节点为运动节点,局部路径节点集合由A节点、B节点、C节点组成,当D节点与C节点的连线不经过障碍物,且运动节点不位于第一节点时,显然,此时由AD、DC组成的局部路径ADC的路径比由AB、BC组成局部路径ABC的路径更短。用D节点取代B节点,组成新的局部路径节点集合,新的局部路径节点集合由A节点、D节点、C节点组成,在全局路径节点集合中将第二节点对应的位置更新为运动节点的位置。通过对局部路径节点集合中的节点进行节点更新,更新后的局部路径比更新前的局部路径有所缩短,且不会与障碍物发生碰撞。
循环步骤S104:终端在全局路径节点集合中从上一组局部路径节点集合的运动节点对应的节点开始以依次相邻的三个节点为一组得到新的局部路径节点集合,对所述新的局部路径节点集合执行节点更新步骤S103的操作,以对全局路径节点集合进行更新;以此类推,直至最新的局部路径节点集合中包括尾节点。
其中,对局部路径节点集合中的节点更新之后,需要对全局路径节点集合中剩下的其他节点进行更新。因此终端以上一组局部路径节点集合的运动节点对应的节点开始以依次相邻的三个节点为一组得到新的局部路径节点集合,对所述新的局部路径节点集合执行节点更新步骤的操作。例如:如图3所示,第一组局部路径节点集合由A节点、B节点、C节点组成,对第一组局部路径节点集合进行更新之后的局部路径节点集合由A节点、D节点、C节点组成,其中A节点、C节点分别为第一组局部路径节点集合的第一节点、第三节点,D节点为第一组局部路径节点集合的运动节点,运动节点D节点取代第二运动节点。此时的全局路径节点集合中的节点依次为A节点、D节点、C节点、E节点,在下一个局部路径节点集合的更新中,上一组局部路径节点集合即为第一组局部路径节点集合,以D节点为起始节点,将依次相邻的D节点、C节点、E节点组成局部路径节点集合,此时的局部路径节点集合的D节点、C节点、E节点分别为该局部路径节点集合的第一节点、第二节点、第三节点。对该局部路径节点集合重复上述节点更新步骤S103,得到新的局部路径节点集合,该新的局部路径节点集合由D节点、F节点、E节点组成,其中D节点、E节点分别为该局部路径节点集合的第一节点、第三节点,F节点为该局部路径节点集合的运动节点,运动节点F节点取代原来的第二节点C节点。更新前的全局路径节点集合依次为A节点、B节点、C节点、E节点,经过两次局部路径更新之后的全局路径节点集合依次为:A节点、D节点、F节点、E节点。以此类推,直至最新的局部路径节点集合中包括尾节点,尾节点为全局路径节点集合中排最后的一个节点。通过依次对局部路径进行循环更新,使得全局路径节点集合中的节点全部更新,完成循环更新之后的全局路径相较于更新前的的全局路径有了较大缩短。
上述路径优化方法,首先获取基于PRM算法生成的全局路径,将全局路径节点集合中的节点以首节点为起点,相邻三个节点为一组划分为局部路径节点集合,对当前局部路径节点集合进行节点更新后,以当前局部路径节点集合中运动节点对应的节点作为起点,生成新的局部路径节点集合,对新的局部路径节点集合进行节点更新,以此类推,直至直至最新的局部路径节点集合中包括尾节点。通过对局部路径节点结合进行节点更新,使得局部路径节点集合中的节点按顺序连接的路径缩短,并依次对之后局部路径节点集合进行节点更新,使得节点更新后的全局路径节点集合中的节点按顺序连接的路径较更新前有较大地缩短。
在一个实施例中,所述节点更新步骤S103还包括:终端在第一节点和第二节点的连线上设置一运动节点,在运动节点从第一节点向第二节点移动的过程中,检测到运动节点与第三节点的连线不经过障碍物时,若运动节点位于第一节点,在全局路径节点集合中将第二节点删除并将第一节点对应的位置更新为运动节点的位置。
其中,在局部路径的更新中,运动节点以第一节点为起始点,向第二运动节点移动,运动节点在移动时与第三节点连接形成连线,终端对运动节点与第三节点的连线进行碰撞检测,通过对连线进行等距采样,从中挑选出若干个相互等距的采样点,从而逐个检测其是否处于障碍物的位置。当终端检测到运动节点与第三节点的连线不经过障碍物,且运动节点位于第一节点时,即代表第一节点与第三节点能直接相连而不与障碍物碰撞,此时在全局路径节点集合中将第二节点删除并将第一节点对应的位置更新为运动节点的位置。例如:如图4所示,局部路径由AB线段、BC线段组成,A节点、B节点、C节点分别为该局部路径节点集合中的第一节点、第二节点、第三节点。运动节点在第一节点时,此时的运动节点也为A点,终端连接运动节点与第三节点,对运动节点与第三节点的连线做碰撞测试,检测到连线没有经过障碍物,说明此时的运动节点(即第一节点)能直接与第三节点相连而不会与障碍物发生碰撞。因此终端将第二节点删除并将第一节点对应的位置更新为运动节点的位置。通过对局部路径节点集合中的节点进行节点更新,更新后的局部路径比更新前的局部路径有所缩短,且不会与障碍物发生碰撞。
在一个实施例中,所述节点更新步骤S103还包括:在第一节点和第二节点的连线上设置一运动节点,在运动节点从第一节点向第二节点移动的过程中,检测到运动节点与第三节点的连线上任一点在预设范围内不存在障碍物时,若运动节点不位于第一节点,在全局路径节点集合中将第二节点对应的位置更新为运动节点的位置。
其中,本申请是对PRM算法生成的路径进行优化,优化后的路径应用于机器人。由于机器人是实物,需要占据一定的空间,因此优化后的路径需要与障碍物预留一定的空间,保证机器人能顺利通过。在本实施例中,在对局部路径进行更新,优化路径长度时,将运动节点与第三节点的连线的碰撞检测的条件改为:运动节点与第三节点的连线上任一点在预设范围内不存在障碍物。可以根据实际需要设置预设范围的大小,预设范围的大小与机器人所占空间适配,使得机器人按照优化后的路径移动不会碰撞到障碍物。当运动节点与第三节点的连线满足此条件,且运动节点不位于第一节点时,在全局路径节点集合中将第二节点对应的位置更新为运动节点的位置。例如:如图5所示,更新前的局部路径由AB线段、BC线段组成,更新前的局部路径为ABC,A节点、B节点、C节点分别为该局部路径节点集合中的第一节点、第二节点、第三节点,D为运动节点,当运动节点与第三节点的连线上任一点在预设范围内不存在障碍物时,图5虚线构成的矩形区域即为预设范围。在全局路径节点集合中将第二节点对应的位置更新为运动节点的位置,即用D点取代B点。更新后的局部路径为ADC,即更新后的局部路径由A节点、D节点、C节点依次连接而成。此外,如图6所示,预设范围还可以设定为:运动节点与第三节点的连线上任一点以预设距离为半径的圆不与障碍物发生碰撞。图6虚线所示为运动节点与第三节点的连线上任一点以预设距离为半径生成的圆。预设距离根据实际需要设置,使得以预设距离为半径的圆与机器人所占空间适配,使得机器人按照优化后的路径移动不会碰撞到障碍物。通过对局部路径节点集合中的节点进行节点更新,同时限定碰撞条件,使得更新后的局部路径比更新前的局部路径有所缩短,且保证机器人不会与障碍物发生碰撞。
在一个实施例中,所述节点更新步骤S103还包括:在第一节点和第二节点的连线上设置一运动节点,在运动节点从第一节点向第二节点移动的过程中,检测到运动节点与第三节点的连线上任一点在预设范围内不存在障碍物时,若运动节点位于第一节点,在全局路径节点集合中将第二节点删除并将第一节点对应的位置更新为运动节点的位置。
其中,在对局部路径进行更新,优化路径长度时,将运动节点与第三节点的连线的碰撞检测的条件改为:运动节点与第三节点的连线上任一点在预设范围内不存在障碍物。可以根据实际需要设置预设范围的大小,预设范围的大小与机器人所占空间适配,使得机器人按照优化后的路径移动不会碰撞到障碍物。当运动节点与第三节点的连线满足此条件,且运动节点位于第一节点时,在全局路径节点集合中将第二节点删除并将第一节点对应的位置更新为运动节点的位置。通过对局部路径节点集合中的节点进行节点更新,同时限定碰撞条件,使得更新后的局部路径比更新前的局部路径有所缩短,且保证机器人不会与障碍物发生碰撞。
如图7所示,在一个实施例中,所述方法还包括:平滑处理步骤S105:当所述第一节点与第二节点的连线和第二节点与第三节点的连线之间的夹角小于第一预设角度时,终端对局部路径节点集合进行平滑处理,使得所述局部路径节点集合中按节点顺序连接的连线之间夹角都不小于第一预设角度。
其中,基于PRM算法生成的路径除了路径较长外,还会出现两条线段之间的夹角较小的情况,对于机器人来说,大部分机器人是有最大转弯角度的,如果两条线段之间的夹角过小,会使机器人无法完成转弯。在本实施例中,终端计算得到第一节点与第二节点的连线和第二节点与第三节点的连线之间的夹角,并将夹角与第一预设角度进行比较,当夹角小于第一预设角度时,终端对局部路径节点集合进行平滑处理,第一预设角度可以根据实际需要设置,第一预设角度需与机器人的转弯半径适配。通过对局部路径节点集合进行平滑处理,使得局部路径节点集合中按节点顺序连接的连线之间夹角不小于第一预设角度。通过对所有第一节点与第二节点的连线和第二节点与第三节点的连线之间的夹角小于第一预设角度的局部路径进行平滑处理,使得全局路径中两条线段之间的夹角都满足第一预设角度,进而适配机器人的转弯角度,从而使得路径更平滑,使机器人的运行更流畅。
在一个实施例中,所述平滑处理包括以下步骤:转向线段生成步骤:在所述第一节点与第二节点的连线上任选一点作为转向线段的起点,过所述起点生成与所述第一节点与第二节点的连线成第二预设角度的转向线段,所述转向线段的长度为预设长度;比较步骤:比较所述转向线段与所述转向线段的终点和所述第三节点的连线之间的夹角与第一预设角度的大小,当所述夹角小于第一预设角度时,以当前转向线段的终点作为下一转向线段的起点,重复转向线段生成步骤,以此类推,直至最终生成的转向线段与所述最终生成的转向线段的终点和第三节点的连线之间的夹角不小于第一预设角度,此时所述局部路径节点集合中按节点顺序连接的连线之间夹角都不小于第一预设角度。
其中,当出现局部路径中两条线段之间的夹角过小,即其中一条线段的延长线与另一条线段之间的夹角大于机器人的最大转弯角度时,机器人无法一次转过,从而影响机器人的前进。例如:如图8所示,在本申请中,对两条线段之间的夹角小于第一预设角度的局部路径进行平滑处理的具体步骤为:初始局部路径为ABC,A节点、B节点、C节点分别为该局部路径节点集合中的第一节点、第二节点、第三节点,由于线段AB与线段BC的夹角小于第一预设角度,使得机器人无法一次转过。第一预设角度根据机器人的最大转弯角度适应性设置,且第一预设角度加上第二预设角度大于或等于180°,在本例中,第一预设角度加上第二预设角度等于180°。在线段AB上任选一点D,过D点做与线段AB的夹角成a的转向线段DE,a为第二预设角度,第二预设角度根据机器人的最大转弯角度适应性设置,第二预设角度需小于或等于机器人的最大转弯角度,当第二预设角度等于机器人的最大转弯角度时,转向线段DE与线段EC的夹角等于第一预设角度。连接E点与C点,由于转向线段DE与线段EC的夹角小于第一预设角度,机器人还是无法转过来,因此过E点做与转向线段DE的夹角成a的转向线段EF,连接F点与C点,由于转向线段EF与线段FC的夹角小于第一预设角度,机器人还是无法转过来,继续过F点做与转向线段EF的夹角成a的线段转向FG,连接G点与C点,此时转向线段FG与线段GC的夹角大于第一预设角度,且转向线段FG的延长线与线段GC的夹角为b,b等于或小于a,因此机器人能顺利转过角b,到达C节点,此时的局部路径为ADEFGC。即转向线段与转向线段的终点和第三节点的连线之间的夹角与第一预设角度的大小,当夹角小于第一预设角度时,以当前转向线段的终点作为下一转向线段的起点,重复转向线段生成步骤,以此类推,直至最终生成的转向线段与最终生成的转向线段的终点和第三节点的连线之间的夹角不小于第一预设角度。本实施例的平滑处理能在拐角较小的局部路径中生成多个中间节点,使得任意两条相邻线段之间的夹角大于或等于第一预设角度,使得机器人能顺利经过拐角,且局部路径的起点和终点不会发生改变。
在一个实施例中,所述方法还包括:当所述局部路径节点集合中第一节点与第二节点的连线和第二节点与第三节点的连线之间的夹角大于第三预设角度时,删除第二节点,连接第一节点与第三节点。
其中,当所述局部路径节点集合中第一节点与第二节点的连线和第二节点与第三节点的连线之间的夹角大于第三预设角度时,删除第二节点。第三预设角度可以根据实际需要自行设置,例如设置成150°。为了便于理解,如图9所示,在局部路径ABC中,线段AB与线段BC之间的夹角为165°,第三预设角度为150°,线段AB与线段BC之间的夹角大于第三预设角度,因此删除第二节点,连接第一节点与第三节点,此时的局部路径为AC。由于三角形第三边小于两边之和,所以局部路径AC短于局部路径ABC。通过设置第三预设角度,删除第一节点与第二节点的连线和第二节点与第三节点的连线之间的夹角大于第三预设角度时的局部路径中的第二节点,连接第一节点与第三节点,缩短了局部路径,且不改变局部路径的起点和终点。
如图10所示,提出了一种一种路径优化装置,所述装置包括:
全局路径获取模块201,用于获取全局路径,所述全局路径是基于PRM算法得到的,且所述全局路径由多个节点依次连接而成;
局部路径获取模块202,用于在全局路径节点集合中,从首节点开始以依次相邻的三个节点为一组得到局部路径节点集合,所述局部路径节点集合包括依次相邻的第一节点、第二节点和第三节点;
节点更新模块203,用于在第一节点和第二节点的连线上设置一运动节点,在运动节点从第一节点向第二节点移动的过程中,检测到运动节点与第三节点的连线不经过障碍物时,若运动节点不位于第一节点,在全局路径节点集合中将第二节点对应的位置更新为运动节点的位置;
循环模块204,用于在全局路径节点集合中从上一组局部路径节点集合的运动节点对应的节点开始以依次相邻的三个节点为一组得到新的局部路径节点集合,对所述新的局部路径节点集合循环执行节点更新模块203,以对全局路径节点集合进行更新;以此类推,直至最新的局部路径节点集合中包括尾节点。
上述路径优化装置,首先获取基于PRM算法生成的全局路径,将全局路径节点集合中的节点以首节点为起点,相邻三个节点为一组划分为局部路径节点集合,对当前局部路径节点集合进行节点更新后,以当前局部路径节点集合中运动节点对应的节点作为起点,生成新的局部路径节点集合,对新的局部路径节点集合进行节点更新,以此类推,直至直至最新的局部路径节点集合中包括尾节点。通过对局部路径节点结合进行节点更新,使得局部路径节点集合中的节点按顺序连接的路径缩短,并依次对之后局部路径节点集合进行节点更新,使得节点更新后的全局路径节点集合中的节点按顺序连接的路径较更新前有较大地缩短。
在一个实施例中,所述节点更新模块203还用于:在第一节点和第二节点的连线上设置一运动节点,在运动节点从第一节点向第二节点移动的过程中,检测到运动节点与第三节点的连线不经过障碍物时,若运动节点位于第一节点,在全局路径节点集合中将第二节点删除并将第一节点对应的位置更新为运动节点的位置。
在一个实施例中,所述节点更新模块203还用于:在第一节点和第二节点的连线上设置一运动节点,在运动节点从第一节点向第二节点移动的过程中,检测到运动节点与第三节点的连线上任一点在预设范围内不存在障碍物时,若运动节点不位于第一节点,在全局路径节点集合中将第二节点对应的位置更新为运动节点的位置。
在一个实施例中,所述节点更新模块203还用于:在第一节点和第二节点的连线上设置一运动节点,在运动节点从第一节点向第二节点移动的过程中,检测到运动节点与第三节点的连线上任一点在预设范围内不存在障碍物时,若运动节点位于第一节点,在全局路径节点集合中将第二节点删除并将第一节点对应的位置更新为运动节点的位置。
在一个实施例中,所述装置还包括:平滑处理模块205,用于当所述第一节点与第二节点的连线和第二节点与第三节点的连线之间的夹角小于第一预设角度时,对局部路径节点集合进行平滑处理,使得所述局部路径节点集合中按节点顺序连接的连线之间夹角都不小于第一预设角度。
在一个实施例中,所述装置还包括删除模块206,用于当所述局部路径节点集合中第一节点与第二节点的连线和第二节点与第三节点的连线之间的夹角大于第二预设角度时,删除第二节点。
如图11所示,该计算机设备包括通过终端总线连接的处理器、存储器和网络接口。其中,存储器包括非易失性存储介质和内存储器。该计算机设备的非易失性存储介质有存储操作终端,还可有存储计算机程序,该计算机程序被处理器执行时,可使得处理器实现上述的路径优化方法。该内存储器中也可储存有计算机程序,该计算机程序被处理器执行时,可使得处理器执行上述的路径优化方法。本领域技术人员可以理解,图11中示出的结构,仅仅是与本申请方案相关的部分结构的框图,并不构成对本申请方案所应用于其上的设备的限定,具体的设备可以包括比图中所示更多或更少的部件,或者组合某些部件,或者具有不同的部件布置。
在一个实施例中,提出了一种计算机可读存储介质,有存储计算机程序,计算机程序被处理器执行时,使得处理器执行上述路径优化方法的步骤。
可以理解的是,上述路径优化方法、设备及存储介质属于一个总的发明构思,实施例可相互适用。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程,是可以通过计算机程序来指令相关的硬件来完成,所述的程序可存储于一非易失性计算机可读取存储介质中,该程序在执行时,可包括如上述各方法的实施例的流程。其中,本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用,均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦除可编程ROM(EEPROM)或闪存。易失性存储器可包括随机存取存储器(RAM)或者外部高速缓冲存储器。作为说明而非局限,RAM以多种形式可得,诸如静态RAM(SRAM)、动态RAM(DRAM)、同步DRAM(SDRAM)、双数据率SDRAM(DDRSDRAM)、增强型SDRAM(ESDRAM)、同步链路(Synchlink)DRAM(SLDRAM)、存储器总线(Rambus)直接RAM(RDRAM)、直接存储器总线动态RAM(DRDRAM)、以及存储器总线动态RAM(RDRAM)等。
以上实施例的各技术特征可以进行任意的组合,为使描述简洁,未对上述实施例中的各个技术特征所有可能的组合都进行描述,然而,只要这些技术特征的组合不存在矛盾,都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式,其描述较为具体和详细,但并不能因此而理解为对本申请专利范围的限制。应当指出的是,对于本领域的普通技术人员来说,在不脱离本申请构思的前提下,还可以做出若干变形和改进,这些都属于本申请的保护范围。因此,本申请专利的保护范围应以所附权利要求为准。
Claims (7)
1.一种路径优化方法,其特征在于,所述方法包括以下步骤:
全局路径获取步骤:获取全局路径,所述全局路径是基于PRM算法得到的,且所述全局路径由多个节点依次连接而成;
局部路径获取步骤:在全局路径节点集合中,从首节点开始以依次相邻的三个节点为一组得到局部路径节点集合,所述局部路径节点集合包括依次相邻的第一节点、第二节点和第三节点;
节点更新步骤:在第一节点和第二节点的连线上设置一运动节点,在运动节点从第一节点向第二节点移动的过程中,检测到运动节点与第三节点的连线不经过障碍物时,若运动节点不位于第一节点,在全局路径节点集合中将第二节点对应的位置更新为运动节点的位置;
循环步骤:在全局路径节点集合中从上一组局部路径节点集合的运动节点对应的节点开始以依次相邻的三个节点为一组得到新的局部路径节点集合,对所述新的局部路径节点集合执行节点更新步骤的操作,以对全局路径节点集合进行更新;以此类推,直至最新的局部路径节点集合中包括尾节点;
所述方法还包括:
平滑处理步骤:当所述第一节点与第二节点的连线和第二节点与第三节点的连线之间的夹角小于第一预设角度时,对局部路径节点集合进行平滑处理,使得所述局部路径节点集合中按节点顺序连接的连线之间夹角都不小于第一预设角度;
所述平滑处理包括以下步骤:
转向线段生成步骤:在所述第一节点与第二节点的连线上任选一点作为转向线段的起点,过所述起点生成与所述第一节点与第二节点的连线成第二预设角度的转向线段,所述转向线段的长度为预设长度;
比较步骤:比较所述转向线段与所述转向线段的终点和所述第三节点的连线之间的夹角与第一预设角度的大小,当所述夹角小于第一预设角度时,以当前转向线段的终点作为下一转向线段的起点,重复转向线段生成步骤,以此类推,直至最终生成的转向线段与所述最终生成的转向线段的终点和第三节点的连线之间的夹角不小于第一预设角度,此时所述局部路径节点集合中按节点顺序连接的连线之间夹角都不小于第一预设角度。
2.如权利要求1所述的路径优化方法,其特征在于,所述节点更新步骤还包括:
在第一节点和第二节点的连线上设置一运动节点,在运动节点从第一节点向第二节点移动的过程中,检测到运动节点与第三节点的连线不经过障碍物时,若运动节点位于第一节点,在全局路径节点集合中将第二节点删除并将第一节点对应的位置更新为运动节点的位置。
3.如权利要求1所述的路径优化方法,其特征在于,所述节点更新步骤还包括:
在第一节点和第二节点的连线上设置一运动节点,在运动节点从第一节点向第二节点移动的过程中,检测到运动节点与第三节点的连线上任一点在预设范围内不存在障碍物时,若运动节点不位于第一节点,在全局路径节点集合中将第二节点对应的位置更新为运动节点的位置。
4.如权利要求1所述的路径优化方法,其特征在于,所述节点更新步骤还包括:
在第一节点和第二节点的连线上设置一运动节点,在运动节点从第一节点向第二节点移动的过程中,检测到运动节点与第三节点的连线上任一点在预设范围内不存在障碍物时,若运动节点位于第一节点,在全局路径节点集合中将第二节点删除并将第一节点对应的位置更新为运动节点的位置。
5.一种路径优化装置,其特征在于,所述装置包括:
全局路径获取模块,用于获取全局路径,所述全局路径是基于PRM算法得到的,且所述全局路径由多个节点依次连接而成;
局部路径获取模块,用于在全局路径节点集合中,从首节点开始以依次相邻的三个节点为一组得到局部路径节点集合,所述局部路径节点集合包括依次相邻的第一节点、第二节点和第三节点;
节点更新模块,用于在第一节点和第二节点的连线上设置一运动节点,在运动节点从第一节点向第二节点移动的过程中,检测到运动节点与第三节点的连线不经过障碍物时,若运动节点不位于第一节点,在全局路径节点集合中将第二节点对应的位置更新为运动节点的位置;
循环模块,用于在全局路径节点集合中从上一组局部路径节点集合的运动节点对应的节点开始以依次相邻的三个节点为一组得到新的局部路径节点集合,对所述新的局部路径节点集合循环执行节点更新模块,以对全局路径节点集合进行更新;以此类推,直至最新的局部路径节点集合中包括尾节点;
还包括:
平滑处理模块:当所述第一节点与第二节点的连线和第二节点与第三节点的连线之间的夹角小于第一预设角度时,对局部路径节点集合进行平滑处理,使得所述局部路径节点集合中按节点顺序连接的连线之间夹角都不小于第一预设角度;
所述平滑处理包括以下步骤:
转向线段生成步骤:在所述第一节点与第二节点的连线上任选一点作为转向线段的起点,过所述起点生成与所述第一节点与第二节点的连线成第二预设角度的转向线段,所述转向线段的长度为预设长度;
比较步骤:比较所述转向线段与所述转向线段的终点和所述第三节点的连线之间的夹角与第一预设角度的大小,当所述夹角小于第一预设角度时,以当前转向线段的终点作为下一转向线段的起点,重复转向线段生成步骤,以此类推,直至最终生成的转向线段与所述最终生成的转向线段的终点和第三节点的连线之间的夹角不小于第一预设角度,此时所述局部路径节点集合中按节点顺序连接的连线之间夹角都不小于第一预设角度。
6.如权利要求5所述的路径优化装置,其特征在于,所述节点更新模块还用于:
在第一节点和第二节点的连线上设置一运动节点,在运动节点从第一节点向第二节点移动的过程中,检测到运动节点与第三节点的连线不经过障碍物时,若运动节点位于第一节点,在全局路径节点集合中将第二节点删除并将第一节点对应的位置更新为运动节点的位置。
7.如权利要求5所述的路径优化装置,其特征在于,所述节点更新模块还用于:
在第一节点和第二节点的连线上设置一运动节点,在运动节点从第一节点向第二节点移动的过程中,检测到运动节点与第三节点的连线上任一点在预设范围内不存在障碍物时,若运动节点不位于第一节点,在全局路径节点集合中将第二节点对应的位置更新为运动节点的位置。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210024272.XA CN114526750B (zh) | 2022-01-10 | 2022-01-10 | 一种路径优化方法及装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210024272.XA CN114526750B (zh) | 2022-01-10 | 2022-01-10 | 一种路径优化方法及装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114526750A CN114526750A (zh) | 2022-05-24 |
CN114526750B true CN114526750B (zh) | 2024-02-27 |
Family
ID=81620770
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210024272.XA Active CN114526750B (zh) | 2022-01-10 | 2022-01-10 | 一种路径优化方法及装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114526750B (zh) |
Citations (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP3349088A1 (en) * | 2017-01-13 | 2018-07-18 | Tata Consultancy Services Limited | Systems and method for turn angle constrained aerial path planning |
CN110744543A (zh) * | 2019-10-25 | 2020-02-04 | 华南理工大学 | 基于ur3机械臂的改进式prm避障运动规划方法 |
EP3644014A1 (en) * | 2018-10-23 | 2020-04-29 | Offshore Navigation Limited | An apparatus for determining an optimal route of a maritime ship |
CN111220157A (zh) * | 2020-01-10 | 2020-06-02 | 重庆康爵特智能科技有限公司 | 基于区域分割的导航路径规划方法及计算机可读存储介质 |
CN111531550A (zh) * | 2020-07-09 | 2020-08-14 | 浙江大华技术股份有限公司 | 运动规划方法及装置、存储介质、电子装置 |
CN111639811A (zh) * | 2020-06-01 | 2020-09-08 | 中国农业大学 | 基于改进蚁群算法的多农机协同作业远程管理调度方法 |
CN112286202A (zh) * | 2020-11-11 | 2021-01-29 | 福州大学 | 一种非均匀采样fmt*的移动机器人路径规划方法 |
CN112304318A (zh) * | 2020-11-10 | 2021-02-02 | 河北工业大学 | 一种虚实耦合约束环境下的机器人自主导航方法 |
CN112683278A (zh) * | 2021-01-08 | 2021-04-20 | 东南大学 | 一种基于改进a*算法和贝塞尔曲线的全局路径规划方法 |
CN112987737A (zh) * | 2021-02-26 | 2021-06-18 | 华中科技大学 | 考虑转角约束的Bi-RRT无人艇多艇导航方法及设备 |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US8262554B2 (en) * | 2007-03-30 | 2012-09-11 | Accuray Incorporated | Apparatus and method for determining an optimized path traversal for radiation treatment delivery system |
US10921816B2 (en) * | 2017-04-21 | 2021-02-16 | Korea Advanced Institute Of Science And Technology | Method and apparatus for producing map based on hierarchical structure using 2D laser scanner |
-
2022
- 2022-01-10 CN CN202210024272.XA patent/CN114526750B/zh active Active
Patent Citations (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP3349088A1 (en) * | 2017-01-13 | 2018-07-18 | Tata Consultancy Services Limited | Systems and method for turn angle constrained aerial path planning |
EP3644014A1 (en) * | 2018-10-23 | 2020-04-29 | Offshore Navigation Limited | An apparatus for determining an optimal route of a maritime ship |
CN110744543A (zh) * | 2019-10-25 | 2020-02-04 | 华南理工大学 | 基于ur3机械臂的改进式prm避障运动规划方法 |
CN111220157A (zh) * | 2020-01-10 | 2020-06-02 | 重庆康爵特智能科技有限公司 | 基于区域分割的导航路径规划方法及计算机可读存储介质 |
CN111639811A (zh) * | 2020-06-01 | 2020-09-08 | 中国农业大学 | 基于改进蚁群算法的多农机协同作业远程管理调度方法 |
CN111531550A (zh) * | 2020-07-09 | 2020-08-14 | 浙江大华技术股份有限公司 | 运动规划方法及装置、存储介质、电子装置 |
CN112304318A (zh) * | 2020-11-10 | 2021-02-02 | 河北工业大学 | 一种虚实耦合约束环境下的机器人自主导航方法 |
CN112286202A (zh) * | 2020-11-11 | 2021-01-29 | 福州大学 | 一种非均匀采样fmt*的移动机器人路径规划方法 |
CN112683278A (zh) * | 2021-01-08 | 2021-04-20 | 东南大学 | 一种基于改进a*算法和贝塞尔曲线的全局路径规划方法 |
CN112987737A (zh) * | 2021-02-26 | 2021-06-18 | 华中科技大学 | 考虑转角约束的Bi-RRT无人艇多艇导航方法及设备 |
Non-Patent Citations (4)
Title |
---|
PRM路径规划算法优化研究;夏炎;隋岩;;应用科技(10);全文 * |
夏炎;隋岩.PRM路径规划算法优化研究.应用科技.2010,(10),全文. * |
用于无人车路径规划的改进A~*算法;田海波, 李陆军, 畅科剑等;现代制造工程(第11期);第63-68页 * |
移动机器人紧急状态路径规划方法研究;孙乐乐;《中国优秀硕士学位论文全文数据库·信息科技辑》;全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN114526750A (zh) | 2022-05-24 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Erwig | The graph Voronoi diagram with applications | |
CN111504325B (zh) | 一种基于扩大搜索邻域的加权a*算法的全局路径规划方法 | |
CN113467456B (zh) | 一种未知环境下用于特定目标搜索的路径规划方法 | |
Myers | An O (ND) difference algorithm and its variations | |
CN102646070B (zh) | 一种基于区域的时空轨迹数据存储方法 | |
CN110702133B (zh) | 路径规划方法、机器人以及具有存储功能的装置 | |
CN111220157A (zh) | 基于区域分割的导航路径规划方法及计算机可读存储介质 | |
CN105527964A (zh) | 一种机器人路径规划方法 | |
CN111708364A (zh) | 一种基于a*算法改进的agv路径规划方法 | |
CN112129296A (zh) | 一种机器人轨迹规划方法及系统 | |
CN114526750B (zh) | 一种路径优化方法及装置 | |
Sarmiento et al. | An efficient strategy for rapidly finding an object in a polygonal world | |
CN115291597A (zh) | 基于双向混合a*算法的路径规划方法及装置、终端 | |
CN114536326B (zh) | 一种路标数据处理方法、装置及存储介质 | |
CN115047880A (zh) | 一种未知动态环境下机器人智能路径规划方法 | |
CN110727831A (zh) | 全路径计算方法、装置、计算机设备和存储介质 | |
CN115167388A (zh) | 一种基于目标引导的rrt多机器人编队路径规划算法 | |
CN109360215A (zh) | 三维模型的外轮廓查找方法、装置、设备和存储介质 | |
CN112711259B (zh) | 动态生成脚印集方法、装置、存储介质及双足机器人 | |
CN112393739B (zh) | 大面积环境中的改进型快速搜索随机树路径规划方法 | |
CN109597866B (zh) | 基于交通规则的道路网络中移动对象连续最近邻监视方法 | |
Zhang et al. | Incremental subspace clustering over multiple data streams | |
CN116135159A (zh) | 三维路径规划方法、装置、设备和存储介质 | |
Huang et al. | Continuous within query in road networks | |
CN114545926A (zh) | 融合人工势场和随机树的apf-rrt方法和装置 |
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 |