Nothing Special   »   [go: up one dir, main page]

CN114509085A - 一种结合栅格和拓扑地图的快速路径搜索方法 - Google Patents

一种结合栅格和拓扑地图的快速路径搜索方法 Download PDF

Info

Publication number
CN114509085A
CN114509085A CN202210123852.4A CN202210123852A CN114509085A CN 114509085 A CN114509085 A CN 114509085A CN 202210123852 A CN202210123852 A CN 202210123852A CN 114509085 A CN114509085 A CN 114509085A
Authority
CN
China
Prior art keywords
path
edge
candidate
map
grid
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
CN202210123852.4A
Other languages
English (en)
Other versions
CN114509085B (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.)
CETC 54 Research Institute
Original Assignee
CETC 54 Research Institute
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 CETC 54 Research Institute filed Critical CETC 54 Research Institute
Priority to CN202210123852.4A priority Critical patent/CN114509085B/zh
Publication of CN114509085A publication Critical patent/CN114509085A/zh
Application granted granted Critical
Publication of CN114509085B publication Critical patent/CN114509085B/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/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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明属于自动化技术领域，提供了一种结合栅格和拓扑地图的快速路径搜索方法，从环境的栅格地图中生成拓扑地图，结合两种地图各自的优势来设计和实现移动机器人路径搜索。拓扑地图用于在大尺度上表达整个环境的连通性和可达性，在大范围路径搜索上具有较大优势，避免了在整个栅格地图上进行像素遍历带来的巨大计算负担。栅格地图用于在小尺度上补足拓扑地图在路径细节上的缺失。本发明基于栅格和拓扑地图结合的思想，大幅度减少了在栅格地图中进行像素连通性遍历的计算，显著提高了算法的收敛速度，降低了空间复杂度和时间复杂度，使得在大尺度场景下进行实时路径搜索成为可能，可以满足地面移动机器人自主导航的功能性、实时性和完整性需求。

Description

