CN114509085A - Quick path searching method combining grid and topological map - Google Patents

Quick path searching method combining grid and topological map 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
Chinese (zh)
Other versions
CN114509085B (en
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/en
Publication of CN114509085A publication Critical patent/CN114509085A/en
Application granted granted Critical
Publication of CN114509085B publication Critical patent/CN114509085B/en
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

Abstract

The invention belongs to the technical field of automation, and provides a rapid path searching method combining a grid map and a topological map. The topological map is used for expressing the connectivity and accessibility of the whole environment on a large scale, has great advantages on large-scale path search, and avoids huge calculation burden brought by pixel traversal on the whole raster map. Grid maps are used to complement the lack of topological maps in path details on a small scale. Based on the idea of combining the grid map and the topological map, the method greatly reduces the calculation of pixel connectivity traversal in the grid map, obviously improves the convergence speed of the algorithm, reduces the space complexity and the time complexity, enables the real-time path search to be possible in a large-scale scene, and can meet the requirements of the functionality, the real-time performance and the integrity of the autonomous navigation of the ground mobile robot.

Description

Quick path searching method combining grid and topological map
Technical Field
The invention belongs to the technical field of automation, and particularly relates to a path searching method for a ground mobile robot.
Background
With the increasing of the intelligent demands of related industries, the ground mobile robot is widely applied to the fields of intelligent park logistics, intelligent mines, building surveying, emergency search and rescue and the like, so that the autonomous navigation capability is one of the research hotspots of the robot technology. The path search is the core of the autonomous navigation technology, and is used for generating a passable path and ensuring that the robot safely and quickly moves in the environment to complete a given specific task. According to the existing research, the mobile robot path search technology has difficulties mainly in the following three aspects: (I) how to model the environment, (II) how to reduce the sampling space, and (III) how to improve the convergence efficiency of the search algorithm.
At present, some feasible methods have been successfully realized on simulation experiments and real robots, wherein the method based on greedy search and Rapid Random Tree (RRT) has good effect. The idea of a classical greedy search algorithm, such as a and D, is to iteratively traverse and grow passable pixel points from a starting point until a target point is found. The RRT-based method is equivalent to increase the step size of an A-algorithm, and the core idea is that starting from a starting point, a random tree is iteratively grown until the random tree finds a target point, the algorithm is high in efficiency, but the randomness of the algorithm causes that a final path may not be an optimal path, and longer-time optimization is needed to enable the path to converge.
Disclosure of Invention
The invention aims to solve the problem of how to design and realize a rapid ground mobile robot path search algorithm by combining a grid map and a topological map. The invention aims to improve the existing greedy search algorithm and graph search algorithm and utilize the characteristics of the greedy search algorithm and the graph search algorithm to realize real-time path search in a large-scale environment.
The technical scheme adopted by the invention is as follows:
a quick path searching method combining a grid and a topological map is suitable for a real-time autonomous navigation task of a ground mobile robot, and comprises the following steps:
step 1, extracting a simplified generalized Voronoi diagram G ═ E, V, M from a grid map M based on an improved K3M algorithmdistAs a topological map, where E is an edge, V is a node, MdistIs a distance matrix of the graph;
step 2, searching candidate starting edges PSE and candidate target edges PTE in the topological map obtained in the step 1 by using a grid map M;
step 3, using the candidate starting edge PSE and the candidate target edge PTE found in the step 2 as elicitations to search the candidate starting path Ta→GAnd candidate target path TG→b(ii) a For each pair of candidate start path and candidate target path therein
Figure BDA0003499546340000021
Finding connections in a topological map
Figure BDA0003499546340000022
And
Figure BDA0003499546340000023
sub-drawing of
Figure BDA0003499546340000024
Generating all candidate paths Ta→bAnd taking the path with the shortest length as a result path.
Further, the step 2 specifically comprises the following steps:
step 2.1, extracting an obstacle pixel point set O from the grid map M, as shown in formula 1:
Figure BDA0003499546340000031
in the formula, width is the width of the robot motion model, x is any pixel point in the grid map M, and x isiIs an obstacle pixel point in the grid map M;
step 2.2, finding out all candidate starting edge PSE and candidate target edge PTE in the topological map, as shown in formula 2 and formula 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)
wherein E isaAnd EbSets of edges, X, representing candidate start edges and candidate target edges, respectivelyaAnd XbRepresenting a set of corresponding via points, starting point laFrom a single point of approach
Figure BDA0003499546340000032
Corresponding candidate starting edges can be reached
Figure BDA0003499546340000033
lbFrom one point of approach
Figure BDA0003499546340000034
The corresponding candidate target edge can be reached
Figure BDA0003499546340000035
VT(laE) and VT (l)bAnd e) are respectively starting points laAnd end point lbVisibility detection function from edge e, its function prototype VT (x)mAnd e) is pixel point xmAnd e, see equation 4:
Figure BDA0003499546340000036
wherein the return value of the function
Figure BDA0003499546340000037
Is a distance x on the edge emNearest and xmVisual pixel dot, VT (x)m，xn) Is two pixels xmAnd xnSee equation 5:
Figure BDA0003499546340000038
wherein the content of the first and second substances,
Figure BDA0003499546340000039
representing a pixel point xmAnd xnThe line segment in between.
Further, step 3 specifically includes the following steps:
step 3.1, for all candidate starting edges PSE, combining the grid map M and the visibility detection function to find all candidate starting paths Ta→GAs shown in equation 6:
Figure BDA0003499546340000041
in the formula (I), the compound is shown in the specification,
Figure BDA0003499546340000042
is the t-th candidate starting edge,
Figure BDA0003499546340000043
is that
Figure BDA0003499546340000044
The corresponding passing-through point is arranged on the base,
Figure BDA0003499546340000045
and
Figure BDA0003499546340000046
respectively represent
Figure BDA0003499546340000047
The two nodes of the network node (c),
Figure BDA0003499546340000048
representing line segments
Figure BDA0003499546340000049
The path of the indication is such that,
Figure BDA00034995463400000410
representing line segments
Figure BDA00034995463400000411
The path of the indication is such that,
Figure BDA00034995463400000412
representing line segments
Figure BDA00034995463400000413
The indicated path;
Figure BDA00034995463400000414
representing along edge e, from
Figure BDA00034995463400000415
To the edge
Figure BDA00034995463400000416
The path segment of the 1 st node of (1) is an edge
Figure BDA00034995463400000417
A part of (a);
Figure BDA00034995463400000418
representing along edge e, from
Figure BDA00034995463400000419
To the edge
Figure BDA00034995463400000420
The path segment of the 2 nd node of (1), is an edge
Figure BDA00034995463400000421
A part of (a);
step 3.2, for all candidate target edges pTE, combining the grid map M and the visibility detection function to find all candidate target paths TG→bAs shown in equation 7:
Figure BDA00034995463400000422
in the formula (I), the compound is shown in the specification,
Figure BDA00034995463400000423
is the jth candidate target edge,
Figure BDA00034995463400000424
is that
Figure BDA00034995463400000425
The corresponding passing-through point is arranged on the base,
Figure BDA00034995463400000426
and
Figure BDA00034995463400000427
respectively represent
Figure BDA00034995463400000428
The two nodes of the network node (c),
Figure BDA00034995463400000429
representing line segments
Figure BDA00034995463400000430
The path of the indication is such that,
Figure BDA00034995463400000431
representing line segments
Figure BDA00034995463400000432
The path of the indication is such that,
Figure BDA00034995463400000433
representing line segments
Figure BDA00034995463400000434
The indicated path;
Figure BDA00034995463400000435
indicating a border
Figure BDA00034995463400000436
From the side
Figure BDA00034995463400000437
Figure BDA00034995463400000437
1 st node to
Figure BDA00034995463400000438
The path section of is an edge
Figure BDA00034995463400000439
A part of (a);
Figure BDA00034995463400000440
indicating a border
Figure BDA00034995463400000441
From the side
Figure BDA00034995463400000442
2 nd node to
Figure BDA00034995463400000443
The path section of is an edge
Figure BDA00034995463400000444
A part of (a);
step 3.3, find all regular candidate paths
Figure BDA00034995463400000445
As shown in equation 8:
Figure BDA0003499546340000051
in the formula (I), the compound is shown in the specification,
Figure BDA0003499546340000052
is a connection node in a topological map G
Figure BDA0003499546340000053
And
Figure BDA0003499546340000054
the subgraph of (1) can be searched by using Dijkstra algorithm;
step 3.4, conventional candidate Path in step 3.3
Figure BDA0003499546340000055
On the basis of (a), two cases need to be considered: (I) laAnd lbVisible, i.e. VT (l)a，lb) 1 is ═ 1; (II) edge set E of candidate starting edges PSEaEdge set E with candidate target edge PTEbThere is an intersection, i.e.
Figure BDA0003499546340000056
Thus all candidate paths Ta→bThree cases are included as shown in equation 9:
Figure BDA0003499546340000057
step 3.5, selecting candidate path Ta→bOne path with the shortest medium length
Figure BDA0003499546340000058
Figure BDA0003499546340000059
As a final path search result, where length (T)i) Representative route TiIs calculated using the euclidean distance.
Compared with the prior art, the invention has the following advantages:
the invention greatly reduces the calculation of pixel connectivity traversal in the grid map, avoids the randomness brought by the traditional algorithm based on fast exploration random tree, obviously improves the convergence speed of the algorithm, greatly reduces the space complexity and time complexity of the traditional method, enables the real-time path search to be possible in a large-scale scene, and can meet the requirements of the autonomous navigation of the ground mobile robot on functionality, real-time performance and integrity.
Drawings
Fig. 1 is a schematic diagram of a grid map M and a topological map G in the embodiment of the present invention.
FIG. 2 is a schematic diagram of a candidate start edge PSE and a candidate target edge PTE according to an embodiment of the present invention.
Fig. 3 is a schematic diagram of a candidate start path and a candidate target path according to an embodiment of the present invention.
FIG. 4 is a diagram illustrating a final path search result T according to an embodiment of the present inventiona→bSchematic representation.
Detailed Description
For the purpose of illustrating the technical contents, features and effects of the invention in detail, the following detailed description is given in conjunction with the accompanying drawings.
The invention provides a rapid path searching method combining a grid map and a topological map, wherein the topological map is generated from the grid map of the environment, and the path searching method of a mobile robot is designed and realized by combining the respective advantages of the two maps. The topological map can express the connectivity and accessibility of the whole environment on a large scale, has great advantages on large-scale path search, and avoids huge calculation burden caused by pixel traversal on the whole raster map. Grid maps are used to complement the lack of topological maps in path details on a small scale, since pixel traversal is only required over a small range of the grid map. The algorithm provided by the invention is based on the idea of combining the grid map and the topological map, the calculation of pixel connectivity traversal in the grid map is greatly reduced, meanwhile, the randomness caused by the traditional algorithm based on the Rapid-exploration Random Tree (RRT) is avoided, the convergence speed of the algorithm is obviously improved, the space complexity and the time complexity of the traditional method are greatly reduced, the real-time path search in a large-scale scene becomes possible, and the requirements of the functionality, the real-time performance and the integrity of the autonomous navigation of the ground mobile robot can be met.
The steps of the present invention are described in further detail below:
step 1, extracting a simplified generalized Voronoi diagram G (G is { E, V, M) } from a grid map M based on an improved K3M algorithmdtstAs a topological map, where E is an edge, V is a node, MdistFor the distance matrix of the map, as shown in fig. 1, in the topological map example, a white area is a passable area, a black area is an obstacle, a gray area is an unknown area, a solid line is an edge of the topological map, and a square is a node of the topological map. Specific algorithm reference patent (robot autonomous exploration method based on simplified generalized Voronoi diagram: CN 110703747A)]。
Step 2, searching candidate initial edges in the topological map obtained in the step 1 by using the grid map M
Figure BDA0003499546340000071
And candidate target edges
Figure BDA0003499546340000072
The result is shown in fig. 2, which illustrates candidate starting edges and candidate target edges. Starting point is laEnd point is lb。
Figure BDA0003499546340000073
As candidate starting points,
Figure BDA0003499546340000074
is a candidate target edge. The method comprises the following substeps:
step 2.1, extracting an obstacle pixel point set O from the grid map M, as shown in formula 1:
Figure BDA0003499546340000075
in the formula, width is the width of the robot motion model, x is any pixel point in the grid map M, and x isiIs an obstacle pixel point in the grid map M;
step 2.2, finding out all candidate starting edge PSE and candidate target edge PTE in the topological map, as shown in formula 2 and formula 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)
wherein E isaAnd EbSets of edges, X, representing candidate start edges and candidate target edges, respectivelyaAnd XbRepresenting a set of corresponding via points, starting point laFrom a single point of approach
Figure BDA0003499546340000076
Corresponding candidate starting edges can be reached
Figure BDA0003499546340000077
lbFrom a point of approach
Figure BDA0003499546340000078
The corresponding candidate target edge can be reached
Figure BDA00034995463400000715
For example, in FIG. 3,/aPathway(s)
Figure BDA00034995463400000710
Point to edge
Figure BDA00034995463400000711
Figure BDA00034995463400000712
lbPathway(s)
Figure BDA00034995463400000713
To the edge
Figure BDA00034995463400000714
VT(laE) and VT (l)bAnd e) are respectively starting points laAnd end point lbVisibility detection function from edge e, its function prototype VT (x)mAnd e) is pixel point xmAnd e, see equation 4:
Figure BDA0003499546340000081
wherein the return value of the function
Figure BDA0003499546340000082
Is a distance x on the edge emNearest and xmVisual pixel dot, VT (x)m，xn) Is two pixels xmAnd xnSee equation 5:
Figure BDA0003499546340000083
wherein the content of the first and second substances,
Figure BDA0003499546340000084
representing a pixel point xmAnd xnThe line segment in between.
Step 3, using the candidate starting edge PSE and the candidate target edge PTE found in the step 2 as elicitations to search the candidate starting path Ta→GAnd candidate target path TG→b(ii) a For each pair of candidate start path and candidate target path therein
Figure BDA0003499546340000085
Finding connections in a topological map
Figure BDA0003499546340000086
And
Figure BDA0003499546340000087
sub-drawing of
Figure BDA0003499546340000088
Generating all candidate paths Ta→bAnd taking the path with the shortest length as a result path. The method comprises the following substeps:
step 3.1, for all candidate starting edges PSE, combining the grid map M and the visibility detection function to find all candidate starting paths Ta→GAs shown in equation 6:
Figure BDA0003499546340000089
in the formula (I), the compound is shown in the specification,
Figure BDA00034995463400000810
is the ith candidate starting edge,
Figure BDA00034995463400000811
is that
Figure BDA00034995463400000812
The corresponding passing-through point is arranged on the base,
Figure BDA00034995463400000813
and
Figure BDA00034995463400000814
respectively represent
Figure BDA00034995463400000815
The two nodes of the network node (c),
Figure BDA00034995463400000816
representing line segments
Figure BDA00034995463400000817
The path of the indication is such that,
Figure BDA00034995463400000818
representing line segments
Figure BDA00034995463400000819
The path of the indication is such that,
Figure BDA00034995463400000820
representing line segments
Figure BDA00034995463400000821
The indicated path;
Figure BDA00034995463400000822
Figure BDA00034995463400000823
representing along edge e, from
Figure BDA00034995463400000824
To the edge
Figure BDA00034995463400000825
The path segment of the 1 st node of (1) is an edge
Figure BDA00034995463400000826
A part of (a);
Figure BDA0003499546340000091
representing along edge e, from
Figure BDA0003499546340000092
To the edge
Figure BDA0003499546340000093
The path segment of the 2 nd node of (1), is an edge
Figure BDA0003499546340000094
A part of (a); the result is shown in FIG. 3, the candidate start path Ta→RAGVG(left dotted line) and candidate target path TRAGVG→b(dotted right).
Step 3.2, for all candidate target edge PTEs, combining the grid map M and a visibility detection function to find all candidate target paths TG→bAs shown in equation 7:
Figure BDA0003499546340000095
in the formula (I), the compound is shown in the specification,
Figure BDA0003499546340000096
is the jth candidate target edge,
Figure BDA0003499546340000097
is that
Figure BDA0003499546340000098
The corresponding passing-through point is arranged on the base,
Figure BDA0003499546340000099
and
Figure BDA00034995463400000910
and respectively represent
Figure BDA00034995463400000911
The two nodes of the network node (c),
Figure BDA00034995463400000912
representing line segments
Figure BDA00034995463400000913
The path of the indication is such that,
Figure BDA00034995463400000914
representing line segments
Figure BDA00034995463400000915
The path of the indication is such that,
Figure BDA00034995463400000916
representing line segments
Figure BDA00034995463400000917
The indicated path;
Figure BDA00034995463400000918
indicating a border
Figure BDA00034995463400000919
From the side
Figure BDA00034995463400000920
Figure BDA00034995463400000920
1 st node to
Figure BDA00034995463400000921
The path section of is an edge
Figure BDA00034995463400000922
A part of (a);
Figure BDA00034995463400000923
indicating a border
Figure BDA00034995463400000924
From the side
Figure BDA00034995463400000925
2 nd node to
Figure BDA00034995463400000926
The path section of is an edge
Figure BDA00034995463400000927
A part of (a); the results are shown in FIG. 3.
Step 3.3, find all the conventional candidate paths
Figure BDA00034995463400000928
As shown in equation 8:
Figure BDA00034995463400000929
in the formula (I), the compound is shown in the specification,
Figure BDA00034995463400000930
is a connection node in a topological map G
Figure BDA00034995463400000931
And
Figure BDA00034995463400000932
the subgraph of (1) can be searched by using Dijkstra algorithm;
step 3.4, conventional candidate Path in step 3.3
Figure BDA00034995463400000933
On the basis of (a), two cases need to be considered: (I) laAnd lbVisible, i.e. VT (l)a，lb) 1 is ═ 1; (II) edge set E of candidate starting edges PSEaEdge set E with candidate target edge pTEbThere is an intersection, i.e.
Figure BDA0003499546340000101
Thus all candidate paths Ta→bThree cases are included as shown in equation 9:
Figure BDA0003499546340000102
step 3.5, selecting candidate path Ta→bOne path with the shortest medium length
Figure BDA0003499546340000103
Figure BDA0003499546340000104
As a final path search result, where length (T)i) Representative path TiIs calculated using the euclidean distance. As shown in fig. 4, connect l from left to rightaAnd lbThe dotted line of (b) is the final path search result.
The above description is only an example of the present invention and is not intended to limit the present invention. Any modification, improvement or the like made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims (3)

