CN115469662B - Environment exploration method, device and application - Google Patents
Environment exploration method, device and application Download PDFInfo
- Publication number
- CN115469662B CN115469662B CN202211110233.8A CN202211110233A CN115469662B CN 115469662 B CN115469662 B CN 115469662B CN 202211110233 A CN202211110233 A CN 202211110233A CN 115469662 B CN115469662 B CN 115469662B
- Authority
- CN
- China
- Prior art keywords
- node
- nodes
- heuristic
- feature
- robot
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 29
- 238000001514 detection method Methods 0.000 claims abstract description 32
- 238000010586 diagram Methods 0.000 claims abstract description 31
- 239000011159 matrix material Substances 0.000 claims abstract description 21
- 238000004422 calculation algorithm Methods 0.000 claims description 27
- 230000008901 benefit Effects 0.000 claims description 26
- 230000007613 environmental effect Effects 0.000 claims description 15
- 238000000605 extraction Methods 0.000 claims description 11
- 230000004927 fusion Effects 0.000 claims description 10
- 238000010276 construction Methods 0.000 claims description 5
- 239000000284 extract Substances 0.000 claims 1
- 238000004364 calculation method Methods 0.000 abstract description 4
- 230000006870 function Effects 0.000 description 23
- 238000012959 renal replacement therapy Methods 0.000 description 11
- 238000005516 engineering process Methods 0.000 description 4
- 238000011156 evaluation Methods 0.000 description 4
- 238000002474 experimental method Methods 0.000 description 4
- 238000004088 simulation Methods 0.000 description 4
- 238000005070 sampling Methods 0.000 description 3
- 238000004590 computer program Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012952 Resampling Methods 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000013210 evaluation model Methods 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
- G05D1/0274—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means using mapping information stored in a memory device
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Aviation & Aerospace Engineering (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Manipulator (AREA)
Abstract
The invention relates to an environment exploration method, which comprises the steps of obtaining environment information, generating a generalized voronoi diagram and constructing a GVD node set; selecting nodes from the GVD node set to construct a heuristic boundary point set, fusing, removing redundant nodes, and generating a fused heuristic boundary point set; the GVD node set is fused, redundant nodes are removed, and the GVD characteristic node set is constructed; combining the GVD characteristic node set, the fused heuristic boundary point set and the current pose of the robot to obtain a GVD characteristic node complete set; performing collision detection on two optional feature nodes in the GVD feature node total set to obtain a feature matrix with side information; calculating the gain function value of each node in the fused boundary point set according to the information gain and the feature matrix; selecting a target node according to the gain function value, navigating to the target node, and updating map data; the exploration is repeated until the whole environment exploration is completed. By fusing boundary points, redundant nodes are removed, so that the calculation consumption in the process of robot boundary decision is reduced, backtracking is reduced, and the exploration efficiency is improved.
Description
Technical Field
The invention relates to the technical field of robot path planning, in particular to an environment exploration method, an environment exploration device and application.
Background
The rapid development of computer technology and artificial intelligence technology brings great breakthrough to the research of mobile robots, and the market scale of service robots in China is increased at a high speed. The autonomous exploration technology of the mobile robot is a key technology in the field of mobile robots, and is widely applied to the fields of tunnel exploration, underwater exploration, robot search and rescue and the like. The autonomous exploration of the mobile robot requires that the robot obtains unknown environmental information as much as possible by moving itself in a limited time to build a complete environmental map. The boundary is used as a boundary line for distinguishing the explored area from the unknown area, and more uncertain environment information exists nearby the boundary, so that the exploration strategy based on the boundary is favored by a large number of scholars. The boundary-based exploration strategy can be briefly summarized as follows: firstly, the robot detects a boundary in a map according to map data updated by a sensor of the robot, then generates a target point near the boundary, then combines an evaluation function to select the target point with the highest exploration value as the optimal target point, finally controls the robot to move to the optimal target point by utilizing a motion control module, and updates an environment map until exploration is completed.
However, as the complexity of the environment increases, this method of acquiring boundaries based on image processing increases dramatically in time and space complexity, making autonomous exploration by mobile robots inefficient. The algorithm based on random sampling can avoid the construction of complex space, so that Umari et al can acquire boundary points by utilizing the RRT algorithm, namely, two quick search random trees RRT are grown in the idle area of the map to quickly acquire the boundary points. However, due to the existence of the trap space, the random sampling algorithm based on the RRT is difficult to quickly acquire boundary point information in narrow corridor, maze and other scenes, so that the robot is trapped in place, and even autonomous exploration fails; secondly, the current area of the robot is diverted to other areas when the current area of the robot is not explored due to slow extraction of boundary points, so that a backtracking phenomenon is caused; the random sampling method has larger redundancy for extracting the boundary points, and the redundant boundary points increase the computational complexity of the mobile robot when selecting the optimal target point, so that the time-consuming problem of the robot occurs in the motion decision; finally, the robot calculates the path cost by adopting the Euclidean distance when selecting the explored target point, ignores the environmental characteristics, and easily causes inaccurate evaluation of the optimal target point so as to generate a backtracking phenomenon.
Therefore, the existing autonomous exploration method of the mobile robot uses a clustering algorithm to cluster boundary points to remove redundant points, and the boundary points are slowly extracted and still have larger redundancy, so that the exploration of the robot is slow and the efficiency is low.
Disclosure of Invention
Therefore, the technical problem to be solved by the invention is to solve the problem of larger redundancy in boundary point extraction in the prior art.
In order to solve the technical problems, the invention provides an environment exploration method, which comprises the following steps:
s1, acquiring current pose environment information of a robot, constructing map data and repositioning the current pose of the robot;
s2, generating a generalized voronoi diagram according to the environment information, extracting all boundary points of the generalized voronoi diagram, and constructing a GVD node set;
s3, selecting GVD nodes with the number of unknown grids exceeding a preset threshold value in the GVD node radius in the GVD node set, and constructing a heuristic boundary point set;
s4, selecting the GVD node with the largest radius in the heuristic boundary point set, adding the GVD node into the heuristic boundary point set after fusion, and deleting the GVD node with the largest radius and other GVD nodes within the radius range of the GVD node in the heuristic boundary point set; repeating the step until the heuristic boundary point set is empty, and obtaining a fused heuristic boundary point set;
s5, selecting the GVD node with the largest radius from the GVD node set, adding the GVD node with the largest radius into the GVD feature node set, and deleting the GVD node with the largest radius and other nodes within the radius range of the GVD node set; repeating the step until the GVD node set is empty, and obtaining a GVD characteristic node set;
s6, solving a union set of the GVD characteristic node set, the current pose of the robot and the fused heuristic boundary point set to obtain a GVD characteristic node complete set;
s7, randomly selecting two GVD feature nodes from the GVD feature node total set to perform collision detection, designing a benefit function, selecting a target node from the fused heuristic boundary point set according to the benefit function value, navigating to the target node, and updating map data and the current pose of the robot;
s8, repeating the steps S1 to S7 until the whole environment exploration is completed.
In one embodiment of the present invention, the acquiring the environmental information, constructing the map data and locating the current pose of the robot includes:
the method comprises the steps that laser data are collected by using a sensor carried by a robot, and surrounding environment information is obtained;
receiving laser data acquired by the sensor by utilizing a SLAM module, converting the laser data into map data, and constructing a grid map;
and correcting and repositioning the current pose of the robot by utilizing the grid map constructed by the SLAM module.
In one embodiment of the present invention, the generating a generalized voronoi diagram according to the environmental information, extracting all boundary points of the generalized voronoi diagram to construct a GVD node set includes:
acquiring the map data issued by the SLAM module to generate a generalized voronoi diagram, wherein the map data comprises an unknown area, a free area and an obstacle area;
extracting all boundary points of the generalized voronoi diagram as GVD nodes, and constructing a GVD node set;
wherein the GVD node is denoted as G i =(x i ,y i ,r i ),x i ,y i Representing the location of the GVD node, r i Represents the radius of the GVD node, i.e. the distance of said GVD node to the nearest obstacle.
In an embodiment of the present invention, the constructing a heuristic boundary point set of GVD nodes with the number of unknown grids exceeding a preset threshold within the GVD node radius in the GVD node set further includes:
and discarding the GVD nodes with the unknown grid number not exceeding a preset threshold value in the GVD node radius range in the GVD node set.
In one embodiment of the present invention, the selecting two GVD feature nodes from the GVD feature node total set at will for collision detection, designing a benefit function, selecting a target node from the fused heuristic boundary point set according to a benefit function value, navigating to the target node, and updating map data and a current pose of the robot, includes:
two GVD feature nodes are selected at will from the GVD feature node total set to carry out collision detection, all GVD feature nodes are traversed, and a feature matrix with side information is generated;
calculating the path cost from the current pose of the robot to each GVD feature node according to the feature matrix with the side information;
designing a benefit function, calculating a benefit function value of the GVD node in the fused heuristic boundary point set, and selecting a target node according to the benefit function value;
and navigating to the target node, updating the coordinates of the target node to be the current pose of the robot and updating map data.
In one embodiment of the present invention, the step of arbitrarily selecting two GVD feature nodes from the GVD feature node total set to perform collision detection, traversing all GVD feature nodes, and generating a feature matrix with side information includes:
two GVD feature nodes in the GVD feature node total set are selected at will to carry out collision detection;
if collision detection is passed, generating an edge connecting the two GVD feature nodes, and calculating Euclidean distances of the two GVD feature nodes as the length of the edge;
if the collision detection does not pass, recording the length of the edge connecting the two GVD characteristic nodes as infinity;
and traversing all GVD feature nodes in the GVD feature node total set to generate a feature matrix with side information.
In one embodiment of the invention, the benefit function is R1 f =w 1 *I f -w 2 *N f ;
Wherein I is f The information gain of the target node is represented by the number of unknown grids within the range of the information gain radius of the target node being 3; n (N) f The path cost is obtained by traversing the feature matrix with the side information by using Dijkstra algorithm and represents the actual path cost of the current pose of the robot and the target node; w (w) 1 And w is equal to 2 Is a custom weight.
In one embodiment of the invention, the navigating to the target node comprises:
using an A-global path planning rule to draw a path from the current pose of the robot to the target node;
and finishing obstacle avoidance by combining with a DWA local path planning algorithm, and guiding the robot to navigate to the target node.
The invention also provides an environment exploration device, which comprises:
the information acquisition module is used for acquiring environment information;
the map data updating module comprises a SLAM module and is used for updating map data according to the environment information acquired by the information acquisition module;
the heuristic boundary point extraction and fusion module is used for extracting heuristic boundary points and fusing the heuristic boundary points to generate a fused heuristic boundary point set;
the GVD feature node extraction fusion module is used for extracting GVD feature nodes and generating a GVD feature node complete set by combining the current pose of the robot and the fused heuristic boundary point set;
the collision detection module is used for performing collision detection on the GVD nodes in the GVD characteristic node total set, generating a characteristic matrix with side information, and selecting a target node according to the characteristic matrix with the side information and a benefit function;
and the path generation module is used for planning a path from the current pose of the robot to the target node by utilizing an A-based global path planning algorithm and a DWA local path planning rule.
The invention also provides application of the environment exploration device in the field of tunnel exploration.
Compared with the prior art, the technical scheme of the invention has the following advantages:
according to the environment exploration method, the boundary points with the number of unknown grids exceeding the preset threshold value in the radius range are selected from the boundary point set of the generalized voronoi diagram, the heuristic boundary point set is constructed and fused, redundant nodes with less boundary information are deleted, the extraction speed of the boundary points is accelerated while the heuristic boundary point set contains the boundary points of each region, the redundancy is reduced, and then the calculation consumption of a robot in the decision of the boundary points is reduced; and calculating the gain function value of each GVD node in the integrated heuristic boundary point set of the robot according to the information gain and the path cost by using the designed gain function, selecting the GVD node with the best gain function value as a target node, reducing the backtracking phenomenon caused by inaccurate evaluation and selection of the optimal boundary point, and improving the environment exploration efficiency of the robot.
Drawings
In order that the invention may be more readily understood, a more particular description of the invention will be rendered by reference to specific embodiments thereof that are illustrated in the appended drawings, in which
FIG. 1 is a process schematic of the RRTs algorithm;
FIG. 2 is a flowchart of an environment exploration method according to an embodiment of the present invention;
FIG. 3 is a generalized voronoi diagram provided by an embodiment of the present invention;
FIG. 4 is a schematic diagram of a GVD node with environmental information according to an embodiment of the present invention;
FIG. 5 is a schematic diagram of a heuristic set of boundary points provided by an embodiment of the present invention;
FIG. 6 is a schematic diagram of a fused heuristic set of boundary points provided by an embodiment of the present invention;
FIG. 7 is a schematic diagram of a GVD feature node set according to an embodiment of the present invention;
FIG. 8 is a schematic view of collision detection provided by an embodiment of the present invention;
FIG. 9 is a schematic diagram of a path of a robot navigating to a target node according to an embodiment of the present invention;
FIG. 10 is a diagram of a simulation environment of a scenario provided by an embodiment of the present invention;
FIG. 11 is a diagram of a scenario two simulation environment provided by an embodiment of the present invention;
FIG. 12 is a diagram of a scenario three simulation environment provided by an embodiment of the present invention.
Detailed Description
The present invention will be further described with reference to the accompanying drawings and specific examples, which are not intended to be limiting, so that those skilled in the art will better understand the invention and practice it.
Referring to fig. 1, the general steps of the existing autonomous exploration algorithm based on a fast search random tree include obtaining surrounding environment information of a robot; performing instant positioning and map construction by utilizing a SLAM algorithm, and accurately positioning the current pose of the robot according to the constructed map; generating two fast search random trees, namely boundary point detectors, to extract boundary points; when the boundary points are extracted, a starting point is artificially set in the boundary region of the map and added into the global tree structure to serve as a root node, and the pose of the robot in the map is taken as the root node of the local tree; randomly scattering points in the map area as candidate nodes, and then selecting the node with the lowest cost from the candidate nodes in the adjacent area of the candidate nodes as a growing point; performing collision detection on the growing points and the candidate nodes, if no collision exists, adding the candidate nodes into a tree structure, otherwise, resampling; filtering and removing invalid boundary points; designing a profit function, calculating information gain and path cost, and selecting a target point; navigating to the target point and updating the map.
Specifically, when extracting boundary points, if the candidate points are in a known area, traversing all existing nodes on the tree structure, and selecting the node closest to the candidate points as the nearest neighbor point, wherein the line from the nearest neighbor point to the candidate nodes is taken as the growth direction; if the distance between the nearest point and the candidate node exceeds a preset step length, growing a step length along the growth direction by the nearest point, wherein the reached point is taken as a growth point; if the distance does not exceed the step size, the candidate point is taken as a growth point. And clustering the detected boundary points through a mean-shift clustering algorithm to obtain centroid points, so that a part of boundary points can be filtered, and the calculation consumption is reduced. At each moment, detecting the grid state of the boundary point and the value in the cost map costmap, wherein the costmap divides each grid value between 0 and 255, and the white value is 255, which represents an idle state; a black value of 0, representing an obstacle; values between 0 and 255 are gray, representing unknowns. If the grid state is idle, i.e. the grid point has been explored, and the cosmap median exceeds a certain threshold, it indicates that the area where the grid point is located has been explored, and the grid point should also be rejected as an invalid point.
Because of the randomness of the RRT algorithm, when the boundary points are extracted, the boundary points of the forward region of the robot cannot be extracted timely, in addition, the boundary points extracted by the RRT algorithm have larger redundancy, and because the information gain evaluation model of the boundary points by the current robot exploration strategy does not relate to the consideration of the surrounding environment, the problem that the robot is trapped in place during exploration or the current region is diverted to other regions when the exploration is not completed is frequently caused, and thus a backtracking phenomenon is generated.
In order to solve the problems, the environment exploration method provided by the invention rapidly generates heuristic boundary points by means of the generalized Veno graph GVD and fuses the heuristic boundary points, so that the redundancy of the boundary points is reduced; the robot calculates the actual path cost by means of the characteristic nodes in the Veno graph during exploration, so that the extraction rate of exploration boundary points is increased, the backtracking phenomenon is reduced, and the exploration efficiency is improved.
Referring to fig. 2, the specific steps of the environment exploration method provided by the present invention include:
s1, acquiring environment information, constructing map data and positioning the current pose of a robot;
the robot is positioned at a random position in the environment, acquires surrounding environment information through a sensor carried by the robot, and acquires sensor laser data; the SLAM module receives the sensor laser data, converts the laser data into grid map data by utilizing an SLAM algorithm and updates a part of map into a known area; meanwhile, the pose of the robot is corrected by utilizing the grid map, and the pose of the robot in the environment is accurately positioned;
s2, generating a generalized voronoi diagram according to the environmental information, extracting all boundary points of the generalized voronoi diagram, and constructing a GVD node set;
referring to fig. 3, the subscription sensor receives global map data issued by the SLAM module to generate a generalized voronoi diagram; wherein, the global map divides each grid value into-1, 0, 100, -1 represents an unknown area, 0 represents a free area and 100 represents an obstacle area; referring to fig. 4, a GVD node with environmental information is generated by extracting a free area and an obstacle area in a global map, and all GVD nodes form a GVD node set;
wherein the GVD node is denoted as G i =(x i ,y i ,r i ),x i ,y i Representing GVD sectionThe position of the point, r i Represents the radius of the GVD node, i.e., the distance of the GVD node from the nearest obstacle;
s3, selecting GVD nodes with the number of unknown grids exceeding a preset threshold value in the GVD node radius in the GVD node set, and constructing a heuristic boundary point set;
referring to fig. 5, GVD nodes in the GVD node set are reacted on the global map, the number of unknown area grid values in the radius of the GVD nodes is subjected to threshold value judgment, GVD nodes with the number of grid values exceeding the threshold value in the radius range are extracted as heuristic boundary points, all GVD nodes are traversed, and a heuristic boundary point set is formed; discarding GVD nodes with the number of unknown grids within the radius range of the GVD nodes in the GVD node set not exceeding a preset threshold;
s4, constructing a fused heuristic boundary point set;
referring to fig. 6, a GVD node with the largest radius in the heuristic boundary point set is selected to be added into the heuristic boundary point set after fusion, and the GVD node with the largest radius and other GVD nodes within the radius range thereof are deleted in the heuristic boundary point set; repeating the step until the heuristic boundary point set is empty, and obtaining a fused heuristic boundary point set;
s5, constructing a GVD characteristic node set;
referring to fig. 7, GVD nodes with the largest radius are selected from the GVD node set to be added into the GVD feature node set, meanwhile, the GVD node and other GVD nodes within the radius range are deleted from the GVD node set, GVD nodes with the largest radius in the remaining GVD nodes are repeatedly selected, and other GVD nodes within the radius range are deleted until the GVD node set is empty, so as to obtain the GVD feature node set;
s6, adding the pose of the robot and the heuristic boundary point set after fusion into the GVD feature node set to generate a GVD feature node complete set;
s7, collision detection is carried out on any two nodes in the GVD characteristic node total set, a benefit function is designed, a target node is selected from the fused heuristic boundary point set according to the benefit function value, and map data are navigated and updated to the target node;
referring to fig. 8, during collision detection, all grid points on the straight line connecting any two selected GVD nodes are traversed, and then the grid states of the grid points are judged; if the connecting line crosses the obstacle, collision detection does not pass, and the point is required to be collected again; i.e. the grid point has an obstacle whose state is occupied, collision detection is not passed. If the link does not hit an obstacle, indicating that collision detection is passing, the two nodes may be directly connected. And performing collision detection on any two GVD nodes in the GVD characteristic node total set, and generating an edge connecting the two nodes if the collision detection passes, wherein the Euclidean distance between the two points is the length of the edge. If the collision detection does not pass, the length of the edge is recorded as infinity. And after the collision detection of all the nodes is completed, obtaining a feature matrix with side information. Traversing a feature matrix with side information by using a Dijkstra algorithm, taking the current pose of the robot as a starting node, taking any GVD feature node as an end point, and calculating the shortest path cost from the robot to each GVD feature node;
construction of a benefit function R1 of GVD feature nodes f =w 1 *I f -w 2 *N f Selecting a target node from the fused heuristic boundary point set according to the benefit function value; wherein, the information gain I of the GVD node f For the number of unknown grids within the GVD node information gain radius r=3, the more the number of unknown grids, the more information is harvested to that point; path cost N f The actual path cost, w, of the current position and the boundary point position of the robot 1 And w 2 Is a constant for the custom weight.
Referring to fig. 9, a benefit function value of the robot from the current pose to the integrated heuristic boundary point set GVD node is calculated according to the shortest path cost and the information gain, and an optimal boundary point is selected as a target node according to the benefit function value. Rapidly planning a path from the current position to the target node of the robot in a known environment according to the benefit function by using an A-scale algorithm; combining with a DWA local path planning algorithm to enable the robot to finish obstacle avoidance by utilizing local environment information, navigating to a target node, updating a map and updating the current coordinate position to be the current pose of the robot after the robot reaches the target node;
s8, repeating the steps S1 to S7 until the whole environment exploration is completed.
The robot environment exploration method provided by the embodiment is based on a generalized voronoi diagram, and redundant nodes are deleted by constructing and fusing heuristic boundary point sets, so that the extraction speed of boundary points is increased while each area is ensured to have the boundary points, the redundancy is reduced, and the calculation consumption of a robot in the decision of the boundary points is further reduced; and calculating the path cost from the robot to each GVD node by using the benefit function, selecting the GVD node with the best path cost as a target node, reducing the backtracking phenomenon caused by inaccurate evaluation of the optimal boundary point, and improving the exploration efficiency.
Based on the above embodiment, the embodiment of the present invention further provides an environment exploration device, including an information acquisition module, configured to acquire environment information; the map data updating module comprises a SLAM module and is used for updating map data according to the environment information acquired by the information acquisition module; the heuristic boundary point extraction and fusion module is used for extracting heuristic boundary points and fusing the heuristic boundary points to generate a fused heuristic boundary point set; the GVD feature node extraction fusion module is used for extracting GVD feature nodes and generating a GVD feature node complete set by combining the current pose of the robot and the fused heuristic boundary point set; the collision detection module is used for performing collision detection on the GVD nodes in the GVD characteristic node total set, generating a characteristic matrix with side information, and selecting a target node according to the characteristic matrix with the side information and a benefit function; and the path generation module is used for planning a path from the current pose of the robot to the target node by utilizing an A-based global path planning algorithm and a DWA local path planning rule.
Based on the above embodiment, the present invention further provides an environment exploration robot, including a memory for storing a computer program; a processor for implementing the steps of a generalized voronoi diagram-based environment exploration method as described above when executing the computer program.
Based on the embodiment, the environment exploration method provided by the invention and the autonomous exploration algorithm based on the rapid search random number, namely the RRTs algorithm, are subjected to comparison experiments in three simulation scenes; in each experimental scene, five experiments are carried out by using the environment exploration method based on the generalized voronoi diagram, five experiments are carried out by using the RRTs algorithm, and comparison is carried out according to experimental indexes. Wherein the experimental index refers to the total path length of travel of the robot, as well as the time taken to explore the entire environment, acquire the complete map data.
Referring to table 1, in the first experimental scenario, referring to fig. 10, the average search time is reduced by 15.0% and the average search path length is reduced by 12.9% compared to the RRTs algorithm.
TABLE 1 scene one experiment data
Referring to table 2, in the second experimental scenario, referring to fig. 11, the average search time is reduced by 18.6% and the average search path length is reduced by 17.0% compared to the RRTs algorithm.
Table 2 scene two experimental data
Referring to table 3, in the third experimental scenario, referring to fig. 12, the average search time is reduced by 22.6% and the average search path length is reduced by 4.5% compared to the RRTs algorithm.
Table 3 scene three experimental data
It is apparent that the above examples are given by way of illustration only and are not limiting of the embodiments. Other variations and modifications of the present invention will be apparent to those of ordinary skill in the art in light of the foregoing description. It is not necessary here nor is it exhaustive of all embodiments. While still being apparent from variations or modifications that may be made by those skilled in the art are within the scope of the invention.
Claims (10)
1. An environmental exploration method, comprising:
s1, acquiring environment information of a current pose of a robot, constructing map data and repositioning the current pose of the robot;
s2, generating a generalized voronoi diagram according to the environment information, and extracting all boundary points of the generalized voronoi diagram to constructA node set;
s3, selecting theNode concentration->The number of unknown grids in the radius of the node exceeds the preset threshold value +.>Nodes, constructing heuristic boundary point sets;
s4, selecting the heuristic boundary point set with the largest radiusAdding the nodes into the fused heuristic boundary point set, and deleting the ++f with the largest radius in the heuristic boundary point set>Node and the rest in the radius range>A node; repeating the step until the heuristic boundary point set is empty, and obtaining a fused heuristic boundary point set;
s5, from the aboveSelecting the maximum radius in the node set>Node joining->Feature node concentration in theThe node sets delete the maximum radius +.>Nodes and other nodes within the radius range; repeating the steps until->The node set is empty, get +.>A set of feature nodes;
s6, the step ofThe feature node set, the current pose of the robot and the fused heuristic boundary point set are combined to obtain ∈N>A feature node corpus;
s7, at the siteTwo +.>The feature nodes perform collision detection, a benefit function is designed, a target node is selected from the fused heuristic boundary point set according to the benefit function value, and the map data and the current pose of the robot are navigated and updated to the target node;
s8, repeating the steps S1 to S7 until the whole environment exploration is completed.
2. The method of claim 1, wherein the obtaining environmental information of the current pose of the robot, constructing map data and repositioning the current pose of the robot comprises:
the method comprises the steps that laser data are collected by using a sensor carried by a robot, and surrounding environment information is obtained;
by means ofThe module receives the laser data collected by the sensor, converts the laser data into map data and constructs a grid map;
3. The environment exploration method according to claim 2, wherein said generating a generalized voronoi diagram from said environment information extracts all boundary point constructions of said generalized voronoi diagramA set of nodes, comprising:
acquiring the saidGenerating a generalized voronoi diagram by the map data issued by the module, wherein the map data comprises an unknown area, a free area and an obstacle area;
4. The environmental exploration method of claim 1, wherein said at said step ofNode concentration->The number of unknown grids in the radius of the node exceeds the preset threshold value +.>The node constructs heuristic boundary point set, further includes:
5. The environmental exploration method of claim 1, wherein said at said step ofTwo +.>The feature nodes perform collision detection, a benefit function is designed, a target node is selected from the fused heuristic boundary point set according to the benefit function value, the map data and the current pose of the robot are navigated and updated to the target node, and the method comprises the following steps:
at the position ofTwo +.>The feature nodes perform collision detection and traverse all +.>The feature node generates a feature matrix with side information;
calculating the current pose of the robot to each according to the feature matrix with the side informationPath cost of the feature node;
designing a benefit function, and calculating the heuristic boundary point set after fusionThe node profit function value is used for selecting a target node according to the profit function value;
and navigating to the target node, updating the coordinates of the target node to be the current pose of the robot and updating map data.
6. The method of claim 5, wherein said determining is performed at said locationTwo +.>The feature nodes perform collision detection and traverse all +.>The feature node generates a feature matrix with side information, comprising:
arbitrarily selectTwo of the feature node corpus +.>Performing collision detection on the characteristic nodes;
if the collision detection passes, a connection is generated between the twoEdges of feature nodes, calculating said two +.>The Euclidean distance of the feature node is used as the length of the edge;
if the collision detection is not passed, the two are connectedThe length of the edge of the characteristic node is recorded as infinity;
Wherein,,the information gain of the target node is represented by the number of unknown grids within the range of the information gain radius of the target node being 3; />For the path cost, use ∈>The algorithm traverses the feature matrix with the side information to obtain the feature matrix which represents the actual path cost of the current pose of the robot and the target node; />And->Is self-definedAnd (5) meaning weights.
8. The method of environmental exploration according to claim 5, wherein said navigating to said target node comprises:
using an A-global path planning rule to draw a path from the current pose of the robot to the target node;
and finishing obstacle avoidance by combining with a DWA local path planning algorithm, and guiding the robot to navigate to the target node.
9. An environment exploration apparatus, comprising:
the information acquisition module is used for acquiring environment information;
the map data updating module comprises a SLAM module and is used for updating map data according to the environment information acquired by the information acquisition module;
the heuristic boundary point extraction fusion module is used for generating a generalized voronoi diagram according to the environmental information, and extracting all boundary point construction of the generalized voronoi diagramA node set; selecting said->Node concentration->The number of unknown grids in the radius of the node exceeds the preset threshold value +.>Nodes, constructing heuristic boundary point sets; selecting +.sup.f with maximum radius in the heuristic boundary point set>Adding the nodes into the fused heuristic boundary point set, and deleting the ++f with the largest radius in the heuristic boundary point set>Node and the rest in the radius range>A node; repeating the step until the heuristic boundary point set is empty, and obtaining a fused heuristic boundary point set;
a feature node extraction fusion module for extracting the fusion module from the +.>Selecting the maximum radius in the node set>Node joining->Characteristic node concentration, in the->The node sets delete the maximum radius +.>Nodes and other nodes within the radius range; repeating the steps until->The node set is empty, get +.>A set of feature nodes; the saidThe feature node set, the current pose of the robot and the fused heuristic boundary point set are combined to obtain +.>A feature node corpus;
the collision detection module is used for performing collision detection on the GVD nodes in the GVD characteristic node total set, generating a characteristic matrix with side information, and selecting a target node according to the characteristic matrix with the side information and a benefit function;
and the path generation module is used for planning a path from the current pose of the robot to the target node by utilizing an A-based global path planning algorithm and a DWA local path planning rule.
10. Use of the environment exploration device of claim 9 in the field of tunnel exploration.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211110233.8A CN115469662B (en) | 2022-09-13 | 2022-09-13 | Environment exploration method, device and application |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211110233.8A CN115469662B (en) | 2022-09-13 | 2022-09-13 | Environment exploration method, device and application |
Publications (2)
Publication Number | Publication Date |
---|---|
CN115469662A CN115469662A (en) | 2022-12-13 |
CN115469662B true CN115469662B (en) | 2023-07-07 |
Family
ID=84371299
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202211110233.8A Active CN115469662B (en) | 2022-09-13 | 2022-09-13 | Environment exploration method, device and application |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115469662B (en) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116382307B (en) * | 2023-06-05 | 2023-08-01 | 南开大学 | Multi-robot autonomous exploration method and system based on mass centers of unknown connected areas |
CN116679712B (en) * | 2023-06-19 | 2024-07-12 | 苏州大学 | Efficient exploration decision-making method for indoor environment robot based on generalized voronoi diagram |
Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114779788A (en) * | 2022-05-27 | 2022-07-22 | 昆明理工大学 | Path planning method for improving A-algorithm |
Family Cites Families (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US8060271B2 (en) * | 2008-06-06 | 2011-11-15 | Toyota Motor Engineering & Manufacturing North America, Inc. | Detecting principal directions of unknown environments |
CN106682787A (en) * | 2017-01-09 | 2017-05-17 | 北京航空航天大学 | Method for quickly generating generalized Voronoi diagram based on wavefront algorithm |
US11624833B2 (en) * | 2019-05-29 | 2023-04-11 | Faro Technologies, Inc. | System and method for automatically generating scan locations for performing a scan of an environment |
CN110703747B (en) * | 2019-10-09 | 2021-08-03 | 武汉大学 | Robot autonomous exploration method based on simplified generalized Voronoi diagram |
CN111504321B (en) * | 2020-04-10 | 2022-03-18 | 苏州大学 | Reusable search tree method based on expanded voronoi diagram characteristics |
CN113110482B (en) * | 2021-04-29 | 2022-07-19 | 苏州大学 | Indoor environment robot exploration method and system based on priori information heuristic method |
CN113485375B (en) * | 2021-08-13 | 2023-03-24 | 苏州大学 | Indoor environment robot exploration method based on heuristic bias sampling |
CN113625721B (en) * | 2021-08-19 | 2023-05-16 | 东北大学 | Unknown space autonomous exploration planning method |
-
2022
- 2022-09-13 CN CN202211110233.8A patent/CN115469662B/en active Active
Patent Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114779788A (en) * | 2022-05-27 | 2022-07-22 | 昆明理工大学 | Path planning method for improving A-algorithm |
Also Published As
Publication number | Publication date |
---|---|
CN115469662A (en) | 2022-12-13 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN115469662B (en) | Environment exploration method, device and application | |
CN110262518B (en) | Vehicle navigation method, system and medium based on track topological map and obstacle avoidance | |
CN110531760B (en) | Boundary exploration autonomous mapping method based on curve fitting and target point neighborhood planning | |
CN113110482B (en) | Indoor environment robot exploration method and system based on priori information heuristic method | |
CN113485375B (en) | Indoor environment robot exploration method based on heuristic bias sampling | |
CN103390169B (en) | A kind of city terrain classification method of Vehicle-borne Laser Scanning cloud data | |
CN109163722B (en) | Humanoid robot path planning method and device | |
CN103278170A (en) | Mobile robot cascading map building method based on remarkable scenic spot detection | |
CN103869820A (en) | Ground navigation planning control method of rover | |
JP2020038660A (en) | Learning method and learning device for detecting lane by using cnn, and test method and test device using the same | |
CN110728751A (en) | Construction method of indoor 3D point cloud semantic map | |
CN108592912A (en) | A kind of autonomous heuristic approach of indoor mobile robot based on laser radar | |
JP2020038661A (en) | Learning method and learning device for detecting lane by using lane model, and test method and test device using the same | |
AU2020229324B2 (en) | System and method for surface feature detection and traversal | |
CN114034299B (en) | Navigation system based on active laser SLAM | |
CN109919955A (en) | The tunnel axis of ground formula laser radar point cloud extracts and dividing method | |
CN114119920A (en) | Three-dimensional point cloud map construction method and system | |
Aguiar et al. | Localization and mapping on agriculture based on point-feature extraction and semiplanes segmentation from 3D LiDAR data | |
CN114020015A (en) | Unmanned aerial vehicle path planning system and method based on barrier map bidirectional search | |
CN114186112B (en) | Robot navigation method based on Bayesian optimization multiple information gain exploration strategy | |
Short et al. | Abio-inspiredalgorithminimage-based pathplanning and localization using visual features and maps | |
CN114721370A (en) | Robot rapid optimal path planning method based on double heuristic functions | |
CN115344057B (en) | Vegetation dense area route planning method, system, storage medium and electronic equipment | |
Bai et al. | Semantic segmentation of sparse irregular point clouds for leaf/wood discrimination | |
AU2021273605A1 (en) | Multi-agent map generation |
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 |