一种结合栅格和拓扑地图的快速路径搜索方法
技术领域
本发明属于自动化技术领域，特别涉及地面移动机器人路径搜索方法。
背景技术
随着相关产业智能化需求的不断增多，地面移动机器人被广泛应用于智能园区物流、智能矿山、建筑勘测、应急搜救等领域，因此自主导航能力是机器人技术的研究热点之一。其中，路径搜索是自主导航技术的核心，用于生成可通行路径，保障机器人在环境中安全快速地移动，以完成给定的具体任务。根据现有研究，移动机器人路径搜索技术主要在以下三个方面存在困难：(I)如何对环境进行建模，(II)如何减小采样空间，(III)如何提高搜索算法的收敛效率。
目前，已经在仿真实验和真实机器人上成功实现了一些可行的方法，其中基于贪婪搜索和快速探索随机树(Rapid-exploration Random Tree，RRT)的方法已经有了较好的效果。经典的贪婪搜索算法，例如A*、D*，其思想是从起点出发，迭代地遍历并生长可通行像素点，直到找到目标点，该类算法实现简单，但效率极低，难以应对类似迷宫的环境，收敛速度慢。基于RRT的方法相当于加大了A*算法的步长，其核心思想是从起点出发，在迭代地生长一个随机树，直到随机树找到目标点，该类算法效率较高，但其随机性导致最终的路径可能不是最优路径，需要更长时间的优化来使得路径收敛。
发明内容
本发明所要解决的问题是，如何结合栅格地图和拓扑地图，设计和实现快速的地面移动机器人路径搜索算法。本发明的目的是改进现有贪婪搜索算法和图搜索算法并利用其特点，实现大尺度环境下的实时路径搜索。
本发明采用的技术方案为：
一种结合栅格和拓扑地图的快速路径搜索方法，适用于地面移动机器人实时自主导航任务，包括以下步骤：
步骤1，基于改进的K3M算法，从栅格地图M中提取简化广义Voronoi图G＝{E，V，Mdist}作为拓扑地图，其中E为边，V为节点，Mdist为图的距离矩阵；
步骤2，利用栅格地图M，在步骤1中得到的拓扑地图中，搜索候选起始边PSE和候选目标边PTE；
步骤3，以步骤2中找到的候选起始边PSE和候选目标边PTE为启发，查找候选起始路径Ta→G和候选目标路径TG→b；对于其中的每一对候选起始路径和候选目标路径
Figure BDA0003499546340000021
在拓扑地图中查找连接
Figure BDA0003499546340000022
和
Figure BDA0003499546340000023
的子图
Figure BDA0003499546340000024
生成所有候选路径Ta→b，取其中长度最短的路径作为结果路径。
进一步的，步骤2具体包括以下步骤：
步骤2.1，从栅格地图M中提取障碍物像素点集合O，如公式1所示：
Figure BDA0003499546340000031
式中，width为机器人运动模型的宽度，x是栅格地图M中的任一像素点，xi是栅格地图M中的障碍物像素点；
步骤2.2，在拓扑地图中，找到所有候选起始边PSE和候选目标边PTE，如公式2和公式3所示：
PSE＝(Ea，Xa)＝{(e∈E，x∈e)|VT(la，e)≠none，x＝VT(la，e)} (2)
PTE＝(Eb，Xb)＝{(e∈E，x∈e)|VT(lb，e)≠none，x＝VT(lb，e)} (3)
其中，Ea和Eb分别表示候选起始边和候选目标边的边集合，Xa和Xb表示对应的途经点的集合，起点la从一个途经点
Figure BDA0003499546340000032
可以到达对应的候选起始边
Figure BDA0003499546340000033
lb从一个途径点
Figure BDA0003499546340000034
可以到达对应的候选目标边
Figure BDA0003499546340000035
VT(la，e)和VT(lb，e)分别为起点la和终点lb与边e之间的可视性检测函数，其函数原型VT(xm，e)为像素点xm与边e之间的可视性检测函数，见公式4：
Figure BDA0003499546340000036
其中，函数的返回值
Figure BDA0003499546340000037
为边e上距离xm最近且与xm可视的像素点，VT(xm，xn)为两个像素点xm和xn之间的可视性检测函数，见公式5：
Figure BDA0003499546340000038
其中，
Figure BDA0003499546340000039
表示像素点xm和xn之间的线段。
进一步的，步骤3具体包括以下步骤：
步骤3.1，对于所有候选起始边PSE，结合栅格地图M和可视性检测函数，找到所有候选起始路径Ta→G，如公式6所示：
Figure BDA0003499546340000041
式中，
Figure BDA0003499546340000042
是第t个候选起始边，
Figure BDA0003499546340000043
是
Figure BDA0003499546340000044
对应的途经点，
Figure BDA0003499546340000045
和
Figure BDA0003499546340000046
分别表示
Figure BDA0003499546340000047
的两个节点，
Figure BDA0003499546340000048
表示线段
Figure BDA0003499546340000049
指示的路径，
Figure BDA00034995463400000410
表示线段
Figure BDA00034995463400000411
指示的路径，
Figure BDA00034995463400000412
表示线段
Figure BDA00034995463400000413
指示的路径；
Figure BDA00034995463400000414
表示沿边e的，从
Figure BDA00034995463400000415
到边
Figure BDA00034995463400000416
的第1个节点的路径段，是边
Figure BDA00034995463400000417
的一部分；
Figure BDA00034995463400000418
表示沿边e的，从
Figure BDA00034995463400000419
到边
Figure BDA00034995463400000420
的第2个节点的路径段，是边
Figure BDA00034995463400000421
的一部分；
步骤3.2，对于所有候选目标边pTE，结合栅格地图M和可视性检测函数，找到所有的候选目标路径TG→b，如公式7所示：
Figure BDA00034995463400000422
式中，
Figure BDA00034995463400000423
是第j个候选目标边，
Figure BDA00034995463400000424
是
Figure BDA00034995463400000425
对应的途经点，
Figure BDA00034995463400000426
和
Figure BDA00034995463400000427
分别表示
Figure BDA00034995463400000428
的两个节点，
Figure BDA00034995463400000429
表示线段
Figure BDA00034995463400000430
指示的路径，
Figure BDA00034995463400000431
表示线段
Figure BDA00034995463400000432
指示的路径，
Figure BDA00034995463400000433
表示线段
Figure BDA00034995463400000434
指示的路径；
Figure BDA00034995463400000435
表示沿边
Figure BDA00034995463400000436
的，从边
Figure BDA00034995463400000437
的第1个节点到
Figure BDA00034995463400000438
的路径段，是边
Figure BDA00034995463400000439
的一部分；
Figure BDA00034995463400000440
表示沿边
Figure BDA00034995463400000441
的，从边
Figure BDA00034995463400000442
的第2个节点到
Figure BDA00034995463400000443
的路径段，是边
Figure BDA00034995463400000444
的一部分；
步骤3.3，找到所有的常规候选路径
Figure BDA00034995463400000445
如公式8所示：
Figure BDA0003499546340000051
式中，
Figure BDA0003499546340000052
是拓扑地图G中连接节点
Figure BDA0003499546340000053
和
Figure BDA0003499546340000054
的子图，可用Dijkstra算法查找；
步骤3.4，在步骤3.3中的常规候选路径
Figure BDA0003499546340000055
的基础上，还需要考虑两种情况：(I)la和lb可视，即VT(la，lb)＝1；(II)候选起始边PSE的边集合Ea与候选目标边PTE的边集合Eb存在交集，即
Figure BDA0003499546340000056
因此所有的候选路径Ta→b包括如公式9所示的三种情况：
Figure BDA0003499546340000057
步骤3.5，选取候选路径Ta→b中长度最短的一条路径
Figure BDA0003499546340000058
Figure BDA0003499546340000059
作为最终的路径搜索结果，其中length(Ti)代表路径Ti的长度，使用欧氏距离来计算。
本发明相比现有技术具有如下优点：
本发明大幅度减少了在栅格地图中进行像素连通性遍历的计算，同时避免了传统的基于快速探索随机树算法带来的随机性，显著提高了算法的收敛速度，极大程度地降低了传统方法的空间复杂度和时间复杂度，使得在大尺度场景下进行实时路径搜索成为可能，可以满足地面移动机器人自主导航的功能性、实时性和完整性需求。
附图说明
图1为本发明实施例中的栅格地图M和拓扑地图G示意图。
图2为本发明实施例中的候选起始边PSE和候选目标边PTE示意图。
图3为本发明实施例中的候选起始路径和候选目标路径示意图。
图4为本发明实施例中的最终路径搜索结果Ta→b示意图。
具体实施方式
为详细说明本发明的技术内容、特点和实现效果，以下结合具体实施方式并配合附图详细予以说明。
本发明提供了一种结合栅格和拓扑地图的快速路径搜索方法，从环境的栅格地图中生成拓扑地图，结合两种地图各自的优势来设计和实现移动机器人路径搜索方法。拓扑地图能够在大尺度上表达整个环境的连通性和可达性，在大范围路径搜索上具有较大优势，避免了在整个栅格地图上进行像素遍历带来的巨大计算负担。栅格地图用于在小尺度上补足拓扑地图在路径细节上的缺失，由于只需要在栅格地图的较小范围内进行像素遍历。本发明提出的算法基于栅格和拓扑地图结合的思想，大幅度减少了在栅格地图中进行像素连通性遍历的计算，同时避免了传统的基于快速探索随机树(Rapid-explorationRandom Tree，RRT)算法带来的随机性，显著提高了算法的收敛速度，极大程度地降低了传统方法的空间复杂度和时间复杂度，使得在大尺度场景下进行实时路径搜索成为可能，可以满足地面移动机器人自主导航的功能性、实时性和完整性需求。
下面对本发明步骤做进一步详细介绍：
步骤1、基于改进的K3M算法，从栅格地图M中提取简化广义Voronoi图G＝{E，V，Mdtst}作为拓扑地图，其中E为边，V为节点，Mdist为图的距离矩阵，如附图1所示，拓扑地图示例，白色区域为可通行区域，黑色区域为障碍物，灰色区域为未知区域，实线为拓扑地图的边，方块为拓扑地图的节点。具体算法参考专利[一种基于简化广义Voronoi图的机器人自主探索方法:CN110703747A]。
步骤2，利用栅格地图M，在步骤1中得到的拓扑地图中，搜索候选起始边
Figure BDA0003499546340000071
和候选目标边
Figure BDA0003499546340000072
结果如附图2所示，候选起始边和候选目标边示例。起点为la，终点为lb。
Figure BDA0003499546340000073
为候选起始,，
Figure BDA0003499546340000074
为候选目标边。包括以下子步骤：
步骤2.1，从栅格地图M中提取障碍物像素点集合O，如公式1所示：
Figure BDA0003499546340000075
式中，width为机器人运动模型的宽度，x是栅格地图M中的任一像素点，xi是栅格地图M中的障碍物像素点；
步骤2.2，在拓扑地图中，找到所有候选起始边PSE和候选目标边PTE，如公式2和公式3所示：
PSE＝(Ea，Xa)＝{(e∈E，x∈e)|VT(la，e)≠none，x＝VT(la，e)} (2)
PTE＝(Eb，Xb)＝{(e∈E，x∈e)|VT(lb，e)≠none，x＝VT(lb，e)} (3)
其中，Ea和Eb分别表示候选起始边和候选目标边的边集合，Xa和Xb表示对应的途经点的集合，起点la从一个途经点
Figure BDA0003499546340000076
可以到达对应的候选起始边
Figure BDA0003499546340000077
lb从一个途径点
Figure BDA0003499546340000078
可以到达对应的候选目标边
Figure BDA00034995463400000715
例如在附图3中，la途径
Figure BDA00034995463400000710
点到达边
Figure BDA00034995463400000711
Figure BDA00034995463400000712
lb途径
Figure BDA00034995463400000713
到达边
Figure BDA00034995463400000714
VT(la，e)和VT(lb，e)分别为起点la和终点lb与边e之间的可视性检测函数，其函数原型VT(xm，e)为像素点xm与边e之间的可视性检测函数，见公式4：
Figure BDA0003499546340000081
其中，函数的返回值
Figure BDA0003499546340000082
为边e上距离xm最近且与xm可视的像素点，VT(xm，xn)为两个像素点xm和xn之间的可视性检测函数，见公式5：
Figure BDA0003499546340000083
其中，
Figure BDA0003499546340000084
表示像素点xm和xn之间的线段。
步骤3，以步骤2中找到的候选起始边PSE和候选目标边PTE为启发，查找候选起始路径Ta→G和候选目标路径TG→b；对于其中的每一对候选起始路径和候选目标路径
Figure BDA0003499546340000085
在拓扑地图中查找连接
Figure BDA0003499546340000086
和
Figure BDA0003499546340000087
的子图
Figure BDA0003499546340000088
生成所有候选路径Ta→b，取其中长度最短的路径作为结果路径。包括以下几个子步骤：
步骤3.1，对于所有候选起始边PSE，结合栅格地图M和可视性检测函数，找到所有候选起始路径Ta→G，如公式6所示：
Figure BDA0003499546340000089
式中，
Figure BDA00034995463400000810
是第i个候选起始边，
Figure BDA00034995463400000811
是
Figure BDA00034995463400000812
对应的途经点，
Figure BDA00034995463400000813
和
Figure BDA00034995463400000814
分别表示
Figure BDA00034995463400000815
的两个节点，
Figure BDA00034995463400000816
表示线段
Figure BDA00034995463400000817
指示的路径，
Figure BDA00034995463400000818
表示线段
Figure BDA00034995463400000819
指示的路径，
Figure BDA00034995463400000820
表示线段
Figure BDA00034995463400000821
指示的路径；
Figure BDA00034995463400000822
Figure BDA00034995463400000823
表示沿边e的，从
Figure BDA00034995463400000824
到边
Figure BDA00034995463400000825
的第1个节点的路径段，是边
Figure BDA00034995463400000826
的一部分；
Figure BDA0003499546340000091
表示沿边e的，从
Figure BDA0003499546340000092
到边
Figure BDA0003499546340000093
的第2个节点的路径段，是边
Figure BDA0003499546340000094
的一部分；结果见附图3，候选起始路径Ta→RAGVG(左侧虚线)和候选目标路径TRAGVG→b(右侧虚线)。
步骤3.2，对于所有候选目标边PTE，结合栅格地图M和可视性检测函数，找到所有的候选目标路径TG→b，如公式7所示：
Figure BDA0003499546340000095
式中，
Figure BDA0003499546340000096
是第j个候选目标边，
Figure BDA0003499546340000097
是
Figure BDA0003499546340000098
对应的途经点，
Figure BDA0003499546340000099
和
Figure BDA00034995463400000910
和分别表示
Figure BDA00034995463400000911
的两个节点，
Figure BDA00034995463400000912
表示线段
Figure BDA00034995463400000913
指示的路径，
Figure BDA00034995463400000914
表示线段
Figure BDA00034995463400000915
指示的路径，
Figure BDA00034995463400000916
表示线段
Figure BDA00034995463400000917
指示的路径；
Figure BDA00034995463400000918
表示沿边
Figure BDA00034995463400000919
的，从边
Figure BDA00034995463400000920
的第1个节点到
Figure BDA00034995463400000921
的路径段，是边
Figure BDA00034995463400000922
的一部分；
Figure BDA00034995463400000923
表示沿边
Figure BDA00034995463400000924
的，从边
Figure BDA00034995463400000925
的第2个节点到
Figure BDA00034995463400000926
的路径段，是边
Figure BDA00034995463400000927
的一部分；结果见附图3。
步骤3.3，找到所有的常规候选路径
Figure BDA00034995463400000928
如公式8所示：
Figure BDA00034995463400000929
式中，
Figure BDA00034995463400000930
是拓扑地图G中连接节点
Figure BDA00034995463400000931
和
Figure BDA00034995463400000932
的子图，可用Dijkstra算法查找；
步骤3.4，在步骤3.3中的常规候选路径
Figure BDA00034995463400000933
的基础上，还需要考虑两种情况：(I)la和lb可视，即VT(la，lb)＝1；(II)候选起始边PSE的边集合Ea与候选目标边pTE的边集合Eb存在交集，即
Figure BDA0003499546340000101
因此所有的候选路径Ta→b包括如公式9所示的三种情况：
Figure BDA0003499546340000102
步骤3.5，选取候选路径Ta→b中长度最短的一条路径
Figure BDA0003499546340000103
Figure BDA0003499546340000104
作为最终的路径搜索结果，其中length(Ti)代表路径Ti的长度，使用欧氏距离来计算。如附图4所示，从左到右连接la与lb的虚线为最终的路径搜索结果。
以上所述仅为本发明中的一个实例，并不用于限制本发明。凡在本发明的精神与原则之内，所做的任何修改、改进等，均应包含在本发明的保护范围之内。