1. A quick path searching method combining a grid and a topological map is suitable for a real-time autonomous navigation task of a ground mobile robot, and is characterized by comprising the following steps:
step 1, extracting a simplified generalized Voronoi diagram G ═ E, V, M from a grid map M based on an improved K3M algorithmdistAs a topological map, where E is an edge, V is a node, MdistIs a distance matrix of the graph;
step 2, searching candidate starting edges PSE and candidate target edges PTE in the topological map obtained in the step 1 by using a grid map M;
step 3, using the candidate starting edge PSE and the candidate target edge PTE found in the step 2 as elicitations to search the candidate starting path Ta→GAnd candidate target path TG→b(ii) a For each pair of candidate start path and candidate target path therein
Figure FDA0003499546330000011
Finding connections in a topological map
Figure FDA0003499546330000012
And
Figure FDA0003499546330000013
sub-drawing of
Figure FDA0003499546330000014
Generating all candidate paths Ta→bAnd taking the path with the shortest length as a result path.
2. The method for fast path search by combining a grid and a topological map according to claim 1, wherein the step 2 specifically comprises the following steps:
step 2.1, extracting an obstacle pixel point set O from the grid map M, as shown in formula 1:
Figure FDA0003499546330000015
in the formula, width is the width of the robot motion model, x is any pixel point in the grid map M, and x isiIs an obstacle pixel point in the grid map M;
step 2.2, finding out all candidate starting edge PSE and candidate target edge PTE in the topological map, as shown in formula 2 and formula 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)
wherein E isaAnd EbSets of edges, X, representing candidate start edges and candidate target edges, respectivelyaAnd XbRepresenting a set of corresponding via points, starting point laFrom a single point of approach
Figure FDA0003499546330000021
Corresponding candidate starting edges can be reached
Figure FDA0003499546330000022
End point lbFrom one point of approach
Figure FDA0003499546330000023
The corresponding candidate target edge can be reached
Figure FDA0003499546330000024
VT(laE) and VT (l)bE) division ofIs respectively a starting point laAnd end point lbVisibility detection function from edge e, its function prototype VT (x)mAnd e) is pixel point xmAnd e, as shown in equation 4:
Figure FDA0003499546330000025
wherein the return value of the function
Figure FDA00034995463300000210
Is a distance x on the edge emNearest and xmVisual pixel dot, VT (x)m，xn) Is two pixels xmAnd xnThe visibility detection function in between, as equation 5:
Figure FDA0003499546330000027
wherein the content of the first and second substances,
Figure FDA0003499546330000028
representing a pixel point xmAnd xnThe line segment in between.
3. The method for fast path search by combining a grid and a topological map according to claim 2, wherein step 3 specifically comprises the following steps:
step 3.1, for all candidate starting edges PSE, combining the grid map M and the visibility detection function to find all candidate starting paths Ta→GAs shown in equation 6:
Figure FDA0003499546330000029
in the formula (I), the compound is shown in the specification,
Figure FDA0003499546330000031
is the t-th candidate starting edge,
Figure FDA0003499546330000032
is that
Figure FDA0003499546330000033
The corresponding passing-through point is arranged on the surface of the steel strip,
Figure FDA0003499546330000034
and
Figure FDA0003499546330000035
respectively represent
Figure FDA0003499546330000036
The two nodes of the network node (c),
Figure FDA0003499546330000037
representing line segments
Figure FDA0003499546330000038
The path of the indication is such that,
Figure FDA0003499546330000039
representing line segments
Figure FDA00034995463300000310
The path of the indication is such that,
Figure FDA00034995463300000311
representing line segments
Figure FDA00034995463300000312
The indicated path;
Figure FDA00034995463300000313
representing along edge e, from
Figure FDA00034995463300000314
To the edge
Figure FDA00034995463300000315
The path segment of the 1 st node of (1) is an edge
Figure FDA00034995463300000316
A part of (a);
Figure FDA00034995463300000317
representing along edge e, from
Figure FDA00034995463300000318
To the edge
Figure FDA00034995463300000319
The path segment of the 2 nd node of (1), is an edge
Figure FDA00034995463300000320
A part of (a);
step 3.2, for all candidate target edge PTEs, combining the grid map M and a visibility detection function to find all candidate target paths TG→bAs shown in equation 7:
Figure FDA00034995463300000321
in the formula (I), the compound is shown in the specification,
Figure FDA00034995463300000322
is the jth candidate target edge,
Figure FDA00034995463300000323
is that
Figure FDA00034995463300000324
The corresponding passing-through point is arranged on the base,
Figure FDA00034995463300000325
and
Figure FDA00034995463300000326
respectively represent
Figure FDA00034995463300000327
The two nodes of the network node (c),
Figure FDA00034995463300000328
representing line segments
Figure FDA00034995463300000329
The path of the indication is such that,
Figure FDA00034995463300000330
representing line segments
Figure FDA00034995463300000331
The path of the indication is such that,
Figure FDA00034995463300000332
representing line segments
Figure FDA00034995463300000333
The indicated path;
Figure FDA00034995463300000334
indicating a border
Figure FDA00034995463300000335
From the side
Figure FDA00034995463300000336
1 st node to
Figure FDA00034995463300000337
Path segment ofIs a side
Figure FDA00034995463300000338
A part of (a);
Figure FDA00034995463300000339
indicating a border
Figure FDA00034995463300000340
From the side
Figure FDA00034995463300000341
2 nd node to
Figure FDA00034995463300000342
The path section of is an edge
Figure FDA00034995463300000343
A part of (a);
step 3.3, find all the conventional candidate paths
Figure FDA00034995463300000344
As shown in equation 8:
Figure FDA00034995463300000345
in the formula (I), the compound is shown in the specification,
Figure FDA00034995463300000346
is a connection node in a topological map G
Figure FDA00034995463300000347
And
Figure FDA00034995463300000348
the subgraph of (1) can be searched by using Dijkstra algorithm;
step 3.4, usual in step 3.3Rule candidate path
Figure FDA00034995463300000349
On the basis of (1), two situations need to be considered: (I) laAnd lbVisual, i.e. VT (l)a，lb) 1 is ═ 1; (II) edge set E of candidate starting edges PSEaEdge set E with candidate target edge PTEbThere is an intersection, i.e.
Figure FDA0003499546330000041
Thus all candidate paths Ta→bThree cases are included as shown in equation 9:
Figure FDA0003499546330000042
step 3.5, selecting candidate path Ta→bOne path with the shortest medium length
Figure FDA0003499546330000043
Figure FDA0003499546330000044
As a final path search result, where length (T)i) Representative path TiIs calculated using the euclidean distance.
CN202210123852.4A 2022-02-10 2022-02-10 Quick path searching method combining grid and topological map Active CN114509085B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210123852.4A CN114509085B (en) 2022-02-10 2022-02-10 Quick path searching method combining grid and topological map

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210123852.4A CN114509085B (en) 2022-02-10 2022-02-10 Quick path searching method combining grid and topological map

