CN113485375A - Indoor environment robot exploration method based on heuristic bias sampling - Google Patents
Indoor environment robot exploration method based on heuristic bias sampling Download PDFInfo
- Publication number
- CN113485375A CN113485375A CN202110929934.3A CN202110929934A CN113485375A CN 113485375 A CN113485375 A CN 113485375A CN 202110929934 A CN202110929934 A CN 202110929934A CN 113485375 A CN113485375 A CN 113485375A
- Authority
- CN
- China
- Prior art keywords
- point
- heuristic
- boundary points
- robot
- boundary
- 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
Links
- 238000005070 sampling Methods 0.000 title claims abstract description 51
- 238000000034 method Methods 0.000 title claims abstract description 39
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 37
- 238000001514 detection method Methods 0.000 claims description 17
- 238000012360 testing method Methods 0.000 claims description 6
- 238000000605 extraction Methods 0.000 claims description 4
- 230000008569 process Effects 0.000 abstract description 5
- 230000006872 improvement Effects 0.000 description 9
- 238000004364 calculation method Methods 0.000 description 6
- 230000006870 function Effects 0.000 description 6
- 238000012959 renal replacement therapy Methods 0.000 description 5
- 238000010586 diagram Methods 0.000 description 3
- 241000282414 Homo sapiens Species 0.000 description 2
- 230000009471 action Effects 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 208000037170 Delayed Emergence from Anesthesia Diseases 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000013135 deep learning Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000005065 mining Methods 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000011176 pooling Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 238000006467 substitution reaction 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/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
- G05D1/0251—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means extracting 3D information from a plurality of images taken from different locations, e.g. stereo vision
-
- 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/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- 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/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Multimedia (AREA)
- Electromagnetism (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Manipulator (AREA)
Abstract
The invention discloses an indoor environment robot exploration method based on heuristic bias sampling, which comprises the following steps: A. initializing; B. positioning and drawing; C. extracting boundary points by using the two fast growing random trees, and recording the boundary points as RRT boundary points; extracting boundary points in the prior area by a bias sampling fast search random tree algorithm, and recording the boundary points as room boundary points; eliminating the RRT boundary points and invalid boundary points in the room boundary points; D. the robot preferentially selects the room boundary point with the maximum profit value as a target point for exploration, and preferentially selects the RRT boundary point with the maximum profit value as the target point for exploration after the exploration of all the room boundary points is finished; E. guiding the robot to navigate to a target point and updating a map; F. destroying the prior area when the boundary point cannot be detected in the prior area; G. and (4) circulating the steps A-F until the whole environment exploration is completed. The invention can effectively reduce backtracking phenomenon in the exploration process and improve exploration efficiency.
Description
Technical Field
The invention relates to the technical field of robots, in particular to an indoor environment robot exploration method based on heuristic bias sampling.
Background
With the development of computer software and hardware technology and the breakthrough of artificial intelligence technology, the research of the service type mobile robot obtains the performance of attention, the application scene and the service mode of the intelligent public service robot are continuously expanded, and the market scale of the service robot in China is driven to grow at a high speed. The autonomous exploration of the mobile robot refers to a process of establishing a complete environment map by moving the robot in a new environment without any prior knowledge.
At present, the mainstream autonomous exploration algorithm is an exploration method based on boundary points, a detection space is divided into a known area and an unknown area by a boundary, and a robot is guided to explore the unknown area to gradually complete modeling of the whole environment. In the occasions with harsh time requirements, such as fire rescue, mine detection and the like, the modeling of the whole environment is quickly completed, and the tasks of rescue, mining and the like can be effectively helped.
In order to complete the environment modeling task more quickly, the research of broad scholars focuses on how to detect and select boundary points, and the selection of the boundary points directly influences the exploration efficiency. However, such exploration strategies only consider the cost of exploring the path at the current time and the information gain from updating a small portion of the map around the boundary point, and ignore the potential gains of the environment structure. For example, in an indoor environment, after a robot enters a room for exploration, there are generally two actions: 1. the room area exploration is completed and then the room area exploration is turned to the outside of the room; 2. the room area is turned outside the room without completing the exploration, and then the room exploration is performed again. Obviously, the second action is more time consuming and less efficient.
The core idea of the autonomous exploration strategy based on the rapid search random tree (RRT) is a greedy strategy, and the information gain model of the boundary points does not design consideration on the surrounding environment, so that the robot can select the boundary point with the maximum profit value as a target point at each moment regardless of the surrounding explored environment structure. For example, when the robot explores in a room area, if the profit value of the boundary point outside the room is large, the robot turns to the outside-room exploration, and then the robot bypasses to finish the room area exploration, so that a backtracking phenomenon is generated; secondly, due to the randomness of the RRT algorithm, the boundary points of the area where the robot moves forward often cannot be extracted in time when the boundary points are extracted, so that the robot also has the phenomenon of selecting another area for exploration and returning to the exploration. The problems of slow exploration and low efficiency of the robot are caused.
Disclosure of Invention
The invention aims to provide an indoor environment robot exploration method based on heuristic bias sampling, which is fast in exploration and high in exploration efficiency.
In order to solve the above problems, the present invention provides an indoor environment robot exploration method based on heuristic bias sampling, which includes:
A. initializing;
B. positioning and drawing;
C. growing two fast search random trees in an idle area of a map, extracting boundary points by using the two fast search random trees, and recording the boundary points as RRT boundary points; identifying a heuristic object and acquiring position information of the heuristic object, constructing a prior area according to the position of the heuristic object, and extracting boundary points in the prior area by a bias sampling fast search random tree algorithm, wherein the boundary points are marked as room boundary points; eliminating the RRT boundary points and invalid boundary points in the room boundary points;
D. when the room boundary points exist, the robot preferentially selects the room boundary point with the maximum profit value as a target point for exploration, explores the RRT boundary points after the exploration of all the room boundary points is finished, and preferentially selects the RRT boundary point with the maximum profit value as the target point for exploration;
E. guiding the robot to navigate to a target point and updating a map;
F. destroying the prior area when the boundary point cannot be detected in the prior area;
G. and (4) circulating the steps A-F until the whole environment exploration is completed.
As a further improvement of the present invention, step B specifically comprises: and receiving the sensor data through the SLAM module, updating a part of the map into a known area, and meanwhile, correcting the pose of the robot in turn through the map constructed by the SLAM module.
As a further improvement of the invention, the two fast search random trees are divided into a global tree and a local tree, and the global tree and the local tree are subjected to RRT boundary point extraction through the following steps; the method specifically comprises the following steps:
c11, adding the starting point into the tree structure as a root node, wherein the starting points of the two fast search random trees are set in the idle area of the map manually;
c12, randomly scattering points in the map area as candidate points;
c13, if the candidate point is in the known area, traversing all the existing nodes on the tree structure, selecting the node closest to the candidate point as the nearest point, and taking the connection line between the nearest point and the candidate node as the growth direction; if the distance between the nearest neighbor point and the candidate node exceeds the preset step length, growing a step length along the growth direction by the nearest neighbor point, and taking the reached point as a growth point; if the distance does not exceed the step length, the candidate point is taken as a growing point;
if the candidate point is in the unknown area, firstly finding the nearest node of the candidate point, taking the connecting line from the nearest node to the candidate point as the growth direction, then growing forwards from the nearest node along the growth direction, and taking the place reaching the boundary as the boundary point;
c14, performing collision detection on a connecting line of the growing point and the candidate node on the map, traversing all grid points on the straight line, then judging the grid state of the grid point, if the grid point is occupied by the obstacle, failing the collision detection, and returning to the step C12 to perform sampling again; if the connecting line does not touch the obstacle, adding the connecting line of the candidate point, the growing point and the candidate node into the tree structure;
and when the local tree detects the boundary point, the local tree is cleared and grows again at the current position of the robot.
As a further improvement of the invention, boundary points are extracted in the prior area by a biased sampling fast search random tree algorithm and are recorded as room boundary points; the method specifically comprises the following steps:
c21, adding the position of the heuristic object into the tree structure as a root node;
c22, using a random function to decide which sampling mode to adopt by the offset sampling fast search random tree algorithm each time; the sampling mode comprises uniform random sampling and offset sampling;
c23, traversing all existing nodes on the tree structure, selecting the node closest to the candidate point as the nearest point, and using the connection line between the nearest point and the candidate node as the growth direction; if the distance between the nearest neighbor point and the candidate node exceeds the preset step length, growing a step length along the growth direction by the nearest neighbor point, and taking the reached point as a growth point; if the distance does not exceed the step length, the candidate point is taken as a growing point;
c24, performing collision detection on the connecting line of the growing point and the candidate node on the map, if the collision test is passed, entering the step C25, otherwise, returning to the step C22;
c25, judging whether the growing point passing the collision test is a boundary point, and changing the grid state of part of the boundary points;
and C26, calculating the coverage rate according to the ratio of the number of the boundary points at the current moment to the total number of the boundary points.
As a further improvement of the present invention, step C24 specifically includes: traversing all grid points on a straight line where the connecting lines of the growing points and the candidate nodes are located, then judging the grid state of the grid points, if the state of the grid points is occupied or unknown, failing to pass the collision detection, and returning to the step C22 for sampling again; if the connecting line does not touch the obstacle and the unknown area, the connecting line of the candidate point, the growing point and the candidate node is added into the tree structure.
As a further improvement of the present invention, the removing of the invalid boundary points in the RRT boundary points and the room boundary points specifically includes:
c31, clustering the RRT boundary points and the room boundary points through a mean-shift clustering algorithm to obtain centroid points;
c32, detecting the grid state of the boundary point and the value in the costmap in real time, and if the grid state is idle and the value in the costmap exceeds a certain threshold value, the grid point is also removed as an invalid point.
As a further improvement of the present invention, the constructing the prior region from the position of the heuristic object specifically includes:
when the robot identifies a heuristic object, if the position of the robot is below the heuristic object, the prior area is above the position of the heuristic object; when the robot position is above the heuristic object, then the prior area is below the position of the heuristic object.
As a further improvement of the invention, the lengths of the prior regions are respectively the lengths a of the positions of the heuristic objects extending towards two sides, and the width of the prior region is the length 2b of the position of the heuristic object extending backwards; wherein a is more than or equal to 2 and less than or equal to 10, and b is more than or equal to 2 and less than or equal to 10.
As a further improvement of the present invention, step E specifically comprises: a global path planning algorithm A is used for planning a path from the current position of the robot to a target point in a known environment, a DWA local path planning algorithm is combined to enable the robot to well utilize local environment information to complete obstacle avoidance, and the DWA local path planning algorithm are combined to guide the robot to navigate to the target point and update a map.
As a further improvement of the present invention, in step a, the whole environment is unknown, the robot is located at a certain position in the environment, and the robot acquires data of the surrounding environment information through a sensor carried by the robot.
The invention has the beneficial effects that:
according to the indoor environment robot exploration method based on heuristic bias sampling, heuristic prior information is sampled, after the robot identifies a heuristic object, the robot preferentially explores the environment in the prior area, so that the robot can preferentially explore one room area and then turn to other areas, backtracking in the exploration process is effectively reduced, and exploration efficiency is improved.
The foregoing description is only an overview of the technical solutions of the present invention, and in order to make the technical means of the present invention more clearly understood, the present invention may be implemented in accordance with the content of the description, and in order to make the above and other objects, features, and advantages of the present invention more clearly understood, the following preferred embodiments are described in detail with reference to the accompanying drawings.
Drawings
FIG. 1 is a flow chart of an indoor environment robot exploration method based on heuristic bias sampling in the preferred embodiment of the present invention;
FIG. 2 is a schematic illustration of the prior region constructed in the preferred embodiment of the present invention;
FIG. 3 is a schematic diagram of an experimental scenario one in a preferred embodiment of the present invention;
FIG. 4 is a diagram of an experimental scenario two in a preferred embodiment of the present invention;
fig. 5 is a schematic diagram of an experimental scenario three in the preferred embodiment of the present invention.
Detailed Description
The present invention is further described below in conjunction with the following figures and specific examples so that those skilled in the art may better understand the present invention and practice it, but the examples are not intended to limit the present invention.
As shown in fig. 1, the indoor environment robot exploring method based on heuristic bias sampling in the preferred embodiment of the present invention includes the following steps:
A. initializing; the whole environment is unknown, the robot is located at a certain position in the environment, and the robot acquires data of surrounding environment information through a sensor carried by the robot.
B. Positioning and drawing; the method specifically comprises the following steps: and receiving the sensor data through the SLAM module, updating a part of the map into a known area, and meanwhile, correcting the pose of the robot in turn through the map constructed by the SLAM module.
C. Growing two fast search random trees in an idle area of a map, extracting boundary points by using the two fast search random trees, and recording the boundary points as RRT boundary points; identifying a heuristic object and acquiring position information of the heuristic object, constructing a prior area according to the position of the heuristic object, and extracting boundary points in the prior area by a bias sampling fast search random tree algorithm, wherein the boundary points are marked as room boundary points; and eliminating the RRT boundary points and invalid boundary points in the room boundary points.
The two fast search random trees are divided into a global tree and a local tree, and the global tree and the local tree are subjected to RRT boundary point extraction through the following steps; the method specifically comprises the following steps:
c11, adding the starting point into the tree structure as a root node, wherein the starting points of the two fast search random trees are set in the idle area of the map manually;
c12, randomly scattering points in the map area as candidate points;
c13, if the candidate point is in the known area, traversing all the existing nodes on the tree structure, selecting the node closest to the candidate point as the nearest point, and taking the connection line between the nearest point and the candidate node as the growth direction; if the distance between the nearest neighbor point and the candidate node exceeds the preset step length, growing a step length along the growth direction by the nearest neighbor point, and taking the reached point as a growth point; if the distance does not exceed the step length, the candidate point is taken as a growing point;
if the candidate point is in the unknown region, firstly finding the nearest node of the candidate point, taking the connecting line between the nearest node and the candidate point as the growth direction, then growing forwards from the nearest node along the growth direction, taking the place reaching the boundary as the boundary point, and the algorithm is characterized in that traversal is started from the nearest node along the growth direction, and the grid state is unknown as the boundary point;
c14, performing collision detection on a connecting line of the growing point and the candidate node on the map, traversing all grid points on the straight line, then judging the grid state of the grid point, if the grid point is occupied by the obstacle, failing the collision detection, and returning to the step C12 to perform sampling again; if the connecting line does not touch the obstacle, adding the connecting line of the candidate point, the growing point and the candidate node into the tree structure;
and when the local tree detects the boundary point, the local tree is cleared and grows again at the current position of the robot.
Optionally, identifying a heuristic object and acquiring position information of the heuristic object specifically include:
s1, using a target detection method based on deep learning to identify the heuristic object, which is a lightweight network improved on the basis of YOLOv4_ tiny. The lightweight network mainly comprises a convolution layer, an inverse residual block, a pooling layer and an SPP (spatial Pyramid) layer, wherein the total number of the layers is 42, and the output is simplified into two layers. Compared with YOLOv4_ tiny, the detection accuracy of the lightweight network is improved by 19.2%, which is close to YOLOv4, but the speed is almost 4 times of YOLOv 4. Overall, this lightweight network is very efficient in terms of both speed and accuracy, and it is suitable for heuristic target recognition.
And S2, when the heuristic object is detected by using the network, acquiring the coordinate information of the heuristic object under the pixel plane. And then, converting the image coordinate system into a camera coordinate system and then into a world coordinate system, wherein the coordinates of the central point P of the output heuristic object in the image coordinate system are (u, v), and the coordinates of the central point P in the camera coordinate system are (x)c,yc,zc) The coordinates in the world coordinate system are (x, y, z), and the following mapping relationship exists between them:
for convenience of actual conversion calculation, the camera parameters are expressed by homogeneous coordinates in the invention, so the conversion from two-dimensional points to three-dimensional points can be expressed as:
wherein, camera internal parameters (f)x,fy,cx,cy) The topics published under the ROS by the subscription depth camera can be obtained.
In the embodiment, a priori region is constructed by taking the position of a heuristic object as a reference, the region follows the perception habit of human beings, the region behind a door is a room, the size of the region is set by human experience, and the size of the region is ensured to be slightly larger than the size of an actual region in most occasions. The method specifically comprises the following steps: when the robot identifies a heuristic object, if the position of the robot is below the heuristic object, the estimated prior area is above the position of the heuristic object; similarly, when the robot position is above the heuristic object, the estimated prior area is below the position of the heuristic object. Optionally, the estimated lengths of the prior regions are lengths a of the heuristic object extending from the position to the two sides, respectively, and the estimated width of the region is a length 2b of the heuristic object extending from the position to the back. In the embodiment, a is more than or equal to 2 and less than or equal to 10, and b is more than or equal to 2 and less than or equal to 10; alternatively, a and b are both set to 6. Refer to fig. 2.
Extracting boundary points in the prior area by a bias sampling fast search random tree algorithm, and recording the boundary points as room boundary points; the method specifically comprises the following steps:
c21, adding the position of the heuristic object into the tree structure as a root node;
c22, using a random function to decide which sampling mode to adopt by using a biased sampling fast search random tree algorithm (bias _ RRT) each time; the sampling mode comprises uniform random sampling and offset sampling; wherein the probability of the uniform random sampling, i.e., sampling at each location of a given map area, is the same; the offset samples only select boundary location samples in the map. The boundary position of the map includes the boundary between the free area and the obstacle area, and the boundary between the free area and the unknown area, and these boundaries can be extracted by directly using an image contour extraction method in OpenCV (classical special library in computer vision), and the number of boundary points at this time is calculated and recorded as the total number of boundary points. The bias _ RRT algorithm of the invention uses a random function to decide which sampling mode to adopt each time;
c23, traversing all existing nodes on the tree structure, selecting the node closest to the candidate point as the nearest point, and using the connection line between the nearest point and the candidate node as the growth direction; if the distance between the nearest neighbor point and the candidate node exceeds the preset step length, growing a step length along the growth direction by the nearest neighbor point, and taking the reached point as a growth point; if the distance does not exceed the step length, the candidate point is taken as a growing point;
c24, performing collision detection on the connecting line of the growing point and the candidate node on the map, if the collision test is passed, entering the step C25, otherwise, returning to the step C22; traversing all grid points on the straight line, judging the grid state of the grid point, if the state of the grid point is occupied or unknown, failing to pass the collision detection, and returning to the step C22 for sampling again; if the connecting line does not touch the obstacle and the unknown area, adding the connecting line of the candidate point, the growing point and the candidate node into the tree structure; (comparing the process of extracting the boundary points by the RRT algorithm, the bias _ RRT algorithm of the invention considers the occupied and unknown grid states as the obstacles in the collision detection, thus the establishment of the tree structure can be rapidly carried out);
c25, judging whether the growing point passing the collision test is a boundary point, and changing the grid state of part of the boundary points;
specifically, the determination method is to check whether there are occupied grid points and unknown grid points in a square area centered on the growing point, if there are occupied grid points in the square area, it is said that the tree grows to the vicinity of the obstacle, and at the same time, the state of the grid points in these square areas is changed to be known, which indicates that this area is covered by the tree structure, so that the total number of boundary points in step C22 is reduced by this processing; if unknown grid points exist in the square area, the tree is described to grow to the vicinity of the unknown area, and meanwhile, the state of the grid points in the square area is changed to be known, so that the area is indicated to be covered by the tree structure, and the points are distributed as room boundary points, and the room boundary points are used as target points to guide the robot to search towards the room area. Likewise, the total number of boundary points in step C22 will again decrease because the state of the grid points within the square region is changed;
and C26, calculating the coverage rate according to the ratio of the number of the boundary points at the current moment to the total number of the boundary points.
Specifically, the coverage rate calculation formula is as follows:
wherein N isnowNumber of boundary points at the present time, NtotalIs the total number of boundary points. Each coverage calculation is compared with a preset threshold, if the coverage calculation is smaller than the preset threshold, it indicates that the tree structure covers the whole prior area, this flag is used as an important index for whether the prior area is searched, and the threshold is set to 0.1 in this embodiment. Due to the randomness of the RRT algorithm, all boundary points can not be extracted in time under certain extreme conditions, so that the coverage rate and no boundary point in the prior area are jointly used as the mark for completing the exploration of the prior area, the prior area can be effectively explored by the robot preferentially, and the backtracking phenomenon is avoided.
The removing of the invalid boundary points in the RRT boundary points and the room boundary points specifically includes:
c31, clustering the RRT boundary points and the room boundary points through a mean-shift clustering algorithm to obtain centroid points;
c32, detecting the grid state of the boundary point and the value in the costmap in real time, and if the grid state is idle and the value in the costmap exceeds a certain threshold value, the grid point is also removed as an invalid point.
Specifically, the detected boundary points are clustered through a mean-shift clustering algorithm to obtain centroid points, so that a part of boundary points can be filtered, and calculation consumption is reduced. Meanwhile, the grid state of the boundary point and the value in a costmap (costmap) are detected at each moment (costmap divides each grid value into 0-255, the white value is 255 and represents an idle state, the black value is 0 and represents an obstacle, the value between the white value and the black value is gray and represents unknown), if the grid state is idle (indicating that the grid point is explored) and the costmap median exceeds a certain threshold value, the area where the grid point is located is found, and the grid point is also taken as an invalid point to be removed.
D. When the room boundary points exist, the robot preferentially selects the room boundary point with the maximum profit value as a target point for exploration, explores the RRT boundary points after all the room boundary points are explored, and preferentially selects the RRT boundary point with the maximum profit value as the target point for exploration.
Specifically, the robot preferentially selects room boundary points for exploration, so that the robot preferentially enters a priori region for exploration after identifying heuristic objects. When the room boundary point exists, the prior area is not explored completely, and then the RRT boundary point is selected for exploration after the room boundary point is explored completely. Therefore, the robot can finish the exploration of one area and then switch to the exploration of other areas according to the artificial assumption;
when a plurality of boundary points exist, a profit function is designed for the boundary points, the calculation formula of the room boundary point profit function is the same as that of the RRT boundary points, but the two types of boundary points are separately compared, namely, when the room boundary points exist, the room boundary point with the maximum profit value is selected as a target point; when no room boundary point exists, selecting the maximum RRT boundary point as a target point; the revenue function for the boundary points is as follows:
R1f＝w1*If-w2*Nf
wherein, IfFor information gain: namely the number of unknown grids in the information gain radius r of the center of mass point being 1; n is a radical offFor the path cost: namely the Euclidean distance between the current position of the robot and the position of the centroid; in this example w1Is set to 3, w2Is set to 1.
E. Guiding the robot to navigate to a target point and updating a map; the method specifically comprises the following steps: a global path planning algorithm A is used for planning a path from the current position of the robot to a target point in a known environment, a DWA local path planning algorithm is combined to enable the robot to well utilize local environment information to complete obstacle avoidance, and the DWA local path planning algorithm are combined to guide the robot to navigate to the target point and update a map.
F. Destroying the prior area when the boundary point cannot be detected in the prior area; specifically, when the boundary point is not detected in the prior area, which indicates that the area has been explored completely, the prior area model is destroyed to form the next heuristic area.
G. And (4) circulating the steps A-F until the whole environment exploration is completed.
In order to fully prove the effectiveness of the invention, the invention and an autonomous exploration algorithm (hereinafter, expressed by an RRTs algorithm) based on a fast search random tree are subjected to comparison experiments under two simulation scenes. A total of 20 experiments were performed for each experimental environment. Including 10 uses of the RRTs algorithm, 10 of our improved algorithm. The indicators of the experimental comparisons are the time used to explore the entire environment and the length of the path traveled.
The experimental data are shown in tables 1, 2 and 3, which correspond to experimental scenarios one, two and three, respectively. In the first experimental scenario (as shown in fig. 3), the method of the present invention reduces the search time by 31.2% and the search path length by 21.8% compared to the RRTs algorithm. In the second experimental scenario (as shown in fig. 4), the search time of our method is reduced by 13.4% and the search path length is reduced by 17.2% compared to the RRTs algorithm. In the third experimental scenario (as shown in fig. 5), the search time of our method is reduced by 21.9% and the search path length is reduced by 22.7% compared to the RRTs algorithm.
Watch two
Watch III
According to the indoor environment robot exploration method based on heuristic bias sampling, heuristic prior information is sampled, after the robot identifies a heuristic object, the robot preferentially explores the environment in the prior area, so that the robot can preferentially explore one room area and then turn to other areas, backtracking in the exploration process is effectively reduced, and exploration efficiency is improved.
The above embodiments are merely preferred embodiments for fully illustrating the present invention, and the scope of the present invention is not limited thereto. The equivalent substitution or change made by the technical personnel in the technical field on the basis of the invention is all within the protection scope of the invention. The protection scope of the invention is subject to the claims.
Claims (10)
1. An indoor environment robot exploration method based on heuristic bias sampling is characterized by comprising the following steps:
A. initializing;
B. positioning and drawing;
C. growing two fast search random trees in an idle area of a map, extracting boundary points by using the two fast search random trees, and recording the boundary points as RRT boundary points; identifying a heuristic object and acquiring position information of the heuristic object, constructing a prior area according to the position of the heuristic object, and extracting boundary points in the prior area by a bias sampling fast search random tree algorithm, wherein the boundary points are marked as room boundary points; eliminating the RRT boundary points and invalid boundary points in the room boundary points;
D. when the room boundary points exist, the robot preferentially selects the room boundary point with the maximum profit value as a target point for exploration, explores the RRT boundary points after the exploration of all the room boundary points is finished, and preferentially selects the RRT boundary point with the maximum profit value as the target point for exploration;
E. guiding the robot to navigate to a target point and updating a map;
F. destroying the prior area when the boundary point cannot be detected in the prior area;
G. and (4) circulating the steps A-F until the whole environment exploration is completed.
2. The indoor environment robot exploration method based on heuristic biased sampling according to claim 1, wherein step B specifically comprises: and receiving the sensor data through the SLAM module, updating a part of the map into a known area, and meanwhile, correcting the pose of the robot in turn through the map constructed by the SLAM module.
3. The indoor environment robot exploration method based on heuristic biased sampling according to claim 1, characterized in that the two fast search random trees are divided into a global tree and a local tree, both global tree and local tree performing the extraction of RRT boundary points by the following steps; the method specifically comprises the following steps:
c11, adding the starting point into the tree structure as a root node, wherein the starting points of the two fast search random trees are set in the idle area of the map manually;
c12, randomly scattering points in the map area as candidate points;
c13, if the candidate point is in the known area, traversing all the existing nodes on the tree structure, selecting the node closest to the candidate point as the nearest point, and taking the connection line between the nearest point and the candidate node as the growth direction; if the distance between the nearest neighbor point and the candidate node exceeds the preset step length, growing a step length along the growth direction by the nearest neighbor point, and taking the reached point as a growth point; if the distance does not exceed the step length, the candidate point is taken as a growing point;
if the candidate point is in the unknown area, firstly finding the nearest node of the candidate point, taking the connecting line from the nearest node to the candidate point as the growth direction, then growing forwards from the nearest node along the growth direction, and taking the place reaching the boundary as the boundary point;
c14, performing collision detection on a connecting line of the growing point and the candidate node on the map, traversing all grid points on the straight line, then judging the grid state of the grid point, if the grid point is occupied by the obstacle, failing the collision detection, and returning to the step C12 to perform sampling again; if the connecting line does not touch the obstacle, adding the connecting line of the candidate point, the growing point and the candidate node into the tree structure;
and when the local tree detects the boundary point, the local tree is cleared and grows again at the current position of the robot.
4. The indoor environment robot exploration method based on heuristic biased sampling according to claim 1, characterized in that boundary points are extracted in the prior area by a biased sampling fast search random tree algorithm and are marked as room boundary points; the method specifically comprises the following steps:
c21, adding the position of the heuristic object into the tree structure as a root node;
c22, using a random function to decide which sampling mode to adopt by the offset sampling fast search random tree algorithm each time; the sampling mode comprises uniform random sampling and offset sampling;
c23, traversing all existing nodes on the tree structure, selecting the node closest to the candidate point as the nearest point, and using the connection line between the nearest point and the candidate node as the growth direction; if the distance between the nearest neighbor point and the candidate node exceeds the preset step length, growing a step length along the growth direction by the nearest neighbor point, and taking the reached point as a growth point; if the distance does not exceed the step length, the candidate point is taken as a growing point;
c24, performing collision detection on the connecting line of the growing point and the candidate node on the map, if the collision test is passed, entering the step C25, otherwise, returning to the step C22;
c25, judging whether the growing point passing the collision test is a boundary point, and changing the grid state of part of the boundary points;
and C26, calculating the coverage rate according to the ratio of the number of the boundary points at the current moment to the total number of the boundary points.
5. The indoor environment robot exploration method based on heuristic biased sampling as claimed in claim 4, wherein step C24 specifically comprises: traversing all grid points on a straight line where the connecting lines of the growing points and the candidate nodes are located, then judging the grid state of the grid points, if the state of the grid points is occupied or unknown, failing to pass the collision detection, and returning to the step C22 for sampling again; if the connecting line does not touch the obstacle and the unknown area, the connecting line of the candidate point, the growing point and the candidate node is added into the tree structure.
6. The indoor environment robot exploration method based on heuristic biased sampling of claim 1, wherein said culling of invalid boundary points among RRT boundary points and room boundary points comprises:
c31, clustering the RRT boundary points and the room boundary points through a mean-shift clustering algorithm to obtain centroid points;
c32, detecting the grid state of the boundary point and the value in the costmap in real time, and if the grid state is idle and the value in the costmap exceeds a certain threshold value, the grid point is also removed as an invalid point.
7. The indoor environment robot exploration method based on heuristic biased sampling as claimed in claim 1, wherein constructing the prior area from the position of the heuristic object specifically comprises:
when the robot identifies a heuristic object, if the position of the robot is below the heuristic object, the prior area is above the position of the heuristic object; when the robot position is above the heuristic object, then the prior area is below the position of the heuristic object.
8. The indoor environment robot exploration method based on heuristic biased sampling according to claim 7, wherein the lengths of the prior areas are lengths a respectively extending from the positions of the heuristic objects to both sides, and the width of the prior area is a length 2b extending from the positions of the heuristic objects to the back; wherein a is more than or equal to 2 and less than or equal to 10, and b is more than or equal to 2 and less than or equal to 10.
9. The indoor environment robot exploration method based on heuristic biased sampling according to claim 1, characterized in that step E specifically comprises: a global path planning algorithm A is used for planning a path from the current position of the robot to a target point in a known environment, a DWA local path planning algorithm is combined to enable the robot to well utilize local environment information to complete obstacle avoidance, and the DWA local path planning algorithm are combined to guide the robot to navigate to the target point and update a map.
10. The indoor environment robot exploration method based on heuristic bias sampling as claimed in claim 1, characterized in that, in step a, the whole environment is unknown, the robot is at a certain position in the environment, and the robot acquires data of the surrounding environment information through its own carried sensor.
Priority Applications (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110929934.3A CN113485375B (en) | 2021-08-13 | 2021-08-13 | Indoor environment robot exploration method based on heuristic bias sampling |
| PCT/CN2022/100861 WO2023016101A1 (en) | 2021-08-13 | 2022-06-23 | Heuristic bias sampling-based indoor environment robot exploration method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110929934.3A CN113485375B (en) | 2021-08-13 | 2021-08-13 | Indoor environment robot exploration method based on heuristic bias sampling |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113485375A true CN113485375A (en) | 2021-10-08 |
| CN113485375B CN113485375B (en) | 2023-03-24 |
Family
ID=77945221
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110929934.3A Active CN113485375B (en) | 2021-08-13 | 2021-08-13 | Indoor environment robot exploration method based on heuristic bias sampling |
Country Status (2)
| Country | Link |
|---|---|
| CN (1) | CN113485375B (en) |
| WO (1) | WO2023016101A1 (en) |
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114153210A (en) * | 2021-12-01 | 2022-03-08 | 苏州盈科电子有限公司 | Movement control method and system for robot |
| CN114253272A (en) * | 2021-12-23 | 2022-03-29 | 北京理工大学重庆创新中心 | Indoor autonomous exploration method for unmanned vehicle |
| CN114740866A (en) * | 2022-05-10 | 2022-07-12 | 山东大学 | Robot autonomous exploration method and system based on deep learning |
| CN114812539A (en) * | 2022-06-21 | 2022-07-29 | 杭州涂鸦信息技术有限公司 | Map search method, map using method, map searching device, map using device, robot and storage medium |
| CN115167433A (en) * | 2022-07-22 | 2022-10-11 | 华南理工大学 | Method and system for robot to independently explore environmental information by fusing global vision |
| CN115469662A (en) * | 2022-09-13 | 2022-12-13 | 苏州大学 | Environment exploration method, device and application |
| WO2023016101A1 (en) * | 2021-08-13 | 2023-02-16 | 苏州大学 | Heuristic bias sampling-based indoor environment robot exploration method |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116382309A (en) * | 2023-06-06 | 2023-07-04 | 康复大学(筹) | Robot path planning algorithm based on cylindrical heuristic sampling |
| CN117406771B (en) * | 2023-10-17 | 2024-04-30 | 武汉大学 | Efficient autonomous exploration method, system and equipment based on four-rotor unmanned aerial vehicle |
| CN117405178B (en) * | 2023-12-15 | 2024-03-15 | 成都电科星拓科技有限公司 | Mobile monitoring method for automatically detecting indoor environment parameters |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| GB201803292D0 (en) * | 2018-02-28 | 2018-04-11 | Five Ai Ltd | Efficient computation of collision probabilities for safe motion planning |
| CN108983780A (en) * | 2018-07-24 | 2018-12-11 | 武汉理工大学 | One kind is based on improvement RRT*The method for planning path for mobile robot of algorithm |
| CN110188696A (en) * | 2019-05-31 | 2019-08-30 | 华南理工大学 | A kind of water surface is unmanned to equip multi-source cognitive method and system |
| CN110221614A (en) * | 2019-06-14 | 2019-09-10 | 福州大学 | A kind of multirobot map heuristic approach based on rapid discovery random tree |
| CN110308740A (en) * | 2019-06-28 | 2019-10-08 | 天津大学 | A kind of unmanned aerial vehicle group dynamic task allocation method towards mobile target tracking |
| CN112286202A (en) * | 2020-11-11 | 2021-01-29 | 福州大学 | Mobile robot path planning method for non-uniform sampling FMT |
| CN112327852A (en) * | 2020-11-09 | 2021-02-05 | 苏州大学 | Mobile robot autonomous exploration method integrating path information richness |
| CN113110482A (en) * | 2021-04-29 | 2021-07-13 | 苏州大学 | Indoor environment robot exploration method and system based on priori information heuristic method |
| CN113110522A (en) * | 2021-05-27 | 2021-07-13 | 福州大学 | Robot autonomous exploration method based on composite boundary detection |
Family Cites Families (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP5998816B2 (en) * | 2012-10-04 | 2016-09-28 | セイコーエプソン株式会社 | Route search method, route search device, robot control device, robot, and program |
| CN108762270B (en) * | 2018-06-01 | 2021-04-27 | 上海理工大学 | Improved path planning algorithm for variable probability bidirectional fast search random tree |
| CN109341707B (en) * | 2018-12-03 | 2022-04-08 | 南开大学 | Method for constructing three-dimensional map of mobile robot in unknown environment |
| EP3748455B1 (en) * | 2019-06-07 | 2022-03-16 | Tata Consultancy Services Limited | A method and a system for hierarchical network based diverse trajectory proposal |
| JP2021033685A (en) * | 2019-08-26 | 2021-03-01 | 株式会社デンソー | Learning program and learning method |
| CN112833904B (en) * | 2021-01-05 | 2024-06-04 | 北京航空航天大学 | Unmanned vehicle dynamic path planning method based on free space and rapid random tree searching algorithm |
| CN113050632B (en) * | 2021-03-11 | 2022-06-14 | 珠海一微半导体股份有限公司 | Map exploration method and chip for robot to explore unknown area and robot |
| CN113064426B (en) * | 2021-03-17 | 2022-03-15 | 安徽工程大学 | Intelligent vehicle path planning method for improving bidirectional fast search random tree algorithm |
| CN113485375B (en) * | 2021-08-13 | 2023-03-24 | 苏州大学 | Indoor environment robot exploration method based on heuristic bias sampling |
-
2021
- 2021-08-13 CN CN202110929934.3A patent/CN113485375B/en active Active
-
2022
- 2022-06-23 WO PCT/CN2022/100861 patent/WO2023016101A1/en unknown
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| GB201803292D0 (en) * | 2018-02-28 | 2018-04-11 | Five Ai Ltd | Efficient computation of collision probabilities for safe motion planning |
| CN108983780A (en) * | 2018-07-24 | 2018-12-11 | 武汉理工大学 | One kind is based on improvement RRT*The method for planning path for mobile robot of algorithm |
| CN110188696A (en) * | 2019-05-31 | 2019-08-30 | 华南理工大学 | A kind of water surface is unmanned to equip multi-source cognitive method and system |
| CN110221614A (en) * | 2019-06-14 | 2019-09-10 | 福州大学 | A kind of multirobot map heuristic approach based on rapid discovery random tree |
| CN110308740A (en) * | 2019-06-28 | 2019-10-08 | 天津大学 | A kind of unmanned aerial vehicle group dynamic task allocation method towards mobile target tracking |
| CN112327852A (en) * | 2020-11-09 | 2021-02-05 | 苏州大学 | Mobile robot autonomous exploration method integrating path information richness |
| CN112286202A (en) * | 2020-11-11 | 2021-01-29 | 福州大学 | Mobile robot path planning method for non-uniform sampling FMT |
| CN113110482A (en) * | 2021-04-29 | 2021-07-13 | 苏州大学 | Indoor environment robot exploration method and system based on priori information heuristic method |
| CN113110522A (en) * | 2021-05-27 | 2021-07-13 | 福州大学 | Robot autonomous exploration method based on composite boundary detection |
Non-Patent Citations (5)
| Title |
|---|
| JIE LIU: "An Efficient Robot Exploration Method based on", 《IEEE TRANSACTIONS ON INDUSTRIAL ELECTRONICS》 * |
| 冯蕊: "初始化参数对RRT算法性能影响研究", 《软件导刊》 * |
| 张云峰: "基于改进快速扩展随机树的机械臂路径规划", 《火力与指挥控制》 * |
| 李秀智: "基于复合式协同策略的移动机器人自主探索", 《机器人》 * |
| 李耀仲: "基于稀疏节点快速扩展随机树的移动机械臂运动规划", 《中国机械工程》 * |
Cited By (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2023016101A1 (en) * | 2021-08-13 | 2023-02-16 | 苏州大学 | Heuristic bias sampling-based indoor environment robot exploration method |
| CN114153210A (en) * | 2021-12-01 | 2022-03-08 | 苏州盈科电子有限公司 | Movement control method and system for robot |
| CN114153210B (en) * | 2021-12-01 | 2024-03-19 | 苏州盈科电子有限公司 | Movement control method and system of robot |
| CN114253272A (en) * | 2021-12-23 | 2022-03-29 | 北京理工大学重庆创新中心 | Indoor autonomous exploration method for unmanned vehicle |
| CN114740866A (en) * | 2022-05-10 | 2022-07-12 | 山东大学 | Robot autonomous exploration method and system based on deep learning |
| CN114812539A (en) * | 2022-06-21 | 2022-07-29 | 杭州涂鸦信息技术有限公司 | Map search method, map using method, map searching device, map using device, robot and storage medium |
| CN114812539B (en) * | 2022-06-21 | 2022-11-18 | 杭州涂鸦信息技术有限公司 | Map searching method, map using method, map searching device, map using device, robot and storage medium |
| CN115167433A (en) * | 2022-07-22 | 2022-10-11 | 华南理工大学 | Method and system for robot to independently explore environmental information by fusing global vision |
| CN115167433B (en) * | 2022-07-22 | 2024-03-19 | 华南理工大学 | Method and system for autonomously exploring environment information of robot fusing global vision |
| CN115469662A (en) * | 2022-09-13 | 2022-12-13 | 苏州大学 | Environment exploration method, device and application |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113485375B (en) | 2023-03-24 |
| WO2023016101A1 (en) | 2023-02-16 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113485375B (en) | Indoor environment robot exploration method based on heuristic bias sampling | |
| CN113110482B (en) | Indoor environment robot exploration method and system based on priori information heuristic method | |
| CN111190981B (en) | Method and device for constructing three-dimensional semantic map, electronic equipment and storage medium | |
| WO2020134082A1 (en) | Path planning method and apparatus, and mobile device | |
| Jebari et al. | Multi-sensor semantic mapping and exploration of indoor environments | |
| CN109163722B (en) | Humanoid robot path planning method and device | |
| WO2012155121A2 (en) | Systems and methods for estimating the geographic location at which image data was captured | |
| CN110146080B (en) | SLAM loop detection method and device based on mobile robot | |
| CN114034299B (en) | Navigation system based on active laser SLAM | |
| CN115167433A (en) | Method and system for robot to independently explore environmental information by fusing global vision | |
| CN115143962A (en) | Positioning method, positioning device, electronic equipment and storage medium | |
| Li et al. | Improving autonomous exploration using reduced approximated generalized voronoi graphs | |
| CN111507161B (en) | Method and device for heterogeneous sensor fusion by utilizing merging network | |
| CN115145289A (en) | Multi-agent cooperative trapping method, system, equipment and storage medium | |
| CN114089752A (en) | Autonomous exploration method for robot, and computer-readable storage medium | |
| Zuo et al. | An improved autonomous exploration framework for indoor mobile robotics using reduced approximated generalized voronoi graphs | |
| Zhang et al. | A symbolic-AI approach for UAV exploration tasks | |
| CN117053779A (en) | Tightly coupled laser SLAM method and device based on redundant key frame removal | |
| CN114740867A (en) | Intelligent obstacle avoidance method and device based on binocular vision, robot and medium | |
| CN115469662A (en) | Environment exploration method, device and application | |
| Jebari et al. | Combined vision and frontier-based exploration strategies for semantic mapping | |
| Zhang et al. | Research on AGV map building and positioning based on SLAM technology | |
| Xu et al. | Hybrid frontier detection strategy for autonomous exploration in multi-obstacles environment | |
| Tian et al. | Autonomous exploration of RRT robot based on seeded region growing | |
| Wang et al. | Path planning model of mobile robots in the context of crowds |
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 |