Claims (3)

1.一种结合栅格和拓扑地图的快速路径搜索方法，适用于地面移动机器人实时自主导航任务，其特征在于，包括以下步骤：
步骤1，基于改进的K3M算法，从栅格地图M中提取简化广义Voronoi图G＝{E，V，Mdist}作为拓扑地图，其中E为边，V为节点，Mdist为图的距离矩阵；
步骤2，利用栅格地图M，在步骤1中得到的拓扑地图中，搜索候选起始边PSE和候选目标边PTE；
步骤3，以步骤2中找到的候选起始边PSE和候选目标边PTE为启发，查找候选起始路径Ta→G和候选目标路径TG→b；对于其中的每一对候选起始路径和候选目标路径
Figure FDA0003499546330000011
在拓扑地图中查找连接
Figure FDA0003499546330000012
和
Figure FDA0003499546330000013
的子图
Figure FDA0003499546330000014
生成所有候选路径Ta→b，取其中长度最短的路径作为结果路径。
2.根据权利要求1所述的结合栅格和拓扑地图的快速路径搜索方法，其特征在于，步骤2具体包括以下步骤：
步骤2.1，从栅格地图M中提取障碍物像素点集合O，如公式1所示：
Figure FDA0003499546330000015
式中，width为机器人运动模型的宽度，x是栅格地图M中的任一像素点，xi是栅格地图M中的障碍物像素点；
步骤2.2，在拓扑地图中，找到所有候选起始边PSE和候选目标边PTE，如公式2和公式3所示：
PSE＝(Ea，Xa)＝{(e∈E，x∈e)|VT(la，e)≠none，x＝VT(la，e)} (2)
PTE＝(Eb，Xb)＝{(e∈E，x∈e)|VT(lb，e)≠none，x＝VT(lb，e)} (3)
其中，Ea和Eb分别表示候选起始边和候选目标边的边集合，Xa和Xb表示对应的途经点的集合，起点la从一个途经点
Figure FDA0003499546330000021
可以到达对应的候选起始边
Figure FDA0003499546330000022
终点lb从一个途径点
Figure FDA0003499546330000023
可以到达对应的候选目标边
Figure FDA0003499546330000024
VT(la，e)和VT(lb，e)分别为起点la和终点lb与边e之间的可视性检测函数，其函数原型VT(xm，e)为像素点xm与边e之间的可视性检测函数，如公式4：
Figure FDA0003499546330000025
其中，函数的返回值
Figure FDA00034995463300000210
为边e上距离xm最近且与xm可视的像素点，VT(xm，xn)为两个像素点xm和xn之间的可视性检测函数，如公式5：
Figure FDA0003499546330000027
其中，
Figure FDA0003499546330000028
表示像素点xm和xn之间的线段。
3.根据权利要求2所述的结合栅格和拓扑地图的快速路径搜索方法，其特征在于，步骤3具体包括以下步骤：
步骤3.1，对于所有候选起始边PSE，结合栅格地图M和可视性检测函数，找到所有候选起始路径Ta→G，如公式6所示：
Figure FDA0003499546330000029
式中，
Figure FDA0003499546330000031
是第t个候选起始边，
Figure FDA0003499546330000032
是
Figure FDA0003499546330000033
对应的途经点，
Figure FDA0003499546330000034
和
Figure FDA0003499546330000035
分别表示
Figure FDA0003499546330000036
的两个节点，
Figure FDA0003499546330000037
表示线段
Figure FDA0003499546330000038
指示的路径，
Figure FDA0003499546330000039
表示线段
Figure FDA00034995463300000310
指示的路径，
Figure FDA00034995463300000311
表示线段
Figure FDA00034995463300000312
指示的路径；
Figure FDA00034995463300000313
表示沿边e的，从
Figure FDA00034995463300000314
到边
Figure FDA00034995463300000315
的第1个节点的路径段，是边
Figure FDA00034995463300000316
的一部分；
Figure FDA00034995463300000317
表示沿边e的，从
Figure FDA00034995463300000318
到边
Figure FDA00034995463300000319
的第2个节点的路径段，是边
Figure FDA00034995463300000320
的一部分；
步骤3.2，对于所有候选目标边PTE，结合栅格地图M和可视性检测函数，找到所有的候选目标路径TG→b，如公式7所示：
Figure FDA00034995463300000321
式中，
Figure FDA00034995463300000322
是第j个候选目标边，
Figure FDA00034995463300000323
是
Figure FDA00034995463300000324
对应的途经点，
Figure FDA00034995463300000325
和
Figure FDA00034995463300000326
分别表示
Figure FDA00034995463300000327
的两个节点，
Figure FDA00034995463300000328
表示线段
Figure FDA00034995463300000329
指示的路径，
Figure FDA00034995463300000330
表示线段
Figure FDA00034995463300000331
指示的路径，
Figure FDA00034995463300000332
表示线段
Figure FDA00034995463300000333
指示的路径；
Figure FDA00034995463300000334
表示沿边
Figure FDA00034995463300000335
的，从边
Figure FDA00034995463300000336
的第1个节点到
Figure FDA00034995463300000337
的路径段，是边
Figure FDA00034995463300000338
的一部分；
Figure FDA00034995463300000339
表示沿边
Figure FDA00034995463300000340
的，从边
Figure FDA00034995463300000341
的第2个节点到
Figure FDA00034995463300000342
的路径段，是边
Figure FDA00034995463300000343
的一部分；
步骤3.3，找到所有的常规候选路径
Figure FDA00034995463300000344
如公式8所示：
Figure FDA00034995463300000345
式中，
Figure FDA00034995463300000346
是拓扑地图G中连接节点
Figure FDA00034995463300000347
和
Figure FDA00034995463300000348
的子图，可用Dijkstra算法查找；
步骤3.4，在步骤3.3中的常规候选路径
Figure FDA00034995463300000349
的基础上，还需考虑两种情况：(I)la和lb可视，即VT(la，lb)＝1；(II)候选起始边PSE的边集合Ea与候选目标边PTE的边集合Eb存在交集，即
Figure FDA0003499546330000041
因此所有的候选路径Ta→b包括如公式9所示的三种情况：
Figure FDA0003499546330000042
步骤3.5，选取候选路径Ta→b中长度最短的一条路径
Figure FDA0003499546330000043
Figure FDA0003499546330000044
作为最终的路径搜索结果，其中length(Ti)代表路径Ti的长度，使用欧氏距离来计算。
CN202210123852.4A 2022-02-10 2022-02-10 一种结合栅格和拓扑地图的快速路径搜索方法 Active CN114509085B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210123852.4A CN114509085B (zh) 2022-02-10 2022-02-10 一种结合栅格和拓扑地图的快速路径搜索方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210123852.4A CN114509085B (zh) 2022-02-10 2022-02-10 一种结合栅格和拓扑地图的快速路径搜索方法

