Three-dimensional environment coverage method for online sub-region updating and hierarchical planning

Abstract

The invention discloses a three-dimensional environment coverage method for on-line sub-region updating and hierarchical planning, which remarkably improves the exploration efficiency and reduces the calculation consumption through on-line sub-region updating and hierarchical planning. Firstly, a fast environment preprocessing method is used, only updated boundary extraction leading edge points are checked, and therefore the calculation burden is reduced. Secondly, constructing a sparse topological graph, discretizing the space, and storing topological nodes by using a KD tree, so that time-consuming path search on a three-dimensional voxel map is avoided; in addition, hierarchical structure division of sub-regions is provided, the environment is divided into a plurality of adaptive sub-regions, environment information is updated in real time, and subsequent exploration planning is supported. A hierarchical planning strategy is adopted, environment coverage is modeled as a traveling salesman problem, a global coverage path is solved, and the total path length of subarea access is considered. Compared with a greedy strategy, the method provides a long-term perspective and improves the exploration efficiency. Finally, the efficiency and feasibility of the method are proved through simulation and actual measurement verification.

Description

Technical Field

[0001] The invention belongs to the technical field of ground robots, and in particular relates to a three-dimensional environment coverage method with online sub-region updating and hierarchical planning. Background Art

[0002] In recent years, the widespread application of ground mobile robots in key areas such as search and rescue, environmental monitoring and industrial automation has significantly improved operational efficiency and ensured the safety of personnel in extreme or dangerous conditions. Its core advantage is that robots can autonomously navigate and explore unknown environments without direct human intervention. Although a variety of autonomous exploration technologies and methods have emerged after decades of research and development, it is still a severe challenge to achieve efficient and comprehensive autonomous exploration tasks in large-scale and topologically complex environments. The mainstream exploration framework mainly includes boundary-based methods and sampling-based methods. Boundary methods focus on identifying and prioritizing the exploration of the boundary between free space and unknown areas, while sampling methods guide the exploration process by randomly selecting and evaluating potential viewpoints. Both methods gradually cover the entire environment through continuous iterative search and evaluation. However, some limitations still hinder the development of autonomous exploration, as described below:

[0003] The first is the high computational cost. The precise detection of boundaries and the potential benefit evaluation of viewpoints involve complex voxel occupancy status checks and ray tracing algorithms, which result in huge consumption of computing resources. For resource-constrained edge computing platforms, it is particularly difficult to meet real-time requirements. The second is low efficiency. Most existing methods tend to adopt a greedy strategy, that is, directly go to the target point with the highest benefit under the current evaluation, while ignoring the optimization of the global coverage path. This short-sighted strategy often leads to unnecessary repeated exploration and waste of resources, especially in large-scale environments. Although there have been attempts to solve the global optimal path through the traveling salesman problem (TSP), the complexity of solving the TSP increases sharply with the increase in the number of exploration viewpoints, making it difficult to implement in practical applications.

[0004] In view of the problems of high computing resource consumption and low exploration efficiency in current methods, the present invention aims to provide a lightweight and efficient autonomous exploration method and system for covering three-dimensional scenes. Summary of the invention

[0005] The present invention aims to solve the technical problems existing in the background technology and to provide a three-dimensional environment coverage method with online sub-region updating and hierarchical planning.

[0006] In order to solve the technical problem, the technical solution of the present invention is:

[0007] A three-dimensional environment coverage method with online sub-region updating and hierarchical planning, the specific technical scheme is as follows:

[0008] Step S1: Real-time positioning and mapping: The laser radar and inertial measurement unit on the ground unmanned platform are used to scan and map the current environment using the SLAM (simultaneous localization and mapping) algorithm, while obtaining the position information of the unmanned platform, and using the UFOMap mapping algorithm to generate a three-dimensional voxel map of the current environment;

[0009] Step S2: Rapid environment preprocessing: Rapid preprocessing of the currently mapped environment area, mainly including incremental frontier point extraction, sparse topology road map construction and viewpoint generation;

[0010] Step S3: Sub-area division: The entire unknown environment to be covered is initially divided into several sub-area units. Each sub-area unit stores the basic environmental information of the unit, namely the sub-area information structure (SIS), which is used for subsequent exploration planning. Each sub-area unit has a hierarchical structure and is dynamically decomposed according to the volume ratio of the unknown area in the unit. At the same time, the sub-area information structure will be continuously updated to adapt to the dynamic changes of the unknown area;

[0011] Step S4: Global coverage path generation: Based on the divided sub-areas, for the sub-areas with viewpoints inside, arrange the entry order of these sub-areas by solving the asymmetric traveling salesman problem to determine the access priority of each sub-area at time t;

[0012] Step S5: Local exploration path optimization: Under the guidance of the global coverage path, the access order of each viewpoint in the sub-area within a certain range from the unmanned platform is optimized by solving the asymmetric traveling salesman problem again;

[0013] Step S6: motion planning: according to the order of viewpoint visits, the next viewpoint to be visited is sent to the motion planning module, prompting the robot to explore the target;

