CN112393728A - 一种基于a*算法和rrt*算法的移动机器人路径规划方法 - Google Patents

一种基于a*算法和rrt*算法的移动机器人路径规划方法 Download PDF

Info

Publication number
CN112393728A
CN112393728A CN202011145680.8A CN202011145680A CN112393728A CN 112393728 A CN112393728 A CN 112393728A CN 202011145680 A CN202011145680 A CN 202011145680A CN 112393728 A CN112393728 A CN 112393728A
Authority
CN
China
Prior art keywords
node
algorithm
path
planning
coordinate
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.)
Granted
Application number
CN202011145680.8A
Other languages
English (en)
Other versions
CN112393728B (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.)
Zhejiang University of Technology ZJUT
Original Assignee
Zhejiang University of Technology ZJUT
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Zhejiang University of Technology ZJUT filed Critical Zhejiang University of Technology ZJUT
Priority to CN202011145680.8A priority Critical patent/CN112393728B/zh
Publication of CN112393728A publication Critical patent/CN112393728A/zh
Application granted granted Critical
Publication of CN112393728B publication Critical patent/CN112393728B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20Instruments 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)
  • Manipulator (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种基于A*算法和RRT*算法的移动机器人路径规划方法,包括:取移动机器人本次运动的起点坐标和终点坐标,将连接起点坐标和终点坐标的线段离散成N个点;根据离散得到的N个点进行路径规划;根据路径规划后得到的路径中的控制节点控制移动机器人由起点坐标运动至终点坐标。本发明根据障碍物所在范围,采用A*算法和RRT*算法RRT*算法融合的方式,极大的降低了RRT*算法计算代价过高,且随机性过强的问题,有效加速路径规划,并且规划最优路径的可靠性高。

Description

一种基于A*算法和RRT*算法的移动机器人路径规划方法
技术领域
本申请属于机器人导航技术领域,具体涉及一种基于A*算法和RRT*算法的移动机器人路径规划方法。
背景技术
移动机器人的路径规划是指机器人在获取到当前环境的地图之后,规划出一条无碰撞的从起点到终点的最优路径。移动机器人的路径规划在各行业的机器人作业中尤为重要。
随着各行业智能化的普及,机器人作业的应用越来越多,因此路径规划问题成为了一个热点探讨问题。常用的路径规划方法有基于采样的规划、图搜索和组合规划等。基于采样的规划中有基于RRT(快速扩展随机树)的路径规划。RRT算法是一种采用增量方式增长的随机采样算法,用于解决有代数约束(障碍带来的)和微分约束(非完整性和动态环境带来的)高维空间问题。
RRT算法的优势在于无需对系统进行建模,无需对搜索区域进行几何划分,在搜索空间的覆盖率高,搜索的范围广,可以尽可能的探索未知区域。但是RRT算法存在计算代价过高,随机性太强等问题,这导致基于RRT的路径规划无法每次均得到最优路径,在路径规划中存在的随机性过强,造成移动机器人作业效率不高、可靠性低,并且路径规划过程过慢,实时性不高。
发明内容
本申请的目的在于提供一种基于A*算法和RRT*算法的移动机器人路径规划方法,有效加速路径规划,并且规划最优路径的可靠性高。
为实现上述目的,本申请所采取的技术方案为:
一种基于A*算法和RRT*算法的移动机器人路径规划方法,所述基于A*算法和RRT*算法的移动机器人路径规划方法,包括:
步骤S1、取移动机器人本次运动的起点坐标和终点坐标,将连接起点坐标和终点坐标的线段离散成N个点;
步骤S2、根据离散得到的N个点进行路径规划,包括:
若N个点均不在障碍物所在范围内,则采用RRT*算法进行全段路径规划;
或者,若N个点中,由起点坐标起的前floor(N/2)个点中存在1个或多个点位于障碍物所在范围内,后N-floor(N/2)个点均不在障碍物所在范围内,则先采用A*算法,以起点坐标作为A*算法的规划起始坐标,以后N-floor(N/2)个点中的第一个不在障碍物所在范围内的点作为A*算法的规划目标坐标进行前段路径规划,后采用RRT*算法,以后N-floor(N/2)个点中的第一个不在障碍物所在范围内的点作为RRT*算法的规划起始坐标,以终点坐标作为RRT*算法的规划目标坐标进行后段路径规划;其中floor()为向下取整函数;
或者,若N个点中,由起点坐标起的前floor(N/2)个点均不在障碍物所在范围内,后N-floor(N/2)个点中存在1个或多个点位于障碍物所在范围内,则先采用RRT*算法,以起点坐标作为RRT*算法的规划起始坐标,以后N-floor(N/2)个点中的第一个不在障碍物所在范围内的点作为RRT*算法的规划目标坐标进行前段路径规划,后采用A*算法,以后N-floor(N/2)个点中的第一个不在障碍物所在范围内的点作为A*算法的规划起始坐标,以终点坐标作为A*算法的规划目标坐标进行后段路径规划;
或者,若N个点中,由起点坐标起的前floor(N/2)个点中存在1个或多点在障碍物所在范围内,后N-floor(N/2)个点中也存在1个或多个点在障碍物所在范围内,则采用A*算法进行全段路径规划;
步骤S3、根据路径规划后得到的路径中的控制节点控制移动机器人由起点坐标运动至终点坐标。
以下还提供了若干可选方式,但并不作为对上述总体方案的额外限定,仅仅是进一步的增补或优选,在没有技术或逻辑矛盾的前提下,各可选方式可单独针对上述总体方案进行组合,还可以是多个可选方式之间进行组合。
作为优选,所述步骤S3,根据路径规划后得到的路径中的控制节点控制移动机器人由起点坐标运动至终点坐标,包括:
步骤S3.1、取全段路径规划得到的全段路径中的控制节点;
或者,整合前段路径规划得到的前段路径和后段路径规划得到的后段路径生成全段路径,取全段路径中的控制节点;
步骤S3.2、将所有控制节点按照由起点坐标至终点坐标的方向,两两以父节点至子节点的顺序保存在一个数组中;
步骤S3.3、取数组中的第一个控制节点作为第一节点,取数组中的最后一个控制节点作为第二节点;
步骤S3.4、判断第一节点和第二节点的连线是否穿过障碍物所在范围,若连线未穿过障碍物所在范围,则仅保留第一节点和第二节点,丢弃第一节点和第二节点之间的其余控制节点,并执行步骤S3.5;若连线穿过障碍物所在范围,则找到第二节点的父节点作为新的第二节点,并重新执行步骤S3.4;
步骤S3.5、判断当前找到的第二节点是否是数组中的最后一个控制节点,若是则执行步骤S3.6;否则以当前找到的第二节点作为新的第一节点,以数组中的最后一个控制节点作为新的第二节点重新执行步骤S3.4;
步骤S3.6、取最终保留的控制节点,作为移动机器人运动中的局部目标点,控制移动机器人由起点坐标运动至终点坐标。
作为优选,所述A*算法,包括:
1)将规划起始坐标作为起始节点a,将规划目标坐标作为终节点b,从起始节点a开始,把起始节点a作为待处理的方格,存入一个open表中,open表存放的是待检查的节点;
2)寻找起始节点a相邻并可以直接到达的节点,将寻找到的节点都放入open表中,并将寻找到的节点的父节点设置为起始节点a;
3)从open表中删除起始节点a,并将起始节点a加入close表中,close表中存放的是不需要再次检查的节点;
4)从open表中找出F值最小的节点c,其中F值的计算如下:
F=G+H
其中G表示从起始节点a移动到当前计算节点的移动代价,H表示从当前计算节点到终节点的估值代价;
5)把节点c从open表中删除,放入close表中;
6)检查节点c所有相邻并且可以直接到达的节点,若所述节点还不在open表中,则将所述节点加入open表,并将所述节点的父节点设置为节点c;
7)如果节点c所有相邻并且可以直接到达的节点中的部分节点已经在open表中,则计算已经在open表中的节点新的路径的G值,若G值更低则将当前计算节点的父节点设置为节点c,并重新计算当前计算节点的F值;否则不作处理;
8)重复步骤4)~7)直至将终节点加入open表,说明路径已经找到,路径规划完成;或者重复步骤4)~7)直至查找终节点失败,并且open表为空,表明不存在路径,路径规划完成。
作为优选,所述RRT*算法,包括:
1)在规划起始坐标和规划目标坐标之间基于RRT算法找到一条可行的初始路径;
2)以规划起始坐标和规划目标坐标作为椭圆的两个焦点,以所述初始路径的总长度为椭圆上的点到两个焦点的距离之和,构建一个椭圆;
3)在所述椭圆中进行重采样,并对重采样得到的节点重新选择父节点;
4)为随机树进行重新布线。
作为优选,所述在椭圆中进行重采样,包括:
3.1)连接椭圆的两个顶点得到线段l1,将线段l1进行M等分,记M-1个等分点为m0,m1,…mM-3,mM-2
3.2)分别过各等分点做线段l1的垂线,记垂线交于椭圆的上半部分的交点为
Figure BDA0002739620150000041
并保存在数组v1中,记垂线交于椭圆的下半部分的交点为
Figure BDA0002739620150000042
并保存在数组v2中;
3.3)判断数组v1和v2中的交点在障碍物所在范围内的情况,选取数组v1和v2中交点在障碍物所在范围内较少的一组所对应的一半椭圆作为重采样区域,在所述重采样区域中进行重采样。
本申请提供的基于A*算法和RRT*算法的移动机器人路径规划方法,根据障碍物所在范围,采用A*算法和RRT*算法RRT*算法融合的方式,极大的降低了RRT*算法计算代价过高,且随机性过强的问题,有效加速路径规划,并且规划最优路径的可靠性高。
附图说明
图1为本申请的基于A*算法和RRT*算法的移动机器人路径规划方法的流程图;
图2为本申请的根据离散得到的N个点进行路径规划的流程图;
图3为本申请的RRT*算法中在椭圆中进行重采样过程的示意图。
具体实施方式
下面将结合本申请实施例中的附图,对本申请实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅是本申请一部分实施例,而不是全部的实施例。基于本申请中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本申请保护的范围。
除非另有定义,本文所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同。本文中在本申请的说明书中所使用的术语只是为了描述具体的实施例的目的,不是在于限制本申请。
其中一个实施例中,提供一种基于A*算法(A星算法)和RRT*算法(RRT星算法)的移动机器人路径规划方法,将A*算法在遇到障碍物时能够快速绕开的特性和RRT*算法在自由空间中能够快速搜索的特性相结合,通过判断起点和终点之间的障碍物情况选择合适的路径规划算法,从而有效加速路径规划,提供规划最优路径的可靠性。
如图1所示,本实施例中基于A*算法和RRT*算法的移动机器人路径规划方法,包括以下步骤:
步骤S1、取移动机器人本次运动的起点坐标和终点坐标,将连接起点坐标和终点坐标的线段离散成N个点。
在路径规划之前通过前期的定位和建图获得当前环境的地图,对周边的障碍物进行膨胀后得到正确的地图。在地图上指定移动机器人本次运动的终点坐标,同时在机器人启动底盘驱动和开启导航功能时,获取本次运动的起点坐标。
容易理解的是,本申请重点在于路径规划,对于路径规划前期的环境地图建模、障碍物膨胀等操作不进行限制,基于现有技术实现即可。
在离散线段时,可以设置固定的离散点的个数,也可以根据线段的长度决定具体的离散点的个数,例如移动机器人底盘直径d=0.4米,起点坐标和终点坐标之间的距离为Dis(start,goal),则离散点的个数可以是
Figure BDA0002739620150000051
其中Dis()为距离函数,ceil()为向上取整函数。根据线段长度确定离散点的个数的方法可得到适宜的离散点,有利于后期路径规划时确定最佳的规划算法。
需要说明的是,判断点是否在障碍物所在范围内为避障领域较为常规的技术,本实施例基于现有的判断方法实现,例如将机器人所在的代价地图中的坐标转换成栅格地图中的坐标,栅格地图中的每个坐标保存了该栅格的占据情况,只要判断离散点在栅格地图中占据情况就能判断出该离散点是否在障碍物中。当然也可以采用其他现有方式进行判断,这里就不进行限制。
步骤S2、根据离散得到的N个点进行路径规划,如图2所示,包括:
1)若N个点均不在障碍物所在范围内,则采用RRT*算法进行全段路径规划。规划时使用机器人的最大安全距离作为RRT*算法的生长步长,取较大的贪心程度p=0.8,这样既满足了RRT算法的快速搜索效率,又保留了探索未知区域的可能性。
2)或者,若N个点中,由起点坐标起的前floor(N/2)个点中存在1个或多个点位于障碍物所在范围内,后N-floor(N/2)个点均不在障碍物所在范围内。
该情况下认定机器人的起始位置距障碍物较近,则先采用A*算法,以起点坐标作为A*算法的规划起始坐标,以后N-floor(N/2)个点中的第一个不在障碍物所在范围内的点作为A*算法的规划目标坐标进行前段路径规划,以“逃离”障碍物;后采用RRT*算法,以后N-floor(N/2)个点中的第一个不在障碍物所在范围内的点作为RRT*算法的规划起始坐标,以终点坐标作为RRT*算法的规划目标坐标进行后段路径规划,以快速规划出后半段路径;其中floor()为向下取整函数。
3)或者,若N个点中,由起点坐标起的前floor(N/2)个点均不在障碍物所在范围内,后N-floor(N/2)个点中存在1个或多个点位于障碍物所在范围内。
该情况下认定机器人的起始位置距离障碍物较远,但是后半程中有障碍物阻挡,则先采用RRT*算法,以起点坐标作为RRT*算法的规划起始坐标,以后N-floor(N/2)个点中的第一个不在障碍物所在范围内的点作为RRT*算法的规划目标坐标进行前段路径规划,以快速完成前半段路径规划,快速靠近障碍物;后采用A*算法,以后N-floor(N/2)个点中的第一个不在障碍物所在范围内的点作为A*算法的规划起始坐标,以终点坐标作为A*算法的规划目标坐标进行后段路径规划,以绕开障碍物后向终点移动。
4)或者,若N个点中,由起点坐标起的前floor(N/2)个点中存在1个或多点在障碍物所在范围内,后N-floor(N/2)个点中也存在1个或多个点在障碍物所在范围内。
该情况下认定此次路线中障碍物较多,使用RRT*算法会消耗更多的时间在新生成节点的障碍物判断中,因此该情况下宜采用A*算法进行全段路径规划,在保证躲避障碍物的前提下,规划出一条最优路径。
本实施例相比于传统的只采用一种路径规划算法,结合了A*算法中能找到最优路径、避障能力强等优点和RRT*算法中搜索效率高、覆盖空间广等优点,即满足了快速搜索路径的要求,又保留了很强的避障能力。
步骤S3、根据路径规划后得到的路径中的控制节点控制移动机器人由起点坐标运动至终点坐标。
在找到一条从起点坐标到终点坐标的路径之后,不管是A*算法还是RRT*算法,都包含很多的控制节点。在机器人的实际控制中,是将这些控制节点当作一个个局部目标点,计算出机器人的运行速度和角度,发送给底盘。
若将路径规划得到的所有控制节点均作为一个个局部目标点,那将导致移动机器人的运动控制十分没有效率,而且机器人很容易出现卡顿的情况。
因此本申请在一实施例中采用节点回溯的方式,剔除冗余的点,只保留最终路径中的个别控制节点,作为机器人的局部目标点,具体过程如下:
步骤S3.1、取全段路径规划得到的全段路径中的控制节点;
或者,整合前段路径规划得到的前段路径和后段路径规划得到的后段路径生成全段路径,取全段路径中的控制节点。
步骤S3.2、将所有控制节点按照由起点坐标至终点坐标的方向,两两以父节点至子节点的顺序保存在一个数组中。
步骤S3.3、取数组中的第一个控制节点作为第一节点,取数组中的最后一个控制节点作为第二节点。
步骤S3.4、判断第一节点和第二节点的连线是否穿过障碍物所在范围,若连线未穿过障碍物所在范围,则仅保留第一节点和第二节点,丢弃第一节点和第二节点之间的其余控制节点,并执行步骤S3.5;若连线穿过障碍物所在范围,则找到第二节点的父节点作为新的第二节点,并重新执行步骤S3.4。
步骤S3.5、判断当前找到的第二节点是否是数组中的最后一个控制节点,若是则执行步骤S3.6;否则以当前找到的第二节点作为新的第一节点,以数组中的最后一个控制节点作为新的第二节点重新执行步骤S3.4。
步骤S3.6、取最终保留的控制节点,作为移动机器人运动中的局部目标点,控制移动机器人由起点坐标运动至终点坐标。
该方法大大的删减了冗余的控制节点,简化整个运动路径为一个或多个线段,便于运动控制的实现,同时也提高了机器人移动的流畅度,降低机器人的运动损耗。
在本申请的整个控制过程中,结合了A*算法和RRT*算法,取两者算法之所长,以实现机器人高效、可靠的路径规划。并且本申请并非简单组合两种算法,而是结合实际障碍点的分布情况,在整个路径规划中融合两种算法,既凸显了每一算法的优势,又可根据实时分析的障碍物位置分布,提升两种算法的融合度,避免算法切换规划节点处出现路径突变或断层的现象。其中A*算法和RRT*算法可采用现有的算法实现。
其中一个实施例中,采用的A*算法包括以下步骤:
1)将规划起始坐标作为起始节点a,将规划目标坐标作为终节点b,从起始节点a开始,把起始节点a作为待处理的方格,存入一个open表中,open表存放的是待检查的节点。
2)寻找起始节点a相邻并可以直接到达的节点,将寻找到的节点都放入open表中,并将寻找到的节点的父节点设置为起始节点a。
3)从open表中删除起始节点a,并将起始节点a加入close表中,close表中存放的是不需要再次检查的节点。
4)从open表中找出F值最小的节点c,其中F值的计算如下:
F=G+H
其中G表示从起始节点a移动到当前计算节点的移动代价,H表示从当前计算节点到终节点的估值代价。
5)把节点c从open表中删除,放入close表中。
6)检查节点c所有相邻并且可以直接到达的节点,若所述节点还不在open表中,则将所述节点加入open表,并将所述节点的父节点设置为节点c。
7)如果节点c所有相邻并且可以直接到达的节点中的部分节点已经在open表中,则计算已经在open表中的节点新的路径的G值,若G值更低则将当前计算节点的父节点设置为节点c,并重新计算当前计算节点的F值;否则不作处理。
8)重复步骤4)~7)直至将终节点加入open表,说明路径已经找到,路径规划完成;或者重复步骤4)~7)直至查找终节点失败,并且open表为空,表明不存在路径,路径规划完成。
本实施例中的A*算法在路径规划过程中能够快速绕开障碍物,加速路径规划速度。关于本实施例A*算法中为具体展开说明的部分参考现有技术实现即可,例如F值、G值、H值的计算过程等。
其中,RRT*算法为基本的RRT算法的一种改进,对于现有的RRT算法而言,其执行步骤如下:
1)在状态空间中随机选择一个采样点Xrand
2)在随机树中找到距采样点Xrand最近的节点Xnearest
3)从节点Xnearest开始,向着采样点Xrand的方向生长一个步长stepsize的距离,得到一个新的节点Xnew。如果节点Xnew与障碍物发生碰撞,则放弃这次生长,否则将节点Xnew加入随机树中。
4)重复上述步骤直到节点Xnearest与终点Xgoal距离小于一个阈值,代表随机树到达了终点,算法结束。
由于RRT算法采样的随机性,导致最终生成的路径往往只是可行路径而不是最优路径,因此本实施例不直接采用RRT算法,而是采用规划性能更优的RRT*算法来得到更好的规划路径,其中RRT*算法主要包括:
1)在规划起始坐标和规划目标坐标之间基于RRT算法找到一条可行的初始路径。
2)以规划起始坐标和规划目标坐标作为椭圆的两个焦点,以所述初始路径的总长度为椭圆上的点到两个焦点的距离之和,构建一个椭圆。
3)在所述椭圆中进行重采样,并对重采样得到的节点(即节点Xnew)重新选择父节点。
4)为随机树进行重新布线。
RRT*算法主要基于RRT算法实现,在RRT算法的基础上增加了重新为节点Xnew选择父节点的过程和重布线随机数的过程,因此弱化了RRT算法的随机性,提高路径规划可靠性。
RRT*算法也是路径规划领域较为成熟的技术,本申请可基于现有的RRT*算法实现,例如基于S.Karaman和E.Frazzoli于2011年提出的RRT*算法。该RRT*算法的过程如下:
1)在规划起始坐标和规划目标坐标之间基于RRT算法找到一条可行的初始路径。
2)以规划起始坐标和规划目标坐标作为椭圆的两个焦点,以所述初始路径的总长度为椭圆上的点到两个焦点的距离之和,构建一个椭圆。
3)在所述椭圆中进行重采样,产生一个新的采用点Xrand
4)在随机树中找到距采样点Xrand最近的节点Xnearest
5)连接Xrand与Xnearest
6)以Xrand为中心,ri=1.5倍的stepsize为半径,在随机树上搜索节点,找出潜在的父节点集合Xpotential_parent,目的是更新Xrand,找到是否有比其更好的父节点
7)从某一个潜在的父节点Xpotential_parent开始考虑,计算出Xparent作为父节点的代价。
8)先不进行碰撞检测,而是将Xpotential_parent与Xrand连接起来,计算出连线路径的代价。
9)将新的这条连线路径的代价与原路径的代价进行比较,如果新的这连线条路径的代价更小则进行碰撞检测,如果新的这条连线路径代价更大则换为下一个潜在的父节点。
10)如果碰撞检测失败,Xpotential_parent与Xrand连线之间有障碍物,则该潜在父节点不作为新的父节点。
11)如果碰撞检测成功,在随机树中将之前的边删掉,将新的边添加进去,将Xpotential_parent作为Xparent
12)遍历所有的潜在父节点,得到更新后的随机树。
RRT*算法是渐进最优的,只要给足够的运行时间,一定可以收敛到最优解。但是得到最优解所花时间较长,不满足机器人路径规划的实时性。因此本实施例为了进一步提升RRT算法的路径规划速度,加速了RRT*算法种的重采样的过程,在一实施例中,在所述椭圆中进行重采样的过程如下:
3.1)连接椭圆的两个顶点得到线段l1,将线段l1进行M等分,记M-1个等分点为m0,m1,…mM-3,mM-2
3.2)分别过各等分点做线段l1的垂线,记垂线交于椭圆的上半部分的交点为
Figure BDA0002739620150000101
并保存在数组v1中,记垂线交于椭圆的下半部分的交点为
Figure BDA0002739620150000102
并保存在数组v2中;
3.3)判断数组v1和v2中的交点在障碍物所在范围内的情况,选取数组v1和v2中交点在障碍物所在范围内较少的一组所对应的一半椭圆作为重采样区域,在所述重采样区域中进行重采样。
本实施例通过判断初始规划的路径所在的椭圆范围内的障碍物情况,缩小了重采样的区域,直接将重采样区域从整个椭圆减少为一半椭圆,并且能够保持重采样的效果,加快收敛到最优路径的速度,即在不影响得到路径规划最优解的前提下,极大程度上的提升了路径规划效率。本申请的路径规划方法对环境适应性强,难度低,易于实施。
以上所述实施例的各技术特征可以进行任意的组合,为使描述简洁,未对上述实施例中的各技术特征所有可能的组合都进行描述,然而,只要这些技术特征的组合不存在矛盾,都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式,其描述较为具体和详细,但并不能因此而理解为对发明专利范围的限制。应当指出的是,对于本领域的普通技术人员来说,在不脱离本申请构思的前提下,还可以做出若干变形和改进,这些都属于本申请的保护范围。因此,本申请专利的保护范围应以所附权利要求为准。