Publications (2)

Publication Number Publication Date
CN114509085A true CN114509085A (zh) 2022-05-17
CN114509085B CN114509085B (zh) 2022-11-01

Family

ID=81551842

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210123852.4A Active CN114509085B (zh) 2022-02-10 2022-02-10 一种结合栅格和拓扑地图的快速路径搜索方法

Country Status (1)

Country Link
CN (1) CN114509085B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117889866A (zh) * 2024-03-18 2024-04-16 深圳竹芒科技有限公司 导航路径规划方法、设备及存储介质

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106970614A (zh) * 2017-03-10 2017-07-21 江苏物联网研究发展中心 改进的栅格拓扑语义环境地图的构建方法
CN107862738A (zh) * 2017-11-28 2018-03-30 武汉大学 一种基于移动激光测量点云进行室内结构化三维重建方法
WO2018113451A1 (zh) * 2016-12-22 2018-06-28 沈阳美行科技有限公司 一种地图数据系统、其生成和使用方法及应用
CN108549378A (zh) * 2018-05-02 2018-09-18 长沙学院 一种基于栅格地图的混合路径规划方法和系统
CN110703747A (zh) * 2019-10-09 2020-01-17 武汉大学 一种基于简化广义Voronoi图的机器人自主探索方法
CN111780775A (zh) * 2020-06-17 2020-10-16 深圳优地科技有限公司 路径规划的方法、装置、机器人及存储介质
CN112683275A (zh) * 2020-12-24 2021-04-20 哈尔滨工业大学芜湖机器人产业技术研究院 一种栅格地图的路径规划方法
US20210333108A1 (en) * 2018-12-28 2021-10-28 Goertek Inc. Path Planning Method And Device And Mobile Device

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2018113451A1 (zh) * 2016-12-22 2018-06-28 沈阳美行科技有限公司 一种地图数据系统、其生成和使用方法及应用
CN106970614A (zh) * 2017-03-10 2017-07-21 江苏物联网研究发展中心 改进的栅格拓扑语义环境地图的构建方法
CN107862738A (zh) * 2017-11-28 2018-03-30 武汉大学 一种基于移动激光测量点云进行室内结构化三维重建方法
CN108549378A (zh) * 2018-05-02 2018-09-18 长沙学院 一种基于栅格地图的混合路径规划方法和系统
US20210333108A1 (en) * 2018-12-28 2021-10-28 Goertek Inc. Path Planning Method And Device And Mobile Device
CN110703747A (zh) * 2019-10-09 2020-01-17 武汉大学 一种基于简化广义Voronoi图的机器人自主探索方法
CN111780775A (zh) * 2020-06-17 2020-10-16 深圳优地科技有限公司 路径规划的方法、装置、机器人及存储介质
CN112683275A (zh) * 2020-12-24 2021-04-20 哈尔滨工业大学芜湖机器人产业技术研究院 一种栅格地图的路径规划方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
DALIN LI ET AL.: "A Multi-Type Features Method for Leg Detection in 2-D Laser Range Data", 《IEEE SENSORS JOURNAL》 *
余等: "基于栅格地图的分层式机器人路径规划算法", 《中国科学院大学学报》 *
张波涛等: "基于栅格-几何混合地图的移动机器人分层路径规划", 《华东理工大学学报(自然科学版)》 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117889866A (zh) * 2024-03-18 2024-04-16 深圳竹芒科技有限公司 导航路径规划方法、设备及存储介质
CN117889866B (zh) * 2024-03-18 2024-06-11 深圳竹芒科技有限公司 导航路径规划方法、设备及存储介质