Publications (2)

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

Family

ID=81551842

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210123852.4A Active CN114509085B (en) 2022-02-10 2022-02-10 Quick path searching method combining grid and topological map

Country Status (1)

Country Link
CN (1) CN114509085B (en)

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106970614A (en) * 2017-03-10 2017-07-21 江苏物联网研究发展中心 The construction method of improved trellis topology semantic environment map
CN107862738A (en) * 2017-11-28 2018-03-30 武汉大学 One kind carries out doors structure three-dimensional rebuilding method based on mobile laser measurement point cloud
WO2018113451A1 (en) * 2016-12-22 2018-06-28 沈阳美行科技有限公司 Map data system, method for generating and using same, and application thereof
CN108549378A (en) * 2018-05-02 2018-09-18 长沙学院 A kind of mixed path method and system for planning based on grating map
CN110703747A (en) * 2019-10-09 2020-01-17 武汉大学 Robot autonomous exploration method based on simplified generalized Voronoi diagram
CN111780775A (en) * 2020-06-17 2020-10-16 深圳优地科技有限公司 Path planning method and device, robot and storage medium
CN112683275A (en) * 2020-12-24 2021-04-20 哈尔滨工业大学芜湖机器人产业技术研究院 Path planning method of grid map
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 (en) * 2016-12-22 2018-06-28 沈阳美行科技有限公司 Map data system, method for generating and using same, and application thereof
CN106970614A (en) * 2017-03-10 2017-07-21 江苏物联网研究发展中心 The construction method of improved trellis topology semantic environment map
CN107862738A (en) * 2017-11-28 2018-03-30 武汉大学 One kind carries out doors structure three-dimensional rebuilding method based on mobile laser measurement point cloud
CN108549378A (en) * 2018-05-02 2018-09-18 长沙学院 A kind of mixed path method and system for planning based on grating map
US20210333108A1 (en) * 2018-12-28 2021-10-28 Goertek Inc. Path Planning Method And Device And Mobile Device
CN110703747A (en) * 2019-10-09 2020-01-17 武汉大学 Robot autonomous exploration method based on simplified generalized Voronoi diagram
CN111780775A (en) * 2020-06-17 2020-10-16 深圳优地科技有限公司 Path planning method and device, robot and storage medium
CN112683275A (en) * 2020-12-24 2021-04-20 哈尔滨工业大学芜湖机器人产业技术研究院 Path planning method of grid map

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》 *
余等: "基于栅格地图的分层式机器人路径规划算法", 《中国科学院大学学报》 *
张波涛等: "基于栅格-几何混合地图的移动机器人分层路径规划", 《华东理工大学学报(自然科学版)》 *

