CN110716547A - 3D exploration method based on wavefront algorithm - Google Patents
3D exploration method based on wavefront algorithm Download PDFInfo
- Publication number
- CN110716547A CN110716547A CN201911041636.XA CN201911041636A CN110716547A CN 110716547 A CN110716547 A CN 110716547A CN 201911041636 A CN201911041636 A CN 201911041636A CN 110716547 A CN110716547 A CN 110716547A
- Authority
- CN
- China
- Prior art keywords
- map
- exploration
- boundary point
- mobile robot
- algorithm
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 45
- 238000004422 calculation algorithm Methods 0.000 title claims abstract description 35
- 238000009792 diffusion process Methods 0.000 claims description 11
- 238000010586 diagram Methods 0.000 description 9
- 238000013507 mapping Methods 0.000 description 6
- 238000010276 construction Methods 0.000 description 2
- 230000003247 decreasing effect Effects 0.000 description 2
- 238000011156 evaluation Methods 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 206010063385 Intellectualisation Diseases 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000001502 supplementing effect Effects 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/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/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/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/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)
Abstract
The invention discloses a 3D exploration method based on a wavefront algorithm, which comprises the following steps: step 1, collecting data of a current environment map, and splicing the collected data of each frame; step 2, performing obstacle expansion on the map obtained after splicing; step 3, finding out boundary points by using the process of diffusing to the periphery in the wave-front algorithm; step 4, selecting an optimal boundary point according to a set boundary point selection function; step 5, planning a path from the current position of the mobile robot to the boundary point by using a path planning method based on a wave-front algorithm; and 6, judging whether boundary points exist in the map or not, if so, returning to the step 1 to continue exploration, and if not, ending the exploration process. The invention can solve the problem of 3D exploration and map building of the mobile robot.
Description
Technical Field
The invention belongs to the field of autonomous exploration and graph building of mobile robots, and particularly relates to a 3D exploration method based on a wavefront algorithm.
Background
Autonomous mobile robot exploration is an important component of the intelligent mobile robot function. When a mobile robot performs a task in an unknown environment, the surrounding environment needs to be modeled first. However, in the past, a mobile robot completes a task of drawing first under the control of people. Autonomous exploration mapping emphasizes that the mobile robot completes the mapping task without human intervention. When facing a dangerous environment or an environment where other people cannot reach, the mobile robot can fully exert the advantage of self-map building, and the efficiency of the mobile robot in executing tasks is greatly improved. Therefore, autonomous exploration mapping is the key to improving mobile intelligence.
The first task of exploration mapping is to find the next explored boundary point from the known map. The boundary point is at the intersection of the known region and the unknown region. According to the characteristic of outward diffusion of the wave-front algorithm, when an unknown voxel is encountered, a boundary point is found. At this time, the starting point of the wave-front algorithm is the current position of the robot, and the wave-front algorithm can diffuse outwards all the time until the known area of the whole map is traversed, so that the wave-front algorithm diffusion process is finished. And after the robot reaches the boundary point and builds the map, searching the next boundary point by taking the current robot position as the starting point of the wave-front algorithm until the whole map is explored.
The path for finding the boundary point is mainly two: the first is an image processing based method; the second is a method of finding boundary points at random. Image processing based methods require a large amount of computation to traverse the entire map. For a scene with a large environment, the method needs a large amount of calculation and takes a long time. A Random-finding boundary point method, such as an algorithm based on rapid-exploration Random Trees (RRT). Randomness may speed up finding boundary points in the map. However, when the environment is complicated, for example, there are many corners in the environment, the random search method may cause the efficiency of searching for the boundary point to be low.
Therefore, the research on rapidly finding the boundary points is the key of the mobile robot for independently exploring and establishing the diagram, and has important significance for improving the intellectualization of the mobile robot.
Disclosure of Invention
The invention aims to: A3D exploration method based on a wavefront algorithm is provided to solve the problem of mobile robot exploration and mapping.
The technical scheme of the invention is as follows: a method of 3D exploration based on a wavefront algorithm, comprising the steps of:
and 6, judging whether boundary points exist in the map or not, if so, returning to the step 1 to continue exploration, and if not, ending the exploration process.
In the above technical solution, the current environment in step 1 is a three-dimensional space, and the map is a three-dimensional oct map.
In the above technical solution, the current environment information is acquired by using the depth camera sensor in step 1, and the map is obtained by splicing data of each frame of camera.
In the above, the oct map contains occupied, free and unknown information. When the point cloud map is constructed, the invalid point cloud with the depth value of 0 needs to be reassigned. Because when there is no obstacle in front of the robot, the depth camera will assign the depth of the point to an invalid value, so that the voxels that should be known areas cannot be updated to be known when the oct map is constructed, which affects finding the boundary points in the map during exploration. In order to obtain complete map information, a point supplementing method is adopted, and point cloud with an invalid value as a depth value is assigned again.
In the above technical solution, in step 2, the voxels occupied by the obtained map are expanded by α grid units to the periphery, where the number α of expanded grid units is:
α＝d/r+c
where d is the diameter of the mobile robot, r is the resolution of the map, and c is a constant. If the mobile robot is far away from the obstacle, increasing the value of c; if it is not desired to move the mobile robot too far from the obstacle, the value of c is decreased.
In the above technical solution, in the step 3, the starting point of the wavefront algorithm is the current position of the mobile robot, and a plurality of boundary points are found in the diffusion process. The specific steps of the step 3 are as follows:
step 31, starting diffusion from the current position of the mobile robot, and finding out boundary points in a map;
and 32, selecting an optimal boundary point from the boundary points in the step 31 for searching, and repeating the steps until the whole map is searched.
In the above technical solution, in the step 4, an optimal boundary point is selected according to a set boundary point selection function. The considered conditions are the distance d from the mobile robot to the boundary point, the number m of unknown voxels at the boundary point and the number n of occupied voxels at the boundary point, and the evaluation function f is:
f＝C1d+C2m+C3n
in the above technical solution, in the step 5, a path is planned by using a method based on path planning in a wavefront algorithm. The planned path is from the current position of the mobile robot to the optimal boundary point. A path consists of the coordinates of a series of voxels.
In the above technical solution, the specific operation of determining whether the search is finished in step 6 is that, when the mobile robot reaches the boundary point and completes the map construction, the mobile robot continues to search for the next boundary point. If the boundary point is found, continuing exploring the environment; if no boundary point is found after traversing the whole map, the exploration process is finished.
The invention has the advantages that:
the invention uses the wave-front algorithm out-diffusion method to search the boundary points in the map, the method can quickly find the boundary points in the map, and the speed of finding the boundary points is irrelevant to the complexity of the map; the boundary point selection function can determine the next explored boundary point according to the actual requirement; the path planned according to the method of planning a path in the wavefront algorithm is the shortest and is also passable safely.
Drawings
The invention is further described with reference to the following figures and examples:
fig. 1 is a flowchart of a method according to a first embodiment of the present invention.
Fig. 2 is a schematic diagram of the joined octomap map.
FIG. 3 is a schematic diagram of a two-dimensional grid map prior to inflation.
FIG. 4 is a schematic diagram of an expanded two-dimensional grid map.
FIG. 5 is a schematic diagram of the process of finding boundary points using wave front algorithm diffusion.
Fig. 6 is a schematic diagram of calculating the number of unknown voxels and the number of occupied voxels at a boundary point.
Fig. 7 is a schematic diagram of calculating the distance d for the robot to reach the boundary point.
Fig. 8 is a schematic diagram of path planning based on a wavefront algorithm.
FIG. 9 is a schematic diagram of a simulation process of three-dimensional exploration mapping.
Detailed Description
The first embodiment is as follows:
in order to achieve the object of the present invention, as shown in fig. 1, in one embodiment of the present invention, there is provided a method for 3D exploration based on a wavefront algorithm, including the steps of:
the current environment is a three-dimensional space, the map is a three-dimensional oct map, the current environment information is acquired by using a depth camera sensor in the step 1, and the map is obtained by splicing data of each frame of camera.
Specifically, as shown in fig. 2, an oct map is obtained by stitching each frame of data acquired by the depth camera sensor. In constructing maps, the method of padding has been used. When the map is spliced, the position and attitude information of the mobile robot is obtained by the odometer carried by the mobile robot.
specifically, the voxels occupied by the obtained map are expanded by α grid units to the periphery, wherein the number α of the expanded grid units is:
α＝d/r+c
where d is the diameter of the mobile robot, r is the resolution of the map, and c is a constant. If the mobile robot is far away from the obstacle, increasing the value of c; if it is not desired to move the mobile robot too far from the obstacle, the value of c is decreased.
More specifically, as shown in fig. 3 and 4, is a grid map of the environment. Taking a two-dimensional grid map as an example, the size of the expansion is larger than the radius of the mobile robot. If the voxel is occupied, it expands all around. The inflated map can avoid the mobile robot colliding with obstacles in the environment. Fig. 3 is a map before inflation, and fig. 4 is a map after inflation.
specifically, the step 3 includes:
step 31, starting diffusion from the current position of the mobile robot, and finding out boundary points in a map;
and 32, selecting an optimal boundary point from the boundary points in the step 31 for searching, and repeating the steps until the whole map is searched.
More specifically, as shown in fig. 5, a process for finding a boundary based on the out-diffusion of a wavefront algorithm. Taking a two-dimensional grid as an example, diffusion starts from a starting point, and the starting point grid value is 0. The next grid is an adjacent grid to the previous grid. The value of each layer of grids is increased by 1 compared to the value of the previous layer of grids. When an occupancy grid is encountered, the value of the grid does not change.
the considered conditions are the distance d from the mobile robot to the boundary point, the number m of unknown voxels at the boundary point and the number n of occupied voxels at the boundary point, and the evaluation function f is:
f＝C1d+C2m+C3n。
specifically, as shown in fig. 6, the number of unknown voxels and the number of occupied voxels at the boundary point are calculated. And constructing a cube with the side length of 6 voxels by taking the boundary point as a center. Then, the number of known and unknown voxels in the cube is calculated from the map information obtained at this time. As shown in fig. 7, the distance d to the boundary point is calculated. According to the diffusion process of the waveform, when the boundary point G is reached, the value in the waveform grid is 6, and the distance from the robot position S point to the boundary point G point is 6.
Specifically, as shown in fig. 8, a path planning process based on a wavefront algorithm is provided. Taking a two-dimensional grid as an example, the waveform is diffused from the initial S point of the robot, and the found boundary point is the G point. When planning the path, the path is planned in the descending order of the grid values from the boundary point G. For example, the starting point has a grid value of 8, the grid value of the starting point is compared with its surrounding 8 grid values, and the initially unknown grid value is assigned a value of-1. When a grid with a grid value 8 smaller than the initial grid value is found and the value of the grid is not equal to-1, the grid is reserved and compared with the surrounding grids as a new comparison grid, and the method is carried out until the starting point S is returned, and the whole path planning process is finished.
the specific operation of determining whether the search is finished in step 6 is that, when the mobile robot reaches the boundary point and completes the map construction, the mobile robot continues to search for the next boundary point. If the boundary point is found, continuing exploring the environment; if no boundary point is found after traversing the whole map, the exploration process is finished.
Fig. 9 shows a 3D exploration process of a simulation experiment. In the figure, a green small cube is a found boundary point, a red line is a planned path, and a blue area is an area covered by the found boundary point by the wave-front algorithm.
It should be understood that the above-mentioned embodiments are only illustrative of the technical concepts and features of the present invention, and are intended to enable those skilled in the art to understand the contents of the present invention and implement the present invention, and not to limit the scope of the present invention. All modifications made according to the spirit of the main technical scheme of the invention are covered in the protection scope of the invention.
Claims (5)
1. A method for 3D exploration based on a wavefront algorithm is characterized by comprising the following steps:
step 1, collecting data of a current environment map, and splicing the collected data of each frame;
step 2, performing obstacle expansion on the map obtained after splicing;
step 3, finding out boundary points by using the process of diffusing to the periphery in the wave-front algorithm;
step 4, selecting an optimal boundary point according to a set boundary point selection function;
step 5, planning a path from the current position of the mobile robot to the boundary point by using a path planning method based on a wave-front algorithm;
and 6, judging whether boundary points exist in the map or not, if so, returning to the step 1 to continue exploration, and if not, ending the exploration process.
2. The method of wavefront algorithm based 3D exploration according to claim 1, characterized in that: the current environment in the step 1 is a three-dimensional space, and the map is a three-dimensional oct map.
3. The method of wavefront algorithm based 3D exploration according to claim 1, characterized in that: in the step 1, the current environment information is acquired by using a depth camera sensor, and the map is obtained by splicing data of each frame of camera.
4. The method of wavefront algorithm based 3D exploration according to claim 1, characterized in that: in step 2, the voxels occupied by the obtained map are expanded by α grid units to the periphery, wherein the number α of the expanded grid units is:
α＝d/r+c
where d is the diameter of the mobile robot, r is the resolution of the map, and c is a constant.
5. The method of wavefront algorithm based 3D exploration according to claim 1, characterized in that: the specific steps of the step 3 are as follows:
step 31, starting diffusion from the current position of the mobile robot, and finding out boundary points in a map;
and 32, selecting an optimal boundary point from the boundary points in the step 31 for searching, and repeating the steps until the whole map is searched.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911041636.XA CN110716547A (en) | 2019-10-30 | 2019-10-30 | 3D exploration method based on wavefront algorithm |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201911041636.XA CN110716547A (en) | 2019-10-30 | 2019-10-30 | 3D exploration method based on wavefront algorithm |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN110716547A true CN110716547A (en) | 2020-01-21 |
Family
ID=69214538
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201911041636.XA Pending CN110716547A (en) | 2019-10-30 | 2019-10-30 | 3D exploration method based on wavefront algorithm |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110716547A (en) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112765304A (en) * | 2021-04-06 | 2021-05-07 | 平安科技（深圳）有限公司 | Automatic completion method and device for polyline boundary scatter points and related equipment |
| CN114384911A (en) * | 2022-01-11 | 2022-04-22 | 云南民族大学 | Multi-unmanned system collaborative autonomous exploration method and device based on boundary guide points |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106840168A (en) * | 2017-03-16 | 2017-06-13 | 苏州大学 | Complete coverage path planning method under clean robot and its dynamic environment |
| CN110244716A (en) * | 2019-05-28 | 2019-09-17 | 苏州大学 | A method of the robot based on wave front algorithm is explored |
| CN110264517A (en) * | 2019-06-13 | 2019-09-20 | 上海理工大学 | A kind of method and system determining current vehicle position information based on three-dimensional scene images |
-
2019
- 2019-10-30 CN CN201911041636.XA patent/CN110716547A/en active Pending
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106840168A (en) * | 2017-03-16 | 2017-06-13 | 苏州大学 | Complete coverage path planning method under clean robot and its dynamic environment |
| CN110244716A (en) * | 2019-05-28 | 2019-09-17 | 苏州大学 | A method of the robot based on wave front algorithm is explored |
| CN110264517A (en) * | 2019-06-13 | 2019-09-20 | 上海理工大学 | A kind of method and system determining current vehicle position information based on three-dimensional scene images |
Non-Patent Citations (2)
| Title |
|---|
| CHUNHUA TANG 等: "Autonomous Indoor Mobile Robot Exploration Based on Wavefront Algorithm", 《SPRINGER NATURE SWITZERLAND》 * |
| 王宏生 等: "空间散乱点三角剖分波前算法的研究", 《第九届中国青年信息与管理学者大会论文集》 * |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112765304A (en) * | 2021-04-06 | 2021-05-07 | 平安科技（深圳）有限公司 | Automatic completion method and device for polyline boundary scatter points and related equipment |
| CN112765304B (en) * | 2021-04-06 | 2021-07-06 | 平安科技（深圳）有限公司 | Automatic completion method and device for polyline boundary scatter points and related equipment |
| CN114384911A (en) * | 2022-01-11 | 2022-04-22 | 云南民族大学 | Multi-unmanned system collaborative autonomous exploration method and device based on boundary guide points |
| CN114384911B (en) * | 2022-01-11 | 2024-06-04 | 云南民族大学 | Multi-unmanned system collaborative autonomous exploration method and device based on boundary guide points |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109541634B (en) | Path planning method and device and mobile device | |
| CN113110457B (en) | Autonomous coverage inspection method for intelligent robot in indoor complex dynamic environment | |
| WO2023016101A1 (en) | Heuristic bias sampling-based indoor environment robot exploration method | |
| CN106774347A (en) | Robot path planning method, device and robot under indoor dynamic environment | |
| US20200192399A1 (en) | Method for traversing a subarea, method for cleaning, and cleaning robot thereof | |
| CN109947120B (en) | Path planning method in warehousing system | |
| CN109163722B (en) | Humanoid robot path planning method and device | |
| CN107677279A (en) | It is a kind of to position the method and system for building figure | |
| CN106643701A (en) | Robot inter-detection method and robot inter-detection device | |
| CN110986953B (en) | Path planning method, robot and computer readable storage medium | |
| CN111152266B (en) | Control method and system of cleaning robot | |
| CN112066976B (en) | Self-adaptive expansion processing method, system, robot and storage medium | |
| CN108334080A (en) | A kind of virtual wall automatic generation method for robot navigation | |
| US6636847B1 (en) | Exhaustive search system and method using space-filling curves | |
| CN113189988B (en) | Autonomous path planning method based on Harris algorithm and RRT algorithm composition | |
| CN111679661A (en) | Semantic map construction method based on depth camera and sweeping robot | |
| CN110716547A (en) | 3D exploration method based on wavefront algorithm | |
| CN110244716A (en) | A method of the robot based on wave front algorithm is explored | |
| CN113405557B (en) | Path planning method and related device, electronic equipment and storage medium | |
| CN110956161A (en) | Autonomous map building method and device and intelligent robot | |
| CN108387240B (en) | Construction method of multilayer hexagonal grid map | |
| JP6810432B2 (en) | A method of detecting a pseudo 3D bounding box used for military purposes, smartphones or virtual driving on a CNN platform that can switch modes according to the conditions of the object, and a device using this | |
| CN109798899B (en) | Tree diffusion heuristic path planning method for submarine unknown terrain search | |
| CN113244619B (en) | Data processing method, device, equipment and storage medium | |
| Jarvis | Distance transform based path planning for robot navigation |
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 | ||
| RJ01 | Rejection of invention patent application after publication | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20200121 |