Also Published As

Publication number Publication date
CN114509085B (zh) 2022-11-01

Similar Documents

Publication Publication Date Title
Niu et al. Voronoi-visibility roadmap-based path planning algorithm for unmanned surface vehicles
CN110021072B (zh) 面向全息测绘的多平台点云智能处理方法
Gu et al. An improved RRT algorithm based on prior AIS information and DP compression for ship path planning
CN106017472A (zh) 全局路线规划方法、全局路线规划系统及无人机
CN113706710B (zh) 基于fpfh特征差异的虚拟点多源点云融合方法及系统
CN110057362B (zh) 有限单元地图的移动机器人路径规划方法
CN112184736A (zh) 一种基于欧式聚类的多平面提取方法
CN113704381B (zh) 路网数据处理方法、装置、计算机设备和存储介质
CN114839968A (zh) 一种水面无人艇路径规划方法
CN107544502A (zh) 一种已知环境下的移动机器人规划方法
CN113325389A (zh) 一种无人车激光雷达定位方法、系统及存储介质
CN112161631A (zh) 一种基于大型卫星栅格地图的安全路径规划方法
CN114509085B (zh) 一种结合栅格和拓扑地图的快速路径搜索方法
CN113804209A (zh) 一种四角格网高精度长距离越野路径规划方法
Liu et al. Application of dijkstra algorithm in path planning for geomagnetic navigation
Kuang et al. Improved A-star algorithm based on topological maps for indoor mobile robot path planning
Ai et al. A map generalization model based on algebra mapping transformation
Chen et al. Trajectory optimization of LiDAR SLAM based on local pose graph
CN107480804B (zh) 一种基于线面空间关系的迷宫求解方法
CN113936109A (zh) 高精地图点云数据的处理方法、装置、设备以及存储介质
CN116661502B (zh) 一种智能农业无人机路径规划方法
CN114882339B (zh) 基于实时稠密点云地图的煤矿巷道孔眼自主识别方法
CN118068367A (zh) 一种融合优先探索和定时器机制的三维激光雷达导航方法
Williams et al. A rapid method for planning paths in three dimensions for a small aerial robot
CN114740898B (zh) 一种基于自由空间与a*算法的三维航迹规划方法

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