Claims (5)

1.一种基于A*算法和RRT*算法的移动机器人路径规划方法,其特征在于,所述基于A*算法和RRT*算法的移动机器人路径规划方法,包括:
步骤S1、取移动机器人本次运动的起点坐标和终点坐标,将连接起点坐标和终点坐标的线段离散成N个点;
步骤S2、根据离散得到的N个点进行路径规划,包括:
若N个点均不在障碍物所在范围内,则采用RRT*算法进行全段路径规划;
或者,若N个点中,由起点坐标起的前floor(N/2)个点中存在1个或多个点位于障碍物所在范围内,后N-floor(N/2)个点均不在障碍物所在范围内,则先采用A*算法,以起点坐标作为A*算法的规划起始坐标,以后N-floor(N/2)个点中的第一个不在障碍物所在范围内的点作为A*算法的规划目标坐标进行前段路径规划,后采用RRT*算法,以后N-floor(N/2)个点中的第一个不在障碍物所在范围内的点作为RRT*算法的规划起始坐标,以终点坐标作为RRT*算法的规划目标坐标进行后段路径规划;其中floor()为向下取整函数;
或者,若N个点中,由起点坐标起的前floor(N/2)个点均不在障碍物所在范围内,后N-floor(N/2)个点中存在1个或多个点位于障碍物所在范围内,则先采用RRT*算法,以起点坐标作为RRT*算法的规划起始坐标,以后N-floor(N/2)个点中的第一个不在障碍物所在范围内的点作为RRT*算法的规划目标坐标进行前段路径规划,后采用A*算法,以后N-floor(N/2)个点中的第一个不在障碍物所在范围内的点作为A*算法的规划起始坐标,以终点坐标作为A*算法的规划目标坐标进行后段路径规划;
或者,若N个点中,由起点坐标起的前floor(N/2)个点中存在1个或多点在障碍物所在范围内,后N-floor(N/2)个点中也存在1个或多个点在障碍物所在范围内,则采用A*算法进行全段路径规划;
步骤S3、根据路径规划后得到的路径中的控制节点控制移动机器人由起点坐标运动至终点坐标。
2.如权利要求1所述的基于A*算法和RRT*算法的移动机器人路径规划方法,其特征在于,所述步骤S3,根据路径规划后得到的路径中的控制节点控制移动机器人由起点坐标运动至终点坐标,包括:
步骤S3.1、取全段路径规划得到的全段路径中的控制节点;
或者,整合前段路径规划得到的前段路径和后段路径规划得到的后段路径生成全段路径,取全段路径中的控制节点;
步骤S3.2、将所有控制节点按照由起点坐标至终点坐标的方向,两两以父节点至子节点的顺序保存在一个数组中;
步骤S3.3、取数组中的第一个控制节点作为第一节点,取数组中的最后一个控制节点作为第二节点;
步骤S3.4、判断第一节点和第二节点的连线是否穿过障碍物所在范围,若连线未穿过障碍物所在范围,则仅保留第一节点和第二节点,丢弃第一节点和第二节点之间的其余控制节点,并执行步骤S3.5;若连线穿过障碍物所在范围,则找到第二节点的父节点作为新的第二节点,并重新执行步骤S3.4;
步骤S3.5、判断当前找到的第二节点是否是数组中的最后一个控制节点,若是则执行步骤S3.6;否则以当前找到的第二节点作为新的第一节点,以数组中的最后一个控制节点作为新的第二节点重新执行步骤S3.4;
步骤S3.6、取最终保留的控制节点,作为移动机器人运动中的局部目标点,控制移动机器人由起点坐标运动至终点坐标。
3.如权利要求1所述的基于A*算法和RRT*算法的移动机器人路径规划方法,其特征在于,所述A*算法,包括:
1)将规划起始坐标作为起始节点a,将规划目标坐标作为终节点b,从起始节点a开始,把起始节点a作为待处理的方格,存入一个open表中,open表存放的是待检查的节点;
2)寻找起始节点a相邻并可以直接到达的节点,将寻找到的节点都放入open表中,并将寻找到的节点的父节点设置为起始节点a;
3)从open表中删除起始节点a,并将起始节点a加入close表中,close表中存放的是不需要再次检查的节点;
4)从open表中找出F值最小的节点c,其中F值的计算如下:
F=G+H
其中G表示从起始节点a移动到当前计算节点的移动代价,H表示从当前计算节点到终节点的估值代价;
5)把节点c从open表中删除,放入close表中;
6)检查节点c所有相邻并且可以直接到达的节点,若所述节点还不在open表中,则将所述节点加入open表,并将所述节点的父节点设置为节点c;
7)如果节点c所有相邻并且可以直接到达的节点中的部分节点已经在open表中,则计算已经在open表中的节点新的路径的G值,若G值更低则将当前计算节点的父节点设置为节点c,并重新计算当前计算节点的F值;否则不作处理;
8)重复步骤4)~7)直至将终节点加入open表,说明路径已经找到,路径规划完成;或者重复步骤4)~7)直至查找终节点失败,并且open表为空,表明不存在路径,路径规划完成。
4.如权利要求1所述的基于A*算法和RRT*算法的移动机器人路径规划方法,其特征在于,所述RRT*算法,包括:
1)在规划起始坐标和规划目标坐标之间基于RRT算法找到一条可行的初始路径;
2)以规划起始坐标和规划目标坐标作为椭圆的两个焦点,以所述初始路径的总长度为椭圆上的点到两个焦点的距离之和,构建一个椭圆;
3)在所述椭圆中进行重采样,并对重采样得到的节点重新选择父节点;
4)为随机树进行重新布线。
5.如权利要求4所述的基于A*算法和RRT*算法的移动机器人路径规划方法,其特征在于,所述在椭圆中进行重采样,包括:
3.1)连接椭圆的两个顶点得到线段l1,将线段l1进行M等分,记M-1个等分点为m0,m1,…mM-3,mM-2
3.2)分别过各等分点做线段l1的垂线,记垂线交于椭圆的上半部分的交点为
Figure FDA0002739620140000031
并保存在数组v1中,记垂线交于椭圆的下半部分的交点为
Figure FDA0002739620140000032
并保存在数组v2中;
3.3)判断数组v1和v2中的交点在障碍物所在范围内的情况,选取数组v1和v2中交点在障碍物所在范围内较少的一组所对应的一半椭圆作为重采样区域,在所述重采样区域中进行重采样。
CN202011145680.8A 2020-10-23 2020-10-23 一种基于a*算法和rrt*算法的移动机器人路径规划方法 Active CN112393728B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011145680.8A CN112393728B (zh) 2020-10-23 2020-10-23 一种基于a*算法和rrt*算法的移动机器人路径规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011145680.8A CN112393728B (zh) 2020-10-23 2020-10-23 一种基于a*算法和rrt*算法的移动机器人路径规划方法

