CN114705196A - 一种用于机器人的自适应启发式全局路径规划方法与系统 - Google Patents

一种用于机器人的自适应启发式全局路径规划方法与系统 Download PDF

Info

Publication number
CN114705196A
CN114705196A CN202210632462.XA CN202210632462A CN114705196A CN 114705196 A CN114705196 A CN 114705196A CN 202210632462 A CN202210632462 A CN 202210632462A CN 114705196 A CN114705196 A CN 114705196A
Authority
CN
China
Prior art keywords
point
current
reverse search
edge
cost
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
CN202210632462.XA
Other languages
English (en)
Other versions
CN114705196B (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.)
Hunan Xinxin Xiangrong Intelligent Technology Co ltd
Original Assignee
Hunan University
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 Hunan University filed Critical Hunan University
Priority to CN202210632462.XA priority Critical patent/CN114705196B/zh
Publication of CN114705196A publication Critical patent/CN114705196A/zh
Application granted granted Critical
Publication of CN114705196B publication Critical patent/CN114705196B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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
    • 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
    • G06F16/90—Details of database functions independent of the retrieved data types
    • G06F16/901—Indexing; Data structures therefor; Storage structures
    • G06F16/9027—Trees
    • 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
    • Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
    • Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
    • Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Databases & Information Systems (AREA)
  • Theoretical Computer Science (AREA)
  • Software Systems (AREA)
  • Data Mining & Analysis (AREA)
  • General Engineering & Computer Science (AREA)
  • Feedback Control In General (AREA)

Abstract

本发明公开了一种用于机器人的自适应启发式全局路径规划方法与系统，本发明方法包括对输入的环境地图进行遍历，得到初始的状态空间、进行碰撞点检测，得到采样点；针对得到的采样点，以目标点出发进行反向搜索生成反向搜索点集，将采样点连接成边并排序，然后利用有序边的子节点、反向搜索点集来构建自适应启发函数，并基于自适应启发函数进行代价优化从起始点出发扩展有序的边队列生成正向搜索树，使正向搜索树延伸至目标点得到规划路径，重复上述步骤多次得到最终的规划路径。本发明能够实现快速、精确的路径规划，不仅可以规划出相对最短的路径，而且提高了规划成功率，以及在复杂障碍物地图上的有效性。

Description