Also Published As

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

Similar Documents

Publication Publication Date Title
KR102125959B1 (en) Method and apparatus for determining a matching relationship between point cloud data
CN110021072B (en) Holographic mapping-oriented multi-platform point cloud intelligent processing method
CN106017472A (en) Global path planning method, global path planning system and unmanned aerial vehicle
CN110057362B (en) Mobile robot path planning method of limited unit map
CN105953785A (en) Map representation method for robot indoor autonomous navigation
CN113706710B (en) Virtual point multi-source point cloud fusion method and system based on FPFH characteristic difference
CN112184736A (en) Multi-plane extraction method based on European clustering
CN107544502A (en) A kind of Planning of Mobile Robot under known environment
CN113704381B (en) Road network data processing method and device, computer equipment and storage medium
CN113804209A (en) High-precision long-distance off-road path planning method for four-corner grid
Gu et al. An improved RRT algorithm based on prior AIS information and DP compression for ship path planning
CN113325389A (en) Unmanned vehicle laser radar positioning method, system and storage medium
CN114839968A (en) Unmanned surface vehicle path planning method
CN106708049A (en) Path planning method of moving body under multi-station relay navigation
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
CN114509085B (en) Quick path searching method combining grid and topological map
CN117053779A (en) Tightly coupled laser SLAM method and device based on redundant key frame removal
CN114882339B (en) Coal mine roadway eyelet autonomous identification method based on real-time dense point cloud map
Zhang et al. Accurate real-time SLAM based on two-step registration and multimodal loop detection
CN116795101A (en) Motion planning method integrating improved A and improved DWA algorithm
CN107480804B (en) Maze solving method based on line-surface spatial relation
Williams et al. A rapid method for planning paths in three dimensions for a small aerial robot

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