Publications (2)

Publication Number Publication Date
CN112393728A true CN112393728A (zh) 2021-02-23
CN112393728B CN112393728B (zh) 2022-05-31

Family

ID=74596304

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011145680.8A Active CN112393728B (zh) 2020-10-23 2020-10-23 一种基于a*算法和rrt*算法的移动机器人路径规划方法

Country Status (1)

Country Link
CN (1) CN112393728B (zh)

Cited By (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113050627A (zh) * 2021-03-02 2021-06-29 北京旷视机器人技术有限公司 路径规划方法、装置、移动机器人及计算机存储介质
CN113359748A (zh) * 2021-06-22 2021-09-07 浙江科技学院 一种融合预测的改进Multi-RRT路径规划方法及AGV小车
CN113624230A (zh) * 2021-06-22 2021-11-09 安克创新科技股份有限公司 用于移动机器人的导航路径生成方法和移动机器人
CN113618277A (zh) * 2021-07-28 2021-11-09 华南理工大学 带可达性球分层搜索树的焊接机器人离线焊接路径规划方法
CN113657636A (zh) * 2021-06-18 2021-11-16 广东电网有限责任公司佛山供电局 一种电网运行方式图的自动规划生成算法
CN113804197A (zh) * 2021-07-30 2021-12-17 深圳市新盒科技有限公司 基于复杂轨道地图的多台机器小车路径规划方法
CN113945216A (zh) * 2021-10-15 2022-01-18 上海懒书智能科技有限公司 一种移动机器设备的路径规划和管理系统和方法
CN114577217A (zh) * 2022-05-05 2022-06-03 季华实验室 基于冯洛诺伊图的路径规划方法、装置、设备及存储介质
CN115046557A (zh) * 2022-08-11 2022-09-13 合肥井松智能科技股份有限公司 一种结合b样条曲线和a星算法的agv路径规划方法
CN115268456A (zh) * 2022-08-10 2022-11-01 哈尔滨理工大学 一种动态变策略informed-RRT*的无人车路径规划方法
CN115826591A (zh) * 2023-02-23 2023-03-21 中国人民解放军海军工程大学 一种基于神经网络估计路径代价的多目标点路径规划方法
CN117032231A (zh) * 2023-08-10 2023-11-10 海南大学 一种基于改进rrt的多智能体路径规划方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20170241790A1 (en) * 2016-02-24 2017-08-24 Honda Motor Co., Ltd. Path plan generating apparatus for mobile body
CN108444489A (zh) * 2018-03-07 2018-08-24 北京工业大学 一种改进rrt算法的路径规划方法
CN108549388A (zh) * 2018-05-24 2018-09-18 苏州智伟达机器人科技有限公司 一种基于改进a星策略的移动机器人路径规划方法
CN108896052A (zh) * 2018-09-20 2018-11-27 鲁东大学 一种基于动态复杂环境下的移动机器人平滑路径规划方法
CN110334391A (zh) * 2019-05-23 2019-10-15 明阳智慧能源集团股份公司 一种多维度约束风电场集电线路自动规划方法
US20200173794A1 (en) * 2018-11-30 2020-06-04 Mazda Motor Corporation Route candidate setting system and route candidate setting method

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20170241790A1 (en) * 2016-02-24 2017-08-24 Honda Motor Co., Ltd. Path plan generating apparatus for mobile body
CN108444489A (zh) * 2018-03-07 2018-08-24 北京工业大学 一种改进rrt算法的路径规划方法
CN108549388A (zh) * 2018-05-24 2018-09-18 苏州智伟达机器人科技有限公司 一种基于改进a星策略的移动机器人路径规划方法
CN108896052A (zh) * 2018-09-20 2018-11-27 鲁东大学 一种基于动态复杂环境下的移动机器人平滑路径规划方法
US20200173794A1 (en) * 2018-11-30 2020-06-04 Mazda Motor Corporation Route candidate setting system and route candidate setting method
CN110334391A (zh) * 2019-05-23 2019-10-15 明阳智慧能源集团股份公司 一种多维度约束风电场集电线路自动规划方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
C. ZAMMIT ET AL.: "Comparison between A and RRT Algorithms for UAV Path Planning", 《2018 AIAA GUIDANCE, NAVIGATION, AND CONTROL CONFERENCE》 *
丁帅等: "基于RRT*算法的水下机器人全局路径规划方法", 《舰船科学技术》 *

Cited By (18)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113050627A (zh) * 2021-03-02 2021-06-29 北京旷视机器人技术有限公司 路径规划方法、装置、移动机器人及计算机存储介质
CN113657636A (zh) * 2021-06-18 2021-11-16 广东电网有限责任公司佛山供电局 一种电网运行方式图的自动规划生成算法
CN113657636B (zh) * 2021-06-18 2024-03-01 广东电网有限责任公司佛山供电局 一种电网运行方式图的自动规划生成算法
CN113359748B (zh) * 2021-06-22 2022-05-10 杭州奇派自动化设备有限公司 一种融合预测的改进Multi-RRT路径规划方法及AGV小车
CN113624230B (zh) * 2021-06-22 2023-07-18 安克创新科技股份有限公司 用于移动机器人的导航路径生成方法和移动机器人
CN113624230A (zh) * 2021-06-22 2021-11-09 安克创新科技股份有限公司 用于移动机器人的导航路径生成方法和移动机器人
CN113359748A (zh) * 2021-06-22 2021-09-07 浙江科技学院 一种融合预测的改进Multi-RRT路径规划方法及AGV小车
CN113618277B (zh) * 2021-07-28 2022-04-05 华南理工大学 带可达性球分层搜索树的焊接机器人离线焊接路径规划方法
CN113618277A (zh) * 2021-07-28 2021-11-09 华南理工大学 带可达性球分层搜索树的焊接机器人离线焊接路径规划方法
CN113804197A (zh) * 2021-07-30 2021-12-17 深圳市新盒科技有限公司 基于复杂轨道地图的多台机器小车路径规划方法
CN113945216A (zh) * 2021-10-15 2022-01-18 上海懒书智能科技有限公司 一种移动机器设备的路径规划和管理系统和方法
CN114577217B (zh) * 2022-05-05 2022-07-29 季华实验室 基于冯洛诺伊图的路径规划方法、装置、设备及存储介质
CN114577217A (zh) * 2022-05-05 2022-06-03 季华实验室 基于冯洛诺伊图的路径规划方法、装置、设备及存储介质
CN115268456A (zh) * 2022-08-10 2022-11-01 哈尔滨理工大学 一种动态变策略informed-RRT*的无人车路径规划方法
CN115268456B (zh) * 2022-08-10 2023-10-17 哈尔滨理工大学 一种动态变策略informed-RRT*的无人车路径规划方法
CN115046557A (zh) * 2022-08-11 2022-09-13 合肥井松智能科技股份有限公司 一种结合b样条曲线和a星算法的agv路径规划方法
CN115826591A (zh) * 2023-02-23 2023-03-21 中国人民解放军海军工程大学 一种基于神经网络估计路径代价的多目标点路径规划方法
CN117032231A (zh) * 2023-08-10 2023-11-10 海南大学 一种基于改进rrt的多智能体路径规划方法

Also Published As

Publication number Publication date
CN112393728B (zh) 2022-05-31

Similar Documents

Publication Publication Date Title
CN112393728B (zh) 一种基于a*算法和rrt*算法的移动机器人路径规划方法
CN109945873B (zh) 一种用于室内移动机器人运动控制的混合路径规划方法
US10365110B2 (en) Method and system for determining a path of an object for moving from a starting state to an end state set avoiding one or more obstacles
CN109579854B (zh) 基于快速扩展随机树的无人车避障方法
CN106774347A (zh) 室内动态环境下的机器人路径规划方法、装置和机器人
CN115079705A (zh) 基于改进a星融合dwa优化算法的巡检机器人路径规划方法
CN110231824B (zh) 基于直线偏离度方法的智能体路径规划方法
CN110986953B (zh) 路径规划方法、机器人及计算机可读存储介质
CN112229419B (zh) 一种动态路径规划导航方法及系统
CN113276109B (zh) 一种基于rrt算法的双机械臂解耦运动规划方法及系统
CN112985408A (zh) 一种路径规划优化方法及系统
CN112344938B (zh) 基于指向和势场参数的空间环境路径生成及规划方法
CN114489052A (zh) 一种改进rrt算法重连策略的路径规划方法
CN110705803B (zh) 基于三角形内心引导rrt算法的路径规划方法
CN114237302B (zh) 一种基于滚动时域的三维实时rrt*航路规划方法
CN113515127B (zh) 一种移动机器人路径规划方法及系统
CN114756034A (zh) 一种机器人实时避障路径规划方法及装置
CN114428499A (zh) 一种融合Astar与DWA算法的移动小车路径规划方法
CN112197783B (zh) 一种考虑车头指向的两阶段多抽样的rrt路径规划方法
CN113485378B (zh) 基于交通规则的移动机器人路径规划方法、系统及存储介质
Wang et al. UAV online path planning based on improved genetic algorithm with optimized search region
CN114911233A (zh) 基于多优化快速拓展随机树的足球机器人路径规划方法
CN115167466A (zh) 一种移动机器人标准控制集局部路径规划方法及系统
CN113341957A (zh) 一种基于痕迹地图A_star算法的多机器人路径规划方法
Chen et al. GVD-Exploration: An Efficient Autonomous Robot Exploration Framework Based on Fast Generalized Voronoi Diagram Extraction

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