一种用于机器人的自适应启发式全局路径规划方法与系统
技术领域
本发明涉及机器人定位自主导航中的路径规划技术，具体涉及一种用于机器人的自适应启发式全局路径规划方法与系统。
背景技术
随着机器人技术的快速发展，同时定位和地图构建作为机器人领域的基础关键技术，通过融合激光雷达和深度相机的信息可以实现优缺点互补，弥补单个传感器不可避免的缺点，提供更加精确且鲁棒的环境地图。在得到二维或者三维实时地图的基础上，机器人的最终实现自主导航需要用到路径规划技术。路径规划是指在确定的环境中，在起始点和目标点之间自动规划出一条长度最短或者代价最小的路径。路径规划方法依据规划任务的不同，可由全局规划和局部规划两个部分组成，全局路径规划是机器人在已知的地图中计算出一条由多个中间目标点组成的最优路径，其精度取决于环境地图的准确性。而局部路径规划更依赖机器人周围场景的实时障碍物特征，根据自身传感器信息对当前环境进行建模，确定周围环境的障碍物信息，进而依次在当前位置和全局路径的中间目标点之间规划出一条局部路径。本文属于全局路径规划范畴，传统的全局路径规划方法为基于采样的全局路径规划方法，这种方法不能准确计算状态空间中节点与目标点之间的距离，存在规划路径较长、规划成功率不高、在复杂障碍物地图上的有效性不高的问题。
发明内容
本发明要解决的技术问题：针对现有技术的上述问题，提供一种用于机器人的自适应启发式全局路径规划方法与系统，本发明能够实现快速、精确的路径规划，不仅可以规划出相对最短的路径，而且提高了规划成功率，以及在复杂障碍物地图上的有效性。
为了解决上述技术问题，本发明采用的技术方案为：
一种用于机器人的自适应启发式全局路径规划方法，包括：
S1、对输入的环境地图进行遍历，得到初始的状态空间；
S2、对状态空间进行碰撞点检测，得到采样点；
S3、针对得到的采样点，以目标点出发向起始点进行反向搜索以生成反向搜索点集；
S4、首先将采样点两两连接成边、加入边队列并进行排序，然后利用排序后得到的有序的边队列中边的子节点通过邻域内为与反向搜索点集中的反向搜索点到目标点的距离来构建自适应启发函数，并基于自适应启发函数进行代价优化以扩展边队列生成正向搜索树，最终使正向搜索树延伸至目标点从而得到通往目标点的规划路径；
S5、判断迭代次数是否满足结束条件，若不满足结束条件，则对状态空间进行椭圆区域计算，并针对椭圆区域进行冗余采样点删除，从而得到更新后的状态空间，跳转步骤S2；否则，将最终得到的通往目标点的规划路径作为结果输出。
可选地，步骤S2之后、步骤S3之前还包括剔除与障碍物发生碰撞的无效的采样点步骤。
可选地，步骤S3中以目标点出发向起始点进行反向搜索以生成反向搜索点集包括：
S3.1、将采样点按照与目标点之间的欧式距离进行升序排列；
S3.2、从升序排列后的采样点遍历取出一个采样点作为当前节点，若采样点已经遍历完毕，则判定采样点已经遍历完毕，跳转步骤S4；否则，跳转步骤S3.3；
S3.3、判断当前节点、目标点之间构成的边是否与障碍物发生碰撞，若未发生碰撞，则直接将当前节点加入反向搜索点集，并保存当前节点与目标点的欧式距离作为当前节点到目标点的代价；若发生碰撞，则先在反向搜索点集中选择与当前节点的欧式距离小于固定阈值的反向搜索点，然后将选择的该部分反向搜索点分别到目标点、当前节点的最小欧式距离和作为当前节点到目标点的代价，并将当前节点加入反向搜索点集，并在选择的该部分反向搜索点中找到欧式距离最近的反向搜索点作为当前节点的依赖节点；
S3.4、判断反向搜索点集中的反向搜索点数量是否已经满足要求，若不满足要求，则跳转步骤S3.2以继续增加反向搜索点，否则判定反向搜索点集生成完毕，跳转步骤S4。
可选地，步骤S3.3中将选择的该部分反向搜索点分别到目标点、当前节点的最小欧式距离和作为当前节点到目标点的代价的函数表达式为：
Figure DEST_PATH_IMAGE001
上式中，g R (x)表示当前节点x到目标点的代价，x i 为反向搜索点集合V R 中的第i个节点，反向搜索点集合V R 为反向搜索点集中以当前节点x为中心、且欧式距离小于固定阈值ε的反向搜索点集合，c(x,x i )为当前节点x、第i个节点x i 的欧式距离，g R (x i )表示反向搜索点集合V R 中的第i个节点x i 到目标点的代价。
可选地，步骤S4包括：
S4.1、将采样点两两连接成边，并将该边加入边队列并进行排序得到有序的边队列，且排序方式为从起始点出发经过该边到达目标点的距离；
S4.2、从有序的边队列中遍历取出一个有序边作为当前边；
S4.3、判断当前边是否与障碍物发生碰撞，若发生碰撞，则将当前边加入无效边；若当前边的父节点在反向搜索点集中，则更新当前边的父节点在反向搜索点集中的状态，并跳转步骤S3进行反向搜索以更新反向搜索点集；若当前边的父节点不在反向搜索点集中，跳转执行步骤S4.2继续遍历；若未发生碰撞，则跳转执行步骤S4.4；
S4.4、结合当前路径的代价判断当前边是否能够优化当前路径的代价，所述当前路径是指起始点出发向目标点经过当前边的路径，若能够优化当前路径的代价，则判断当前边是否已经在正向搜索树中，若已经在正向搜索树中，则利用该边子节点扩展更多的边并加入有序的边队列；若尚未在正向搜索树中，则将当前边加入正向搜索树，并更新当前边的两个节点在正向搜索树中的信息，同时更新优化后的当前路径的代价；跳转执行步骤S4.2继续遍历；
S4.5、判断本轮遍历得到的不能够优化当前路径的代价的有序边的比例是否超过设定值，若超过设定值，则跳转步骤S3进行反向搜索以更新反向搜索点集。
可选地，步骤S4.1中从起始点出发经过该边到达目标点的距离的计算函数表达式为：
Figure 559148DEST_PATH_IMAGE002
上式中，Sort F (x p ,x c )为从起始点出发经过节点x p 和x c 之间构成的边到达目标点的距离，节点x p 和x c 为正向搜索过程中遍历的用于连接成边的采样点对，g F (x p )表示节点x p 到目标点的代价，c(x p ,x c )为节点x p 和x c 之间的欧式距离，h F (x c )表示节点x c 通过自适应启发函数抵达终点的代价，且有：
Figure DEST_PATH_IMAGE003
上式中，h F (x)表示节点x通过自适应启发函数抵达终点的代价，g R (x)表示当前节点x到目标点的代价，x i 为反向搜索点集合V R 中的第i个节点，反向搜索点集合V R 为反向搜索点集中以当前节点x为中心、且欧式距离小于固定阈值ε的反向搜索点集合，c(x,x i )为当前节点x、第i个节点的欧式距离，g R (x i )表示反向搜索点集合V R 中的第i个节点x i 到目标点的代价。
可选地，步骤S4.4中结合当前路径的代价判断当前边是否能够优化当前路径的代价时，能够优化当前路径的代价所对应的条件的函数表达式为：
Figure 32986DEST_PATH_IMAGE004
上式中，c now 为当前路径的代价，g F (x p )表示节点x p 到目标点的代价，c(x p ,x c )为节点x p 和x c 之间的欧式距离，h F (x c )表示节点x c 通过自适应启发函数抵达终点的代价。
可选地，步骤S5中对状态空间进行椭圆区域计算，并针对椭圆区域进行冗余采样点删除，从而得到更新后的状态空间包括：
S5.1、将最终得到的通往目标点的规划路径的代价作为椭圆的长轴，将起始点与目标点之间的欧式距离作为焦距，起始点和目标点作为两个焦点，根据焦距和两个焦点构造椭圆以获取椭圆区域；
S5.2、对正向搜索树进行调整，删除不在椭圆区域内的边；对反向搜索点集进行调整，删除不在椭圆区域内的反向搜索点；在椭圆区域内生成新的采样点，将其加入反向搜索点集，将连接采样点且不与障碍物发生碰撞的有效边加入有序的边队列，并将有效边对应的采样点加入到正向搜索树中。
此外，本发明还提供一种用于机器人的自适应启发式全局路径规划系统，包括相互连接的微处理器和存储器，所述微处理器被编程或配置以执行所述用于机器人的自适应启发式全局路径规划方法的步骤。
此外，本发明还提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，该计算机程序用于被微处理器或者计算机设备编程或配置以执行所述用于机器人的自适应启发式全局路径规划方法的步骤。
和现有技术相比，本发明具有下述优点：本发明采用正向搜索与方向搜索结合的非对称的双向搜索策略，并利用有序边的子节点通过邻域内为与反向搜索点集中的反向搜索点到目标点的距离来构建自适应启发函数来进行代价优化以扩展有序的边队列生成正向搜索树以规划出渐进最优的有效路径，并结合椭圆区域采样优化进行路径优化策略以减小路径优化所需的状态空间，并剔除反向搜索点集和正向搜索树中冗余的节点和边，从而能够实现快速、精确的路径规划，使得本发明在路径长度上的指标上更优秀，不仅可以规划出相对最短的路径，而且提高了规划成功率，以及在复杂障碍物地图上的有效性。
附图说明
图1为本发明实施例方法的基本流程示意图。
图2为本发明实施例方法的原理示意图。
图3为本发明实施方法的反向搜索过程示意图。
图4为本发明实施例方法的正向搜索过程示意图。
图5为本发明实施例中自适应启发函数计算与目标点代价的示意图。
图6为本发明实施例中椭圆区域的示意图。
具体实施方式
路径规划问题的图论描述如下：路径规划问题所输入的环境地图为二维地图，可表示为：M=M0+M1，M0为障碍物区域，M1为可通行区域，状态空间Ms通常被定义为Ms=Rn，n∈N，起始点为x start ∈X，目标点为x goal ∈X，采样点集合Xs⊂X，其中Rn表示空间搜索状态的总集，每一条边表示一个状态的转换，n表示地图节点的个数，N表示正整数集，X表示输入地图中所有节点的集合。采样点（节点）之间的边可以被定义为任意的节点x p 和x c 之间的连接，简称为边(x p, x c )。其中节点x p 为父节点（路径上靠近起始点），节点x c 为子节点（路径上靠近目的点）。正向搜索树和反向搜索点集分别定义为T F =(V F ,E F )和V R ，其中V F 和V R 分别为正向搜索得到的节点集合和反向搜索得到的反向搜索点集合，E F 为正向搜索树上节点之间边的集合，Q F 是正向搜索树中的有序的边队列。定义函数h F (x)为在节点x通过启发式函数抵达终点的代价，函数g F (x)为起始点通过正向搜索树抵达节点x的最小代价，函数f F (x)= h F (x)+ g F (x)为从起始点开始经由节点x抵达终点的代价。同理，函数g R (x)为反向搜索点集中节点x 抵达终点的近似真实代价。函数c(x 1 ,x 2)表示从节点x 1到节点x 2的代价。当规划出一条更优路径时，可表示为X f :={x∈X| f F (x)≤c now }，其中X f 表示正向搜索过程中起始节点到x节点距离小于c now 的节点集合，c now 为当前规划路径的近似代价。下文将基于上述路径规划问题的图论描述，对本发明进行进一步的详细说明。
如图1和图2所示，本实施例用于机器人的自适应启发式全局路径规划方法包括：
S1、对输入的环境地图进行遍历，得到初始的状态空间；
S2、对状态空间进行碰撞点检测，得到采样点；
S3、针对得到的采样点，以目标点出发向起始点进行反向搜索以生成反向搜索点集；
S4、首先将采样点两两连接成边、加入边队列并进行排序，然后利用排序后得到的有序的边队列中边的子节点通过邻域内为与反向搜索点集中的反向搜索点到目标点的距离来构建自适应启发函数，并基于自适应启发函数进行代价优化以扩展边队列生成正向搜索树，最终使正向搜索树延伸至目标点从而得到通往目标点的规划路径；
S5、判断迭代次数是否满足结束条件，若不满足结束条件，则对状态空间进行椭圆区域计算，并针对椭圆区域进行冗余采样点删除，从而得到更新后的状态空间，跳转步骤S2；否则，将最终得到的通往目标点的规划路径作为结果输出。
输入的环境地图参见前文路径规划问题的描述，在数据结构中可以表示成矩阵或者邻接表，自身位置使用横纵坐标表示。经过本实施例方法对输入进行处理，得到机器人当前位置到达目标点的最优路径，路径表示方式是环境地图的地图中节点（采样点）的有序集合。
为了降低无效的采样点的干扰，本实施例中步骤S2之后、步骤S3之前还包括剔除与障碍物发生碰撞的无效的采样点步骤，其中与障碍物发生碰撞是指采样点的位置位于障碍物的区域内，可通过简单坐标判断即可确定。
本实施例中，步骤S3中以目标点出发向起始点进行反向搜索以生成反向搜索点集包括：
S3.1、将采样点按照与目标点之间的欧式距离进行升序排列；
S3.2、从升序排列后的采样点遍历取出一个采样点作为当前节点，若采样点已经遍历完毕，则判定采样点已经遍历完毕，跳转步骤S4；否则，跳转步骤S3.3；
S3.3、判断当前节点、目标点之间构成的边是否与障碍物发生碰撞，若未发生碰撞，则直接将当前节点加入反向搜索点集，并保存当前节点与目标点的欧式距离作为当前节点到目标点的代价；若发生碰撞，则先在反向搜索点集中选择与当前节点的欧式距离小于固定阈值的反向搜索点，然后将选择的该部分反向搜索点分别到目标点、当前节点的最小欧式距离和作为当前节点到目标点的代价，并将当前节点加入反向搜索点集，并在选择的该部分反向搜索点中找到欧式距离最近的反向搜索点作为当前节点的依赖节点；
S3.4、判断反向搜索点集中的反向搜索点数量是否已经满足要求，若不满足要求，则跳转步骤S3.2以继续增加反向搜索点，否则判定反向搜索点集生成完毕，跳转步骤S4。
本实施例中，步骤S3.3中将选择的该部分反向搜索点分别到目标点、当前节点的最小欧式距离和作为当前节点到目标点的代价的函数表达式为：
Figure 847358DEST_PATH_IMAGE001
上式中，g R (x)表示当前节点x到目标点的代价，x i 为反向搜索点集合V R 中的第i个节点，反向搜索点集合V R 为反向搜索点集中以当前节点x为中心、且欧式距离小于固定阈值ε的反向搜索点集合，c(x,x i )为当前节点x、第i个节点x i 的欧式距离，g R (x i )表示反向搜索点集合V R 中的第i个节点x i 到目标点的代价。上式中右侧的函数表达式表示在以当前节点x为中心、ε为半径的范围内的所有节点x i 中值最小的c(x,x i )与g R (x i )之和。如图3所示，当前节点x、目标点x goal 之间近似真实代价为c(x, x goal )，上式中右侧的函数表达式表示在以当前节点x为中心、ε为半径的范围内的所有节点x 1和x 2中值最小的c(x,x i )与g R (x i )之和。图中为c(x,x 1)最小，因此上式中右侧的函数表达式表示c(x,x 1)与g R (x 1)之和。
将当前节点x、目标点x goal 之间构成的边与障碍物发生碰撞，则使得当前节点x、目标点x goal 之间没有直接的路径可达，记为invalid(x, x goal )；将当前节点x、目标点x goal 之间构成的边与障碍物不发生碰撞，则使得当前节点x、目标点x goal 之间有直接的路径可达，记为valid(x, x goal )，则当前节点到目标点的代价的方式可描述为：
Figure 100002_DEST_PATH_IMAGE005
上式中，c(x, x goal )为当前节点x、目标点x goal 之间的欧式距离。
本实施例中，步骤S4包括：
S4.1、将采样点两两连接成边，并将该边加入边队列并进行排序得到有序的边队列，且排序方式为从起始点出发经过该边到达目标点的距离；有序的边队列中，越往前的边代价（起始点出发经过该边到达目标点的距离）越小，所以其中的边也可称为有序边；
S4.2、从有序的边队列中遍历取出一个边作为当前边；
S4.3、判断当前边是否与障碍物发生碰撞，若发生碰撞，则将当前边加入无效边；若当前边的父节点在反向搜索点集中，则更新当前边的父节点在反向搜索点集中的状态，并跳转步骤S3进行反向搜索以更新反向搜索点集；若当前边的父节点不在反向搜索点集中，跳转执行步骤S4.2继续遍历；若未发生碰撞，则跳转执行步骤S4.4；
S4.4、结合当前路径的代价判断当前边是否能够优化当前路径的代价，所述当前路径是指起始点出发向目标点经过当前边的路径，若能够优化当前路径的代价，则判断当前边是否已经在正向搜索树中，若已经在正向搜索树中，则利用该边子节点扩展更多的边并加入有序的边队列；若尚未在正向搜索树中，则将当前边加入正向搜索树，并更新当前边的两个节点在正向搜索树中的信息，同时更新优化后的当前路径的代价；跳转执行步骤S4.2继续遍历；
S4.5、判断本轮遍历得到的不能够优化当前路径的代价的有序边的比例是否超过设定值，若超过设定值，则跳转步骤S3进行反向搜索以更新反向搜索点集。
如图4所示，正向搜索的目的是建立一棵通往目标点的正向搜索树。正向搜索首先从正向搜索点集合V F 选取维持一个有序的边队列，其中的有序边为两个正向搜索点(x p , x c )所构成的边，判断其未与障碍物发生碰撞，并且能够优化当前路径的代价，而且不存在于正向搜索树，于是将其加入到正向搜索树中，更新优化后的当前路径的代价c now 。这是一次正向搜索过程，其中g F (x p )表示节点x p 到目标点的代价，c(x p ,x c )为节点x p 和x c 之间的欧式距离，h F (x c )表示节点x c 通过自适应启发函数抵达终点的代价。正如前文所示，边(x p, x c )中父节点为x p ，子节点为x c ，同理，边(x c, x p )子节点为x p ，父节点为x c 。在有序的边队列中，边的排序方式为从起始点出发经过该边到达目标点的距离，因此边(x p, x c )与边(x c, x p )的代价是不一样的，例如：经过边(x p, x c )则路径为，x start ->…->x p ->x c ，也就是代表了路径的方向，其中x start 为起始点。在正向搜索中会先使用代价更小的边，当边(x p, x c )的加入能使得到目标点的代价比当前路径的代价c now 更小时，则边(x p, x c )能加入到正向搜索树中。本实施例中，步骤S4.1中从起始点出发经过该边到达目标点的距离的计算函数表达式为：
Figure 371618DEST_PATH_IMAGE002
上式中，Sort F (x p ,x c )为从起始点出发经过节点x p 和x c 之间构成的边到达目标点的距离，节点x p 和x c 为正向搜索过程中遍历的用于连接成边的采样点对，g F (x p )表示节点x p 到目标点的代价，c(x p ,x c )为节点x p 和x c 之间的欧式距离，h F (x c )表示节点x c 通过自适应启发函数抵达终点的代价，且有：
Figure 310755DEST_PATH_IMAGE003
上式中，h F (x)表示节点x通过自适应启发函数抵达终点的代价，g R (x)表示当前节点x到目标点的代价，x i 为反向搜索点集合V R 中的第i个节点，反向搜索点集合V R 为反向搜索点集中以当前节点x为中心、且欧式距离小于固定阈值ε的反向搜索点集合，c(x,x i )为当前节点x、第i个节点的欧式距离，g R (x i )表示反向搜索点集合V R 中的第i个节点x i 到目标点的代价。
如图5所示展示了自适应启发函数计算与目标点代价的过程，对于图中描述的当前节点x，若其在反向搜索点集中，则直接使用节点x到目标点的近似代价，否则在反向搜索点集中搜索与节点x相邻（欧式距离小于固定阈值的节点x i ），根据该部分节点到目标点的代价确定节点x到目标点的代价h F (x)。
本实施例中，步骤S4.4中结合当前路径的代价判断当前边是否能够优化当前路径的代价时，能够优化当前路径的代价所对应的条件的函数表达式为：
Figure 306393DEST_PATH_IMAGE004
上式中，c now 为当前路径的代价，g F (x p )表示节点x p 到目标点的代价，c(x p ,x c )为节点x p 和x c 之间的欧式距离，h F (x c )表示节点x c 通过自适应启发函数抵达终点的代价。
本实施例中，步骤S5中对状态空间进行椭圆区域计算，并针对椭圆区域进行冗余采样点删除，从而得到更新后的状态空间包括：
S5.1、将最终得到的通往目标点的规划路径的代价作为椭圆的长轴，将起始点与目标点之间的欧式距离作为焦距，起始点和目标点作为两个焦点，根据焦距和两个焦点构造椭圆以获取椭圆区域，如图6所示；
S5.2、对正向搜索树进行调整，删除不在椭圆区域内的边；对反向搜索点集进行调整，删除不在椭圆区域内的反向搜索点；在椭圆区域内生成新的采样点，将其加入反向搜索点集，将连接采样点且不与障碍物发生碰撞的有效边加入有序的边队列，并将有效边对应的采样点加入到正向搜索树中。
综上所述，本实施例方法采用非对称的双向搜索，提出非对称双向搜索构建正向搜索树和反向搜索点集，正向搜索树由起始点开始往目标点生长，反向搜索点集从目标点附近开始扩充节点。本实施例方法本方法执行反向搜索进行大规模采样，计算有效采样点到目标点的最小代价，并将其加入反向搜索点集。进行正向搜索，将节点通过反向搜索点集中的点到达目标点的代价作为快速自适应启发函数来规划路径。反向搜索建立大规模反向搜索点集，不断的向搜索点集中加入节点，并且更新每个节点到目标点的近似真实代价。最终使状态空间中的节点到达目标点的代价是高度置信的。正向搜索建立一棵通往目标点的正向搜索树，同时正向搜索维持一个有序的边队列，通过将有序的边队列中的边合理的加入正向搜索树，最终使正向搜索树延伸至目标点。当规划出一条初始路径之后，采用椭圆区域采样进行路径优化。
此外，本实施例还提供一种用于机器人的自适应启发式全局路径规划系统，包括相互连接的微处理器和存储器，所述微处理器被编程或配置以执行前述用于机器人的自适应启发式全局路径规划方法的步骤。
此外，本实施例还提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，该计算机程序用于被微处理器或者计算机设备编程或配置以执行前述用于机器人的自适应启发式全局路径规划方法的步骤。
本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可读存储介质（包括但不限于磁盘存储器、CD-ROM、光学存储器等）上实施的计算机程序产品的形式。本申请是参照根据本申请实施例的方法、设备（系统）、和计算机程序产品的流程图和／或方框图来描述的。应理解可由计算机程序指令实现流程图和／或方框图中的每一流程和／或方框、以及流程图和／或方框图中的流程和／或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和／或方框图一个方框或多个方框中指定的功能的装置。这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和／或方框图一个方框或多个方框中指定的功能。这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和／或方框图一个方框或多个方框中指定的功能的步骤。
以上所述仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (10)

1.一种用于机器人的自适应启发式全局路径规划方法，其特征在于，包括：
S1、对输入的环境地图进行遍历，得到初始的状态空间；
S2、对状态空间进行碰撞点检测，得到采样点；
S3、针对得到的采样点，以目标点出发向起始点进行反向搜索以生成反向搜索点集；
S4、首先将采样点两两连接成边、加入边队列并进行排序，然后利用排序后得到的有序的边队列中边的子节点通过邻域内为与反向搜索点集中的反向搜索点到目标点的距离来构建自适应启发函数，并基于自适应启发函数进行代价优化以扩展边队列生成正向搜索树，最终使正向搜索树延伸至目标点从而得到通往目标点的规划路径；
S5、判断迭代次数是否满足结束条件，若不满足结束条件，则对状态空间进行椭圆区域计算，并针对椭圆区域进行冗余采样点删除，从而得到更新后的状态空间，跳转步骤S2；否则，将最终得到的通往目标点的规划路径作为结果输出。
2.根据权利要求1所述的用于机器人的自适应启发式全局路径规划方法，其特征在于，步骤S2之后、步骤S3之前还包括剔除与障碍物发生碰撞的无效的采样点步骤。
3.根据权利要求1所述的用于机器人的自适应启发式全局路径规划方法，其特征在于，步骤S3中以目标点出发向起始点进行反向搜索以生成反向搜索点集包括：
S3.1、将采样点按照与目标点之间的欧式距离进行升序排列；
S3.2、从升序排列后的采样点遍历取出一个采样点作为当前节点，若采样点已经遍历完毕，则判定采样点已经遍历完毕，跳转步骤S4；否则，跳转步骤S3.3；
S3.3、判断当前节点、目标点之间构成的边是否与障碍物发生碰撞，若未发生碰撞，则直接将当前节点加入反向搜索点集，并保存当前节点与目标点的欧式距离作为当前节点到目标点的代价；若发生碰撞，则先在反向搜索点集中选择与当前节点的欧式距离小于固定阈值的反向搜索点，然后将选择的该部分反向搜索点分别到目标点、当前节点的最小欧式距离和作为当前节点到目标点的代价，并将当前节点加入反向搜索点集，并在选择的该部分反向搜索点中找到欧式距离最近的反向搜索点作为当前节点的依赖节点；
S3.4、判断反向搜索点集中的反向搜索点数量是否已经满足要求，若不满足要求，则跳转步骤S3.2以继续增加反向搜索点，否则判定反向搜索点集生成完毕，跳转步骤S4。
4.根据权利要求3所述的用于机器人的自适应启发式全局路径规划方法，其特征在于，步骤S3.3中将选择的该部分反向搜索点分别到目标点、当前节点的最小欧式距离和作为当前节点到目标点的代价的函数表达式为：
Figure 641766DEST_PATH_IMAGE001
上式中，g R (x)表示当前节点x到目标点的代价，x i 为反向搜索点集合V R 中的第i个节点，反向搜索点集合V R 为反向搜索点集中以当前节点x为中心、且欧式距离小于固定阈值ε的反向搜索点集合，c(x,x i )为当前节点x、第i个节点x i 的欧式距离，g R (x i )表示反向搜索点集合V R 中的第i个节点x i 到目标点的代价。
5.根据权利要求1所述的用于机器人的自适应启发式全局路径规划方法，其特征在于，步骤S4包括：
S4.1、将采样点两两连接成边，并将该边加入边队列并进行排序得到有序的边队列，且排序方式为从起始点出发经过该边到达目标点的距离；
S4.2、从有序的边队列中遍历取出一个有序边作为当前边；
S4.3、判断当前边是否与障碍物发生碰撞，若发生碰撞，则将当前边加入无效边；若当前边的父节点在反向搜索点集中，则更新当前边的父节点在反向搜索点集中的状态，并跳转步骤S3进行反向搜索以更新反向搜索点集；若当前边的父节点不在反向搜索点集中，跳转执行步骤S4.2继续遍历；若未发生碰撞，则跳转执行步骤S4.4；
S4.4、结合当前路径的代价判断当前边是否能够优化当前路径的代价，所述当前路径是指起始点出发向目标点经过当前边的路径，若能够优化当前路径的代价，则判断当前边是否已经在正向搜索树中，若已经在正向搜索树中，则利用该边子节点扩展更多的边并加入有序的边队列；若尚未在正向搜索树中，则将当前边加入正向搜索树，并更新当前边的两个节点在正向搜索树中的信息，同时更新优化后的当前路径的代价；跳转执行步骤S4.2继续遍历；
S4.5、判断本轮遍历得到的不能够优化当前路径的代价的有序边的比例是否超过设定值，若超过设定值，则跳转步骤S3进行反向搜索以更新反向搜索点集。
6.根据权利要求5所述的用于机器人的自适应启发式全局路径规划方法，其特征在于，步骤S4.1中从起始点出发经过该边到达目标点的距离的计算函数表达式为：
Figure 558906DEST_PATH_IMAGE003
上式中，Sort F (x p ,x c )为从起始点出发经过节点x p 和x c 之间构成的边到达目标点的距离，节点x p 和x c 为正向搜索过程中遍历的用于连接成边的采样点对，g F (x p )表示节点x p 到目标点的代价，c(x p ,x c )为节点x p 和x c 之间的欧式距离，h F (x c )表示节点x c 通过自适应启发函数抵达终点的代价，且有：
Figure 73064DEST_PATH_IMAGE004
上式中，h F (x)表示节点x通过自适应启发函数抵达终点的代价，g R (x)表示当前节点x到目标点的代价，x i 为反向搜索点集合V R 中的第i个节点，反向搜索点集合V R 为反向搜索点集中以当前节点x为中心、且欧式距离小于固定阈值ε的反向搜索点集合，c(x,x i )为当前节点x、第i个节点的欧式距离，g R (x i )表示反向搜索点集合V R 中的第i个节点x i 到目标点的代价。
7.根据权利要求5所述的用于机器人的自适应启发式全局路径规划方法，其特征在于，步骤S4.4中结合当前路径的代价判断当前边是否能够优化当前路径的代价时，能够优化当前路径的代价所对应的条件的函数表达式为：
Figure DEST_PATH_IMAGE005
上式中，c now 为当前路径的代价，g F (x p )表示节点x p 到目标点的代价，c(x p ,x c )为节点x p 和x c 之间的欧式距离，h F (x c )表示节点x c 通过自适应启发函数抵达终点的代价。
8.根据权利要求1所述的用于机器人的自适应启发式全局路径规划方法，其特征在于，步骤S5中对状态空间进行椭圆区域计算，并针对椭圆区域进行冗余采样点删除，从而得到更新后的状态空间包括：
S5.1、将最终得到的通往目标点的规划路径的代价作为椭圆的长轴，将起始点与目标点之间的欧式距离作为焦距，起始点和目标点作为两个焦点，根据焦距和两个焦点构造椭圆以获取椭圆区域；
S5.2、对正向搜索树进行调整，删除不在椭圆区域内的边；对反向搜索点集进行调整，删除不在椭圆区域内的反向搜索点；在椭圆区域内生成新的采样点，将其加入反向搜索点集，将连接采样点且不与障碍物发生碰撞的有效边加入有序的边队列，并将有效边对应的采样点加入到正向搜索树中。
9.一种用于机器人的自适应启发式全局路径规划系统，包括相互连接的微处理器和存储器，其特征在于，所述微处理器被编程或配置以执行权利要求1-8中任意一项所述用于机器人的自适应启发式全局路径规划方法的步骤。
10.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，该计算机程序用于被微处理器或者计算机设备编程或配置以执行权利要求1-8中任意一项所述用于机器人的自适应启发式全局路径规划方法的步骤。
CN202210632462.XA 2022-06-07 2022-06-07 一种用于机器人的自适应启发式全局路径规划方法与系统 Active CN114705196B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210632462.XA CN114705196B (zh) 2022-06-07 2022-06-07 一种用于机器人的自适应启发式全局路径规划方法与系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210632462.XA CN114705196B (zh) 2022-06-07 2022-06-07 一种用于机器人的自适应启发式全局路径规划方法与系统

Publications (2)

Publication Number Publication Date
CN114705196A true CN114705196A (zh) 2022-07-05
CN114705196B CN114705196B (zh) 2022-08-30

Family

ID=82177641

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210632462.XA Active CN114705196B (zh) 2022-06-07 2022-06-07 一种用于机器人的自适应启发式全局路径规划方法与系统

Country Status (1)

Country Link
CN (1) CN114705196B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115981345A (zh) * 2023-03-17 2023-04-18 山东双力现代农业装备有限公司 一种基于视觉检测的收割机工作路径规划方法
CN116610129A (zh) * 2023-07-17 2023-08-18 山东优宝特智能机器人有限公司 一种腿足机器人的局部路径规划方法及系统

Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20120283948A1 (en) * 2011-05-03 2012-11-08 University Of Southern California Hierarchical and Exact Fastest Path Computation in Time-dependent Spatial Networks
WO2017113569A1 (zh) * 2015-12-30 2017-07-06 深圳大学 一种基于a星策略的最优多会合点路径搜索方法及装置
WO2017173990A1 (zh) * 2016-04-07 2017-10-12 北京进化者机器人科技有限公司 一种机器人避障中的最短路径规划方法
CN108681787A (zh) * 2018-04-28 2018-10-19 南京航空航天大学 基于改进双向快速扩展随机树算法的无人机路径优化方法
CN109115226A (zh) * 2018-09-01 2019-01-01 哈尔滨工程大学 基于跳点搜索的多机器人冲突避免的路径规划方法
KR102035864B1 (ko) * 2018-09-07 2019-10-23 정완식 다중 배송 플랫폼에서의 최단 운송경로 탐색 서비스 제공 방법
CN111678523A (zh) * 2020-06-30 2020-09-18 中南大学 一种基于star算法优化的快速bi_rrt避障轨迹规划方法
US20210041577A1 (en) * 2019-08-08 2021-02-11 United States Of America As Represented By Secretary Of The Navy System And Methods For Planning Routes Over Large Areas
CN113295166A (zh) * 2021-05-12 2021-08-24 广州达泊智能科技有限公司 Agv路径规划方法、电子装置和计算机可读存储介质
CN113485360A (zh) * 2021-07-29 2021-10-08 福州大学 基于改进搜索算法的agv机器人路径规划方法及系统
CN114510056A (zh) * 2022-02-21 2022-05-17 上海机器人产业技术研究院有限公司 室内移动机器人的平稳移动全局路径规划方法

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20120283948A1 (en) * 2011-05-03 2012-11-08 University Of Southern California Hierarchical and Exact Fastest Path Computation in Time-dependent Spatial Networks
WO2017113569A1 (zh) * 2015-12-30 2017-07-06 深圳大学 一种基于a星策略的最优多会合点路径搜索方法及装置
WO2017173990A1 (zh) * 2016-04-07 2017-10-12 北京进化者机器人科技有限公司 一种机器人避障中的最短路径规划方法
CN108681787A (zh) * 2018-04-28 2018-10-19 南京航空航天大学 基于改进双向快速扩展随机树算法的无人机路径优化方法
CN109115226A (zh) * 2018-09-01 2019-01-01 哈尔滨工程大学 基于跳点搜索的多机器人冲突避免的路径规划方法
KR102035864B1 (ko) * 2018-09-07 2019-10-23 정완식 다중 배송 플랫폼에서의 최단 운송경로 탐색 서비스 제공 방법
US20210041577A1 (en) * 2019-08-08 2021-02-11 United States Of America As Represented By Secretary Of The Navy System And Methods For Planning Routes Over Large Areas
CN111678523A (zh) * 2020-06-30 2020-09-18 中南大学 一种基于star算法优化的快速bi_rrt避障轨迹规划方法
CN113295166A (zh) * 2021-05-12 2021-08-24 广州达泊智能科技有限公司 Agv路径规划方法、电子装置和计算机可读存储介质
CN113485360A (zh) * 2021-07-29 2021-10-08 福州大学 基于改进搜索算法的agv机器人路径规划方法及系统
CN114510056A (zh) * 2022-02-21 2022-05-17 上海机器人产业技术研究院有限公司 室内移动机器人的平稳移动全局路径规划方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
LIANGBING FENG 等: "Pheromone based alternative route planning", 《DIGITAL COMMUNICATIONS AND NETWORKS》 *
李逸斐 等: "基于改进RRT*算法的城市低空路径规划方法研究", 《地球信息科学学报》 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115981345A (zh) * 2023-03-17 2023-04-18 山东双力现代农业装备有限公司 一种基于视觉检测的收割机工作路径规划方法
CN116610129A (zh) * 2023-07-17 2023-08-18 山东优宝特智能机器人有限公司 一种腿足机器人的局部路径规划方法及系统

Also Published As

Publication number Publication date
CN114705196B (zh) 2022-08-30

Similar Documents

Publication Publication Date Title
CN109115226B (zh) 基于跳点搜索的多机器人冲突避免的路径规划方法
CN110333714B (zh) 一种无人驾驶汽车路径规划方法和装置
CN114705196B (zh) 一种用于机器人的自适应启发式全局路径规划方法与系统
CN113219998B (zh) 一种基于改进双向informed-RRT*的车辆路径规划方法
CN110887484B (zh) 基于改进遗传算法的移动机器人路径规划方法及存储介质
CN110231824B (zh) 基于直线偏离度方法的智能体路径规划方法
CN106371445B (zh) 一种基于拓扑地图的无人车规划控制方法
CN109579854B (zh) 基于快速扩展随机树的无人车避障方法
CN112327850B (zh) 一种水面无人艇路径规划方法
CN110319837A (zh) 一种服务机器人室内复杂状况路径规划方法
CN112904869B (zh) 基于多树rrt的无人艇加权迭代路径规划方法及设备
CN109163722B (zh) 一种仿人机器人路径规划方法及装置
CN104457775A (zh) 路径确定方法、装置和导航仪
Du et al. An improved RRT-based motion planner for autonomous vehicle in cluttered environments
CN113682318B (zh) 车辆行驶控制方法及装置
CN110487290B (zh) 基于变步长a星搜索的无人驾驶汽车局部路径规划方法
CN113467476B (zh) 考虑转角约束的无碰撞检测快速随机树全局路径规划方法
CN109556609B (zh) 一种基于人工智能的避碰方法及装置
Sundarraj et al. Route planning for an autonomous robotic vehicle employing a weight-controlled particle swarm-optimized Dijkstra algorithm
Benelmir et al. An efficient autonomous vehicle navigation scheme based on LiDAR sensor in vehicular network
CN112197783B (zh) 一种考虑车头指向的两阶段多抽样的rrt路径规划方法
CN115826586B (zh) 一种融合全局算法和局部算法的路径规划方法及系统
CN116560360A (zh) 面向复杂动态场景的医用护理机器人实时动态路径规划方法及系统
CN113110530B (zh) 一种针对三维环境下的水下机器人路径规划方法
Liu et al. Deep Reinforcement Learning-Based Driving Policy at Intersections Utilizing Lane Graph Networks

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
TR01 Transfer of patent right
TR01 Transfer of patent right

Effective date of registration: 20230714

Address after: 410001 No. 002, Floor 5, Building B, No. 10, Zone 2, CSCEC Smart Industrial Park, No. 50, Jinjiang Road, Yuelu Street, Yuelu District, Changsha, Hunan Province

Patentee after: Hunan Xinxin Xiangrong Intelligent Technology Co.,Ltd.

Address before: Yuelu District City, Hunan province 410082 Changsha Lushan Road No. 1

Patentee before: HUNAN University