[0014] Step S7: Check whether there are any unvisited viewpoints in the map. If there are no viewpoints, end the exploration, otherwise repeat the above steps.

[0015] Furthermore, the laser radar and inertial measurement unit carried by the ground unmanned platform are used to scan and map the current environment using the SLAM algorithm, while obtaining the position information of the unmanned platform, and using the UFOMap mapping algorithm to generate a three-dimensional voxel map of the current environment; based on the generated three-dimensional voxel map of the current environment, the currently mapped environmental area is quickly preprocessed, including incremental frontier point extraction, sparse topology road map construction and viewpoint generation, so as to obtain effective information about the current environment.

[0016] Furthermore, the input of the SLAM algorithm is the point cloud data at the current moment and the vehicle body acceleration, angular velocity and angular acceleration information measured by the inertial measurement unit, and the output is the robot's pose estimation and point cloud map; the input of the UFOMap algorithm is the pose estimation and the point cloud data at the current moment, and the output is a three-dimensional voxel map of the explored part of the environment.

[0017] Furthermore, the rapid preprocessing of the currently mapped environment area specifically includes:

[0018] Incremental extraction of frontier points: Get all the voxel sets with changed occupancy values ​​from UFOMap, check the state of each voxel, and if a voxel is in a free state and its adjacent voxels are in an unknown state, mark the voxel as a frontier voxel. At the same time, during the exploration process, check whether each frontier voxel in the map update area is still valid, and remove invalid frontier voxels.

[0019] Construct a sparse topological road map: The nodes in the topological map represent key points in the environment, and the edges represent paths connecting adjacent nodes. During the exploration process, candidate nodes are generated by random sampling. The sampled nodes that meet the conditions will undergo collision checks, which are performed using UFOMap's ray tracing operations and then added to the topological map. The weight of the edge is set to the distance between the two nodes, and all nodes are stored in a KD tree structure to improve computational efficiency.

[0020] Viewpoint generation: For each frontier point, find the node closest to the frontier in the topological graph and use this node as the viewpoint.

[0021] Furthermore, the initial division of the entire unknown environment to be covered specifically includes:

[0022] Sub-area hierarchical recursive division: In the initial stage of exploration, the entire environment to be explored is divided into multiple sub-area units along the axis-aligned boundary, and the initial level of each unit is l i = 1, the sub-region will be used as the basic unit for subsequent exploration and planning tasks; each sub-region unit can be recursively divided through a quadtree structure. Initially, the sub-region unit is large, but as the exploration proceeds, the volume of the unknown area decreases, and the original sub-region is further subdivided. i The sub-area of ​​the level will become l i+1 Level, each sub-area unit is divided into smaller sub-areas in the process of increasing levels, so as to capture the dynamically changing environment during the exploration process;

[0023] Sub-region information structure: Each sub-region unit contains its level information l i , axis-aligned bounding box range, known area volume ratio R known, all viewpoints VP in the sub-region, the centroid C of all viewpoints avg , the distance between each sub-area is L cost Key information, used to dynamically update the status of the sub-area;

[0024] Sub-area online update: During the exploration process, the environment map is constantly updated, and the sub-area units intersecting with the map update area are recalculated and their information structure is updated. If the volume proportion of the known area in the sub-area exceeds a certain threshold and has not reached the maximum level l max , will be further divided into smaller sub-area units, and at the same time, the distance length between sub-area units is updated, l max The calculation rules are as follows:

[0025]

[0026] Among them, W max and W min Indicates the maximum and minimum side lengths allowed for the sub-region, Indicates a round-up operation.

[0027] Furthermore, by solving the asymmetric traveling salesman problem, the order of entering each sub-area is arranged, including:

[0028] The global coverage path problem is transformed into an asymmetric traveling salesman problem by constructing the distance cost matrix M between each sub-region. cost , using OR-Tools and other operations optimization solvers, we can calculate a global path that enters all sub-regions in sequence and has the shortest distance. Only sub-regions with more than 0 internal viewpoints will be included in the global coverage path solution process. The distance cost matrix M cost The calculations for each element are as follows:

[0029] M cost (i,j)=L cost,i,j ,i,j∈{1,2,…,N act}

[0030] Where L cost,i,j represents the distance cost between sub-area i and sub-area j. This part of the cost has been calculated in the sub-area online update process in step S3, so it can be directly filled in. N act Indicates the total number of sub-regions whose internal viewpoints are greater than 0;

[0031]

[0032] in, represents the distance cost from the current position p0 of the unmanned platform to the sub-area k, where p0 is obtained by estimating the position and posture output by the SLAM algorithm;

[0033] Finally, we need to transform the standard traveling salesman problem into an asymmetric traveling salesman problem to optimize the solution speed:

[0034] M cost (k,0)=0,k∈{1,2,…,N act}

[0035] The path length cost in the above process is calculated in the constructed sparse topology graph; specifically, an A* search algorithm is used to search in the topology graph to obtain a collision-free path connecting the search starting point and the target point, and the distance length cost between the two points can be obtained by calculating the length of the path;

[0036] The constructed cost matrix M cost Input into OR-Tools for optimization and solution. The output of the solver is the index sequence of entering these sub-regions in turn, denoted as G * =[S1,S2,…,S act ],S i represents the index of the sub-region, so G * That is, the shortest path that covers all sub-areas in sequence.

[0037] Furthermore, by solving the asymmetric traveling salesman problem, the access order of each viewpoint in the sub-area within a certain range from the unmanned platform is optimized, including:

[0038] According to the obtained entry order of the sub-areas G * , from G * Select the distance robot radius r max N within near sub-regions, and obtain the viewpoints of these sub-regions: Recorded as a total of N v viewpoints, among which Represents the jth viewpoint inside the i-th sub-region, searches and calculates the viewpoint v in the sparse topological road graph m and viewpoint v n The distance cost between cost,m,n And the current position of the unmanned platform p0 reaches the viewpoint v l The distance cost

[0039] M cost (m,n)=D cost,m,n ,m,n∈{1,2,…N V}

[0040]

[0041] And for M costAll elements in the first row of are filled with 0 to construct the asymmetric traveling salesman problem:

[0042] M cost (l,0)=0,l∈{1,2,…N V}

[0043] The cost matrix M cost Sent to OR-Tools for optimization and solution, subject to the global coverage path G * The optimization problem adds constraints, namely, the viewpoint Should be compared to the viewpoint The first viewpoint is visited. This constraint can be set in OR-Tools, so the order of viewpoint visits obtained after solving will satisfy the above constraints and ensure the overall shortest access path length.

[0044] Furthermore, the next viewpoint to be visited is sent to the motion planning module to guide the robot to the exploration target, including:

[0045] According to the obtained viewpoint access sequence, the viewpoint that should be visited at the current time t is selected as the target point, and the target point is sent to the path planning module, which is responsible for planning a path from the robot to the target point and effectively avoiding obstacles during driving;

[0046] The specific operation includes global path planning and local path planning. The global path planning uses the A* algorithm, combined with positioning information and sparse topology map search to obtain the overall path. The local path planning uses point cloud data to obtain obstacle location information, and applies the DWA dynamic window algorithm to achieve local obstacle avoidance during the robot's driving process, so as to ensure that the robot can reach the target area safely and smoothly.

[0047] A computer device comprises a memory, a processor and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, a three-dimensional environment coverage method with online sub-region updating and hierarchical planning as claimed in any one of claims 1 to 8 is implemented.

[0048] A computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements a three-dimensional environment coverage method with online sub-region updating and hierarchical planning as described in any one of claims 1 to 8.

[0049] Compared with the prior art, the advantages of the present invention are:

[0050] The present invention proposes a three-dimensional environment coverage method through online sub-region updating and hierarchical planning, which solves the problems of high computational consumption and low coverage efficiency of current exploration methods. First, the method proposes a fast environment preprocessing method, which only needs to extract the frontier points by checking the updated boundary parts in the map, greatly reducing the computational consumption. Secondly, during the exploration process, the method continuously constructs a sparse topological map of the environment, discretizes the continuous space, and stores the topological nodes in a KD tree, which is convenient for subsequent path search and node query, avoiding the expensive path search operation in the three-dimensional voxel map.

[0051] Secondly, a hierarchical structure division method of sub-regions is proposed. This method divides the entire environment to be explored into several sub-region units and allows the sub-region units to be adaptively decomposed to capture the dynamic changes of the unknown area. During the exploration process, each sub-region unit stores the internal environmental information through the sub-region information structure (SIS) and updates it autonomously during the exploration process to serve the subsequent exploration planning.

[0052] Thirdly, using the hierarchical planning strategy, the environmental coverage problem is first modeled as a traveling salesman problem, and the global coverage path based on sub-regional units is solved. This problem takes into account the overall path length required to visit these sub-regions. Compared with the greedy strategy of common exploration methods, the proposed method provides a long-term perspective for subsequent exploration planning, thereby avoiding the short-sighted phenomenon in the exploration process. Under the guidance of the global coverage path, the local exploration path is refined to further determine the next target to be explored. This coarse-to-fine decision-making method reduces the difficulty of solving the optimization problem and improves the exploration efficiency.

[0053] Finally, this method was verified in simulation and real environments, and the experimental results confirmed the efficiency and feasibility of this method. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] Figure 1 , a flow chart of a three-dimensional environment coverage method for online sub-region updating and hierarchical planning provided by the present invention;

[0055] Figure 2 , visualization effect diagram of environmental pretreatment;

[0056] Figure 3 , a schematic diagram of the sub-region hierarchy;

[0057] Figure 4 , one A schematic diagram of the regional division with only two levels;

[0058] Figure 5 , a diagram representing hierarchical planning;

[0059] Figure 6, Experimental results in real scenarios. DETAILED DESCRIPTION

[0060] The specific implementation mode of the present invention is described below in conjunction with embodiments:

[0061] It should be noted that the structures, proportions, sizes, etc. shown in this specification are only used to match the contents disclosed in the specification so that people familiar with this technology can understand and read them, and are not used to limit the conditions under which the present invention can be implemented. Any structural modification, change in proportional relationship or adjustment of size should still fall within the scope of the technical content disclosed in the present invention without affecting the effects and purposes that can be achieved by the present invention.

[0062] At the same time, the terms such as "upper", "lower", "left", "right", "middle" and "one" cited in this specification are only for the convenience of description and are not used to limit the scope of implementation of the present invention. Changes or adjustments to their relative relationships should be regarded as the scope of implementation of the present invention without substantially changing the technical content.

[0063] Embodiment 1:

[0064] Figure 1 This is a flow chart of a three-dimensional environment coverage method for online sub-region updating and hierarchical planning provided by the present invention.

[0065] Figure 2 It is a visualization of the environment preprocessing. During the exploration process, the voxel state is incrementally checked to extract the frontier voxels, and new nodes are continuously sampled around the unmanned platform to expand the topological road map. The node on the topological map closest to the frontier voxel will be used as the viewpoint.

[0066] Figure 3 is a schematic diagram of the sub-region hierarchy. The sub-region unit can be further subdivided to use smaller units to represent the unknown area, up to the maximum level l max .

[0067] Figure 4 This is a schematic diagram of a region partition with only two levels. If the known voxel ratio in a sub-region unit exceeds a threshold, the initial region will be further subdivided. Regions without viewpoints inside will be considered invalid sub-region units and will not be included in the subsequent global coverage planning.

[0068] Figure 5 The diagram shows the hierarchical planning. The arrow line represents the global coverage path G* that enters all valid sub-areas in sequence, and the dotted line represents the local path optimized under the guidance of the global path. The local path will visit r max All viewpoints within.

[0069] Figure 6 The experimental results in real-world scenarios are shown. The upper left corner of the picture shows the unmanned experimental platform. Figure 6 The right side is the real view of the garage. Figure 6 The picture below uses a point cloud to represent the results of the exploration environment, the curve represents the exploration trajectory of the unmanned platform, and the dot represents the starting point of the exploration.

[0070] like Figure 1 As shown in FIG, the present invention proposes a three-dimensional environment coverage method through online sub-region updating and hierarchical planning, which is verified and tested under the Robot Operating System (ROS). Figure 6 As shown, the specific steps of this method are as follows:

[0071] Step S1: Use the point cloud data at the current moment and the vehicle acceleration, angular velocity and angular acceleration information measured by the inertial measurement unit as the input information of the SLAM algorithm to obtain the pose estimation information of the unmanned platform, and use the UFOMap algorithm to generate a three-dimensional voxel map of the known environment.

[0072] Step S2: Rapid environment preprocessing, extracting data information from the current environment for subsequent exploration planning. The specific processing includes three parts: incremental extraction of frontier points, construction of sparse topology roadmaps, and viewpoint generation. The schematic effect diagram of environment preprocessing is shown in the figure below. Figure 2 The specific processing methods are as follows:

[0073] (1) Incremental extraction of frontier points: Obtain a set of all voxels whose occupancy values ​​have changed from UFOMap and check the state of each voxel. If a voxel is in a free state and its adjacent voxels are in an unknown state, mark the voxel as a frontier voxel. At the same time, during the exploration process, check whether each frontier voxel in the map update area is still valid, and remove invalid frontier voxels. This method only needs to check a small number of voxels in the map, which significantly reduces the workload of frontier point extraction.

[0074] (2) Construct a sparse topological road map: The nodes in the topological map represent key points in the environment, and the edges represent paths connecting adjacent nodes. During the exploration process, candidate nodes are generated by random sampling. The sampled nodes that meet the conditions will undergo collision checks, which are performed using UFOMap's ray tracing operations and then added to the topological map. The weight of the edge is set to the distance between the two nodes. All nodes are stored in a KD tree structure to improve computational efficiency.

[0075] (3) Viewpoint generation: For each frontier point, find the node closest to the frontier in the topology map and use it as the viewpoint. The purpose of viewpoint generation is to find a location near the frontier point that is suitable for environmental perception for subsequent exploration and coverage.

[0076] Step S3: sub-area division, dividing the entire exploration environment into several sub-areas. The specific operations are as follows:

[0077] (1) Recursive sub-region hierarchical division: In the initial stage of exploration, the entire environment to be explored is divided into several sub-region units along the axis-aligned boundary. The initial level of each unit is l i = 1, these sub-areas will be used as the basic units for subsequent exploration and planning tasks. Each sub-area unit can be recursively divided through a quadtree structure. Initially, the sub-area unit is large, but as the exploration progresses, the volume of the unknown area decreases and the original sub-areas are further subdivided. i The sub-area of ​​the level will become l i+1 Level, each sub-area unit is divided into smaller sub-areas in the process of increasing levels, so as to capture the dynamically changing environment during the exploration process, such as Figure 3 and Figure 4 shown.

[0078] (2) Sub-region information structure: Each sub-region unit contains its level information l i , axis-aligned bounding box range, known area volume ratio R known , all viewpoints VP in the sub-region, the centroid C of all viewpoints avg , the distance between each sub-area is L cost This information is used to dynamically update the status of the sub-area and provide data support for subsequent exploration planning.

[0079] (3) Sub-area online update: During the exploration process, the environment map is continuously updated. It is necessary to recalculate the sub-area units that intersect with the map update area and update their information structure. If the volume proportion of the known area in the sub-area exceeds a certain threshold and has not reached the maximum level l max , it will be further divided into smaller sub-area units. At the same time, the distance length between sub-area units is updated to ensure the effectiveness and accuracy of exploration planning. max The calculation rules are as follows:

[0080]

[0081] Among them, W max and W min Indicates the maximum and minimum side lengths allowed for the sub-region, Indicates a round-up operation.

[0082] Step S4: Generate a global coverage path to obtain the best order for entering each sub-area. The specific operations are as follows:

[0083] The global coverage path problem is transformed into an asymmetric traveling salesman problem. By constructing the distance cost matrix M between each sub-region cost , using an operations research optimization solver such as OR-Tools, we can calculate a global path that enters all sub-areas in sequence and has the shortest distance, such as Figure 5 As shown by the arrow segment in the middle. Only those sub-regions whose internal viewpoints are greater than 0 will be included in the global coverage path solution process. The distance cost matrix M cost The calculations for each element are as follows:

[0084] M cost (i,j)=L cost,i,j ,i,j∈{1,2,…,N act}

[0085] Where L cost,i,j Represents the distance cost between sub-area i and sub-area j. This part of the cost has been calculated in the sub-area online update process in step S3, so it can be directly filled in. N act Indicates the total number of sub-regions whose internal viewpoints are greater than 0.

[0086]

[0087] in, It represents the distance cost from the current position p0 of the unmanned platform to the sub-area k, where p0 is obtained through the pose estimation output by the SLAM algorithm in step S1.

[0088] Finally, we need to transform the standard traveling salesman problem into an asymmetric traveling salesman problem to optimize the solution speed:

[0089] M cost (k,0)=0,k∈{1,2,…,N act}

[0090] The path length cost in the above process is calculated by the sparse topological graph constructed in step S2. Specifically, the A* search algorithm is used to search for a collision-free path connecting the search starting point and the target point in the topological road graph, and the distance length cost between the two points can be obtained by calculating the length of the path.

[0091] The constructed cost matrix M cost Input into OR-Tools for optimization and solution. The output of the solver is the index sequence of entering these sub-regions in turn, denoted as G * =[S1,S2,…,S act ],S i represents the index of the sub-region. Therefore, G * That is, the shortest path that covers all sub-areas in sequence.

[0092] Step S5: local exploration path optimization, that is, optimizing the access order of each viewpoint in a sub-region within a certain range. The specific operation is as follows:

[0093] According to the entry order G of the sub-areas obtained in step S4 * , from G * Select the distance robot radius r max N within near sub-regions, and obtain the viewpoints of these sub-regions:

[0094] Recorded as a total of N v viewpoints, among which represents the jth viewpoint inside the i-th sub-region. Similar to step S5, search and calculate the viewpoint v in the sparse topological road map m and viewpoint v n The distance cost between cost,m,n And the current position of the unmanned platform p0 reaches the viewpoint v l The distance cost

[0095] M cost (m,n)=D cost,m,n ,m,n∈{1,2,…N V}

[0096]

[0097] And for M cost All elements in the first row of are filled with 0 to construct the asymmetric traveling salesman problem:

[0098] M cost (l,0)=0,l∈{1,2,…N V}

[0099] The cost matrix M cost The optimization solution is sent to OR-Tools. Different from step S5, the global coverage path G * The optimization problem adds a constraint condition, namely, the viewpoint Should be compared to the viewpoint This is because the global coverage path G obtained by solving * In , the i-th sub-region is visited before the i+1-th sub-region. This constraint can be set in OR-Tools, so the viewpoint access order obtained after solving will satisfy the above constraint and ensure the overall shortest access path length, such as Figure 5 Shown as the dashed line segment.

[0100] Step S6: Motion planning part, prompting the robot to explore the target point. The specific operations are as follows:

[0101] According to the viewpoint access sequence obtained in step S5, the viewpoint that should be visited at the current time t is selected as the target point, and the target point is sent to the path planning module. This module is responsible for planning a path from the robot to the target point and effectively avoiding obstacles during driving. The specific operation includes two parts: global path planning and local path planning: global path planning uses the A* algorithm, combined with positioning information and sparse topological map search to obtain the overall path. Local path planning uses point cloud data to obtain obstacle location information, and applies the DWA (dynamic window) algorithm to achieve local obstacle avoidance during the robot's driving process to ensure that the robot can reach the target area safely and smoothly.

[0102] Step S7: Check whether there is a viewpoint in the current environment. If there is no viewpoint, end the exploration. Otherwise, repeat the above steps until there is no viewpoint.

[0103] Figure 6 The figure shows the experimental results in an underground garage as a real scene. The size of the garage scene is 50m×50m, including parked vehicles and the narrow area formed by the vehicles. During the experiment, the maximum speed of the robot is limited to 1m / s. Figure 6 The exploration coverage space represented by the 3D point cloud is shown. The curve in the figure is the robot's driving trajectory, and the dot indicates the starting point of the trajectory. During the entire exploration process, there is no redundant path. The robot explored 5673.25m in 175.53 seconds. 3 , moving 157.53m. This method completely explored the entire garage without leaving any area behind, achieving effective exploration of unknown environments.

[0104] Embodiment 2:

[0105] This embodiment provides a terminal device, which includes a processor and a memory, wherein the memory is used to store a computer program, the computer program includes program instructions, and the processor is used to execute the program instructions stored in the computer storage medium. The processor may be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components, etc. It is the computing core and control core of the terminal, which is suitable for implementing one or more instructions, and is specifically suitable for loading and executing one or more instructions to implement the corresponding method flow or corresponding function; the processor described in the embodiment of the present invention can be used for the operation of a three-dimensional environment coverage method for online sub-area update and hierarchical planning, including the following steps:

[0106] Step S1: Real-time positioning and mapping: The laser radar and inertial measurement unit on the ground unmanned platform are used to scan and map the current environment using the SLAM (simultaneous localization and mapping) algorithm, while obtaining the position information of the unmanned platform, and using the UFOMap mapping algorithm to generate a three-dimensional voxel map of the current environment;

[0107] Step S2: Rapid environment preprocessing: Rapid preprocessing of the currently mapped environment area, mainly including incremental frontier point extraction, sparse topology road map construction and viewpoint generation;

[0108] Step S3: Sub-area division: The entire unknown environment to be covered is initially divided into several sub-area units. Each sub-area unit stores the basic environmental information of the unit, namely the sub-area information structure (SIS), which is used for subsequent exploration planning. Each sub-area unit has a hierarchical structure and is dynamically decomposed according to the volume ratio of the unknown area in the unit. At the same time, the sub-area information structure will be continuously updated to adapt to the dynamic changes of the unknown area;

[0109] Step S4: Global coverage path generation: Based on the divided sub-areas, for the sub-areas with viewpoints inside, arrange the entry order of these sub-areas by solving the asymmetric traveling salesman problem to determine the access priority of each sub-area at time t;

[0110] Step S5: Local exploration path optimization: Under the guidance of the global coverage path, the access order of each viewpoint in the sub-area within a certain range from the unmanned platform is optimized by solving the asymmetric traveling salesman problem again;

[0111] Step S6: motion planning: according to the order of viewpoint visits, the next viewpoint to be visited is sent to the motion planning module, prompting the robot to explore the target;

[0112] Step S7: Check whether there are any unvisited viewpoints in the map. If there are no viewpoints, end the exploration, otherwise repeat the above steps.

[0113] Embodiment 3:

[0114] This embodiment provides a storage medium, specifically a computer-readable storage medium (Memory), which is a memory device in a terminal device for storing programs and data. It is understandable that the computer-readable storage medium here can include both the built-in storage medium in the terminal device and the extended storage medium supported by the terminal device. The computer-readable storage medium provides a storage space, which stores the operating system of the terminal. In addition, one or more instructions suitable for being loaded and executed by the processor are also stored in the storage space, and these instructions can be one or more computer programs (including program codes). It should be noted that the computer-readable storage medium here can be a high-speed RAM memory or a non-volatile memory, such as at least one disk memory.

[0115] The processor may load and execute one or more instructions stored in a computer-readable storage medium to implement the corresponding steps of the above-mentioned embodiment related to a three-dimensional environment coverage method for online sub-region updating and hierarchical planning; the processor may load and execute the following steps of one or more instructions in the computer-readable storage medium:

[0116] Step S1: Real-time positioning and mapping: The laser radar and inertial measurement unit on the ground unmanned platform are used to scan and map the current environment using the SLAM (simultaneous localization and mapping) algorithm, while obtaining the position information of the unmanned platform, and using the UFOMap mapping algorithm to generate a three-dimensional voxel map of the current environment;

[0117] Step S2: Rapid environment preprocessing: Rapid preprocessing of the currently mapped environment area, mainly including incremental frontier point extraction, sparse topology road map construction and viewpoint generation;

[0118] Step S3: Sub-area division: The entire unknown environment to be covered is initially divided into several sub-area units. Each sub-area unit stores the basic environmental information of the unit, namely the sub-area information structure (SIS), which is used for subsequent exploration planning. Each sub-area unit has a hierarchical structure and is dynamically decomposed according to the volume ratio of the unknown area in the unit. At the same time, the sub-area information structure will be continuously updated to adapt to the dynamic changes of the unknown area;

[0119] Step S4: Global coverage path generation: Based on the divided sub-areas, for the sub-areas with viewpoints inside, arrange the entry order of these sub-areas by solving the asymmetric traveling salesman problem to determine the access priority of each sub-area at time t;

[0120] Step S5: Local exploration path optimization: Under the guidance of the global coverage path, the access order of each viewpoint in the sub-area within a certain range from the unmanned platform is optimized by solving the asymmetric traveling salesman problem again;

[0121] Step S6: motion planning: according to the order of viewpoint visits, the next viewpoint to be visited is sent to the motion planning module, prompting the robot to explore the target;

[0122] Step S7: Check whether there are any unvisited viewpoints in the map. If there are no viewpoints, end the exploration, otherwise repeat the above steps.

[0123] Those skilled in the art will appreciate that embodiments of the present invention may be provided as methods, systems, or computer program products. Therefore, the present invention may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0124] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0125] These computer program instructions may also be stored in a computer readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer readable memory produce an article of manufacture including an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0126] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process in the computer or other programmable device. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

[0127] The preferred embodiments of the present invention are described in detail above, but the present invention is not limited to the above embodiments, and various changes can be made within the knowledge scope of ordinary technicians in this field without departing from the purpose of the present invention.

[0128] Many other changes and modifications may be made without departing from the concept and scope of the present invention.It should be understood that the present invention is not limited to the specific embodiments, and the scope of the present invention is defined by the appended claims.

Claims

1. A three-dimensional environment coverage method with online sub-region updating and hierarchical planning, characterized in that: The method comprises: Based on the obtained valid information of the current environment, the entire unknown environment to be covered is initially divided into multiple sub-area units; Based on the divided sub-area units, for the sub-areas with viewpoints inside, the entry order into each sub-area is arranged by solving the asymmetric traveling salesman problem; Under the guidance of the global coverage path, the access order of each viewpoint in the sub-area within a certain range from the unmanned platform is optimized by solving the asymmetric traveling salesman problem; According to the access order of each viewpoint, the next viewpoint to be visited is sent to the motion planning module to guide the robot to the exploration target; Check whether there are any unvisited viewpoints in the map. If there are no viewpoints, end the exploration, otherwise repeat the above steps.

2. A three-dimensional environment coverage method with online sub-region updating and hierarchical planning according to claim 1, characterized in that: The laser radar and inertial measurement unit on the ground unmanned platform are used to scan and map the current environment using the SLAM algorithm, obtain the position information of the unmanned platform, and use the UFOMap mapping algorithm to generate a three-dimensional voxel map of the current environment; Based on generating a 3D voxel map of the current environment, the currently mapped environment area is quickly preprocessed, including incremental frontier point extraction, sparse topology road map construction and viewpoint generation, so as to obtain effective information of the current environment.

3. A three-dimensional environment coverage method with online sub-region updating and hierarchical planning according to claim 2, characterized in that: The input of the SLAM algorithm is the point cloud data at the current moment and the vehicle body acceleration, angular velocity and angular acceleration information measured by the inertial measurement unit, and the output is the robot's posture estimation and point cloud map; the input of the UFOMap algorithm is the posture estimation and the point cloud data at the current moment, and the output is a three-dimensional voxel map of the explored part of the environment.

4. A three-dimensional environment coverage method with online sub-region updating and hierarchical planning according to claim 2, characterized in that: The rapid preprocessing of the currently mapped environment area specifically includes: Incremental extraction of frontier points: Get all the voxel sets with changed occupancy values ​​from UFOMap, check the state of each voxel, and if a voxel is in a free state and its adjacent voxels are in an unknown state, mark the voxel as a frontier voxel. At the same time, during the exploration process, check whether each frontier voxel in the map update area is still valid, and remove invalid frontier voxels. Construct a sparse topological road map: The nodes in the topological map represent key points in the environment, and the edges represent paths connecting adjacent nodes. During the exploration process, candidate nodes are generated by random sampling. The sampled nodes that meet the conditions will undergo collision checks, which are performed using UFOMap's ray tracing operations and then added to the topological map. The weight of the edge is set to the distance between the two nodes, and all nodes are stored in a KD tree structure to improve computational efficiency. Viewpoint generation: For each frontier point, find the node closest to the frontier in the topological graph and use this node as the viewpoint.

5. The three-dimensional environment coverage method with online sub-region updating and hierarchical planning according to claim 1, characterized in that: The initial division of the entire unknown environment to be covered specifically includes: Sub-area hierarchical recursive division: In the initial stage of exploration, the entire environment to be explored is divided into multiple sub-area units along the axis-aligned boundary, and the initial level of each unit is l i = 1, the sub-region will be used as the basic unit for subsequent exploration and planning tasks; each sub-region unit can be recursively divided through a quadtree structure. Initially, the sub-region unit is large, but as the exploration proceeds, the volume of the unknown area decreases, and the original sub-region is further subdivided. i The sub-area of ​​the level will become l i+1 Level, each sub-area unit is divided into smaller sub-areas in the process of increasing levels, so as to capture the dynamically changing environment during the exploration process; Sub-region information structure: Each sub-region unit contains its level information l i , axis-aligned bounding box range, known area volume ratio R known , all viewpoints VP in the sub-region, the centroid C of all viewpoints avg , the distance between each sub-area is L cost Key information, used to dynamically update the status of the sub-area; Sub-area online update: During the exploration process, the environment map is constantly updated, and the sub-area units intersecting with the map update area are recalculated and their information structure is updated. If the volume proportion of the known area in the sub-area exceeds a certain threshold and has not reached the maximum level l max , will be further divided into smaller sub-area units, and at the same time, the distance length between sub-area units is updated, l max The calculation rules are as follows: Among them, W max and W min Indicates the maximum and minimum side lengths allowed for the sub-region, Indicates a round-up operation.

6. A three-dimensional environment coverage method with online sub-region updating and hierarchical planning according to claim 1, characterized in that: By solving the asymmetric traveling salesman problem, the order of entering each sub-area is arranged, including: The global coverage path problem is transformed into an asymmetric traveling salesman problem by constructing the distance cost matrix M between each sub-region. cost , using OR-Tools and other operations optimization solvers, we can calculate a global path that enters all sub-regions in sequence and has the shortest distance. Only sub-regions with more than 0 internal viewpoints will be included in the global coverage path solution process. The distance cost matrix M cost The calculations for each element are as follows: M cost (i,j)=L cost,i,j ,i,j∈{1,2,…,N act } Where L cost,i,j represents the distance cost between sub-area i and sub-area j. This part of the cost has been calculated in the sub-area online update process in step S3, so it can be directly filled in. N act Indicates the total number of sub-regions whose internal viewpoints are greater than 0; in, represents the distance cost from the current position p0 of the unmanned platform to the sub-area k, where p0 is obtained by estimating the position and posture output by the SLAM algorithm; Finally, we need to transform the standard traveling salesman problem into an asymmetric traveling salesman problem to optimize the solution speed: M cost (k,0)=0,k∈{1,2,…,N act } The path length cost in the above process is calculated in the constructed sparse topology graph; specifically, an A* search algorithm is used to search in the topology graph to obtain a collision-free path connecting the search starting point and the target point, and the distance length cost between the two points can be obtained by calculating the length of the path; The constructed cost matrix M cost Input into OR-Tools for optimization and solution. The output of the solver is the index sequence of entering these sub-regions in turn, denoted as G * =[S1,S2,…,S act ],S i represents the index of the sub-region, so G * That is, the shortest path that covers all sub-areas in sequence.

7. A three-dimensional environment coverage method with online sub-region updating and hierarchical planning according to claim 1, characterized in that: By solving the asymmetric traveling salesman problem, the order of visiting each viewpoint in the sub-area within a certain range from the unmanned platform is optimized, including: According to the obtained entry order of the sub-areas G * , from G * Select the distance robot radius r max N within near sub-regions, and obtain the viewpoints of these sub-regions: Recorded as a total of N v viewpoints, among which Represents the jth viewpoint inside the i-th sub-region, searches and calculates the viewpoint v in the sparse topological road graph m and viewpoint v n The distance cost between cost,m,n And the current position of the unmanned platform p0 reaches the viewpoint v l The distance cost M cost (m,n)=D cost,m,n ,m,n∈{1,2,…N V } And for M cost All elements in the first row of are filled with 0 to construct the asymmetric traveling salesman problem: M cost (l,0)=0,l∈{1,2,…N V } The cost matrix M cost Sent to OR-Tools for optimization and solution, subject to the global coverage path G * The optimization problem adds constraints, namely, the viewpoint Should be compared to the viewpoint The first viewpoint is visited. This constraint can be set in OR-Tools, so the order of viewpoint visits obtained after solving will satisfy the above constraints and ensure the overall shortest access path length.

8. The three-dimensional environment coverage method with online sub-region updating and hierarchical planning according to claim 1, characterized in that: Send the next viewpoint to be visited to the motion planning module to guide the robot to the exploration target, including: According to the obtained viewpoint access sequence, the viewpoint that should be visited at the current time t is selected as the target point, and the target point is sent to the path planning module, which is responsible for planning a path from the robot to the target point and effectively avoiding obstacles during driving; The specific operation includes global path planning and local path planning. The global path planning uses the A* algorithm, combined with positioning information and sparse topology map search to obtain the overall path. The local path planning uses point cloud data to obtain obstacle location information, and applies the DWA dynamic window algorithm to achieve local obstacle avoidance during the robot's driving process, so as to ensure that the robot can reach the target area safely and smoothly.

9. A computer device, characterized in that: The invention comprises a memory, a processor and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, a three-dimensional environment coverage method with online sub-region updating and hierarchical planning as described in any one of claims 1 to 8 is implemented.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, which, when executed by a processor, implements a three-dimensional environment coverage method with online sub-region updating and hierarchical planning according to any one of claims 1 to 8.

Images