Multi-unmanned vehicle autonomous exploration method and system for real large-scale complex scene

Abstract

The invention discloses a multi-unmanned-vehicle autonomous exploration method and system for a real large-scale complex scene, and the method comprises the steps: carrying out the voxelization of a three-dimensional environment, and carrying out the detection of a leading edge point; constructing an initialized undirected passable topological graph for sharing of multiple vehicles and calculation of path distances from the multiple vehicles to a target point; obtaining the pose of the radar relative to the world coordinate system and the pose of the robot relative to the world coordinate system by using observation data of the radar and the IMU; ground radar data extracted through ground segmentation is combined with obstacle point cloud data to guarantee that the robot operates in a complex environment. A brand-new distributed task allocation method for multiple unmanned vehicles is provided, and the dispersion rate and the exploration efficiency between robots are considered; and obtaining a reachable path from the robot to the target point based on the cost function distribution. The problem that in the field of multi-unmanned-vehicle exploration, multi-unmanned-vehicle exploration based on 3D laser radar is mostly applied to a simulation environment and is difficult to deploy to a mobile robot to explore a real environment is solved.

Description

Technical Field

[0001] The present invention relates to the field of robotics, and in particular to a method and system for autonomous exploration of multiple unmanned vehicles in real large-scale complex scenarios. Background Art

[0002] Autonomous exploration is a key capability for modern robots to perform complex tasks in dangerous environments, such as search and rescue, patrol surveillance, and planetary exploration. It requires robots to perform online path planning in unknown environments and build environmental maps based on sensor observation data. At present, autonomous exploration methods for multiple unmanned vehicles have been widely studied, and these methods can be divided into two categories:

[0003] 1. Centralized multi-unmanned vehicle collaborative methods are characterized by a central hub or ground station for global multi-unmanned vehicle path planning, and then sharing the planning results with all robots. Centralized collaborative methods reduce the overlap of explored areas by reducing the information gain of another robot assigned to a different frontier point near the current robot, or adopt a sampling-based method, such as using RRT (rapid random tree) to find and assign frontiers. Recently, a typical example of centralized collaborative methods is to solve the multi-unmanned vehicle multi-objective allocation problem as a multi-traveling salesman problem (mTSP) or a minimum-maximum multi-depot vehicle routing problem (MDVRP), thereby assigning a set of areas to be explored to the robot group. Although this method has the potential to find a global optimal solution, its computational complexity grows exponentially with the increase in the number of robots. At the same time, centralized methods rely on a central node for global coordination and decision-making, which means that if the central node fails, the performance of the entire system may be severely affected, and may even cause the system to fail completely.

[0004] 2. Distributed multi-unmanned vehicle collaboration method allows each robot to plan independently and ensures team coordination by allowing the robot to share local information with other nearby robots. The exploration algorithm based solely on the frontier emphasizes that the robot should move to the nearest unknown area boundary and gradually expand the known area. However, this method may lead to uneven resource allocation. Multiple robots may be concentrated in the same area and ignore other areas to be explored, resulting in a decrease in overall exploration efficiency. Distributed greedy algorithms are also commonly used for task allocation, but they only consider distance relationships and cannot fully disperse robots, resulting in reduced robot exploration efficiency. In the distributed nearest distance method, each robot evaluates its ranking of travel distances to each frontier relative to other robots. According to this criterion, the robot is assigned to its lowest-ranked frontier, which can ensure that all robots are evenly distributed in the environment. However, this method does not consider the exploration efficiency of a single robot, resulting in low overall exploration efficiency of multiple machines. Summary of the invention

[0005] In view of the shortcomings of the above-mentioned prior art, the present invention proposes a multi-unmanned vehicle autonomous exploration method and system for real large-scale complex scenes, which comprehensively considers the dispersion rate between robots and the exploration efficiency of robots, can effectively reduce the exploration failure rate and improve the integrity of exploration, achieve greater team dispersion and higher individual exploration efficiency, and greatly improve the efficiency of multi-unmanned vehicle exploration.

[0006] The objective of the present invention is achieved through the following technical solution: a multi-unmanned vehicle autonomous exploration method for real large-scale complex scenes, comprising the following steps:

[0007] S1: Establish a world coordinate system, voxelize the three-dimensional environment to be explored by the unmanned vehicle, and construct an initial UFOMap map. The UFOMap map is an octree structure. At the same time, an initialized undirected traversable topology map is constructed.

[0008] S2: Read the observation data of the radar and IMU, thereby obtaining the position and posture of the radar coordinate system relative to the world coordinate system and the position and posture of the robot coordinate system relative to the world coordinate system in real time, as well as the radar point cloud data aligned to the world coordinate system.

[0009] S3: Obtain the radar observation data obtained at the current observation position, process it to obtain the ground radar data and the radar point cloud data of the obstacle, and convert it to the world coordinate system. Merge the ground radar data with the obstacle radar data to obtain the registered radar point cloud data within a certain period of time within the local range of the unmanned vehicle.

[0010] S4: Based on the radar point cloud data within a period of time in the local range of the unmanned vehicle obtained in step S3, a two-dimensional grid map is constructed, the radar point cloud data is projected onto the two-dimensional grid map, the number of radar points in each grid and the height information of the radar points are recorded, and the passability of each grid is generated by comparing with a predetermined threshold, thereby constructing a passable two-dimensional grid map.

[0011] S5: Fuse the radar pose and radar point cloud data obtained in step S2 into the existing three-dimensional voxel UFOMap map, and update the occupancy status of the voxels in the UFOMap map.

[0012] S6: Based on the traversable two-dimensional grid map obtained in step S4, sample the traversable points at the current position of the unmanned vehicle and merge them into the traversable topology.

[0013] S7: Based on the UFOMap obtained in step S5 and the traversable topology map obtained in step S6, frontier point extraction and viewpoint extraction in the environment and information gain calculation of the viewpoints are performed.

[0014] S8: Information sharing: sharing the robot’s current position, viewpoint, traversable topology map, and constructed UFOMap, and accepting information shared by other unmanned vehicles.

[0015] S9: Based on the information of other unmanned vehicles obtained in step S8, the local viewpoint state is updated, the traversable topological maps of multiple unmanned vehicles are merged, and then the tasks of multiple unmanned vehicles are allocated to obtain the target point that the current unmanned vehicle is going to.

[0016] S10: Based on the target point obtained in step S9 and the two-dimensional passable grid map obtained in step S4, the A* algorithm is used to perform path planning to obtain a distance-optimal path.

[0017] S11: The unmanned vehicle performs autonomous movement based on the path obtained in step S10, continuously senses the surrounding environment during the movement, and repeats steps S2-S11 until there is no unexplored area in the current environment, thus completing the exploration task.

[0018] Furthermore, the method for constructing the initial UFOMap in step S1 is specifically as follows:

[0019] The UFOMap map is essentially an occupancy octree map. The position of the unmanned vehicle when starting the algorithm is used as the starting point of the world coordinate system, the coordinate system of the UFOMap is set to be the same as the world coordinate system, and the occupancy state of each child node in the UFOMap map is initialized to an unknown state, that is, the occupancy probability is 0.5.

[0020] Furthermore, the method for constructing the initial undirected traversable topological graph in step S1 is:

[0021] Construct an undirected traversable topological graph G m = {V m , E m},in, represents the node of the graph, which is composed of the traversable points in the robot's current environment. In order to ensure the sparsity of the topological graph, the traversable points are obtained by uniform sampling. m ={w ij} represents the edge of the graph, w ij is the connection weight between any two nodes in the graph. The root node of the undirected topological graph is the current starting position of the robot.

[0022] Furthermore, in step S2, the method of acquiring the position and posture of the radar coordinate system relative to the world coordinate system and the position and posture of the robot coordinate system relative to the world coordinate system in real time, and registering the radar point cloud data to the world coordinate system is:

[0023] First, the robot receives radar point cloud data, and after a period of accumulation, it extracts features from the accumulated radar data to extract surface points and corner points. The method for extracting corner points and surface points is:

[0024] Calculate the curvature c at each point using the formula:

[0025]

[0026] Among them, S is the i-th point in the k-th frame point cloud data A set of consecutive points near is a point in S, and |S| represents the number of points in S. After calculating all the points in the point cloud of the current frame, all points are sorted according to the curvature value, and then the points with large curvature are selected as corner points, and the points with small curvature are selected as surface points.

[0027] Then, the IMU data is received for forward propagation, that is, the integration is used to obtain a rough estimate of the pose, and then back propagation is performed for motion compensation. The forward propagation derives the pose change formula through acceleration a and angular velocity ω as follows:

[0028]

[0029] in, It represents the posture transformation of the robot relative to the world coordinate system, including translation and rotation matrices. The function f(a, w) represents the process of updating the posture through IMU data.

[0030] The error caused by the back-propagation compensation motion is corrected by combining the current posture with the posture obtained by IMU integration to obtain the corrected posture error T c , the motion compensation formula is:

[0031]

[0032] Among them, T I is a rough estimate of the pose, that is, obtained during the forward propagation process T e is the estimated error pose.

[0033] Finally, the residual of the radar odometer is calculated, and the pose transformation is estimated using iterative Kalman filtering until convergence, and the pose of the radar relative to the world coordinate system is obtained. Then, the radar point cloud data is aligned to the world coordinate system based on the radar pose.

[0034] Furthermore, the method for obtaining radar point cloud data of ground points in step S3 is as follows:

[0035] First, the received raw radar point cloud data is filtered to remove the reflected noise. Then, a height-based filtering method is used to consider points below a certain height as ground points.

[0036] Finally, these ground radar point data are aligned to the world coordinate system, and the alignment formula is:

[0037]

[0038] Among them, P M is the radar point cloud data after registration, is the coordinate transformation matrix from the radar coordinate system to the world coordinate system, P L It is the current ground radar point cloud data.

[0039] Furthermore, in step S6, the radar point cloud data is integrated into the existing three-dimensional voxel UFOMap map, and the method for updating the occupancy status of the voxels in the UFOMap map is:

[0040] Integrate the point cloud data read by the radar into the UFOMap. 1：t , update the occupancy probability P(n|z 1：t ), use the following formula to update:

[0041]

[0042] Among them, the observation data of the sensor is z t , the prior occupancy probability of the node is P(n), and the node occupancy probability before the current observation is estimated to be P(n|z 1：t-1 ). P(n|z t ) represents a given measurement z t The occupancy probability of voxel n is obtained. Prior probability P(n) = 0.5. Convert the above formula into the following logarithmic symbol representation:

[0043] L(n|z 1：t )=L(n|z 1:t-1 )+L(n|z t ) (6)

[0044]

[0045] The logarithmic notation allows for faster updates because multiplications are replaced by additions. Logarithmic values ​​and probability values ​​can be converted to each other, and the logarithmic values ​​are stored for each voxel in the UFOMap grid.

[0046] The states of nodes in the UFOMap grid graph can be unknown, occupied, and free. They are updated according to the probability values ​​of the nodes. The update formula is as follows:

[0047]

[0048] Among them, occ(n) represents the occupancy probability value of node n, t f , t o The threshold is set manually.

[0049] Furthermore, the method of extracting frontier points in the environment, extracting viewpoints and calculating information gain of viewpoints in step S7 based on the UFOMap map is:

[0050] During the operation of the unmanned vehicle, the voxels in the UFOMap whose occupied state is free and adjacent to at least one voxel whose state is unknown at the same height are extracted as frontier points. In order to improve the detection efficiency of frontier points, only the newly updated voxel grid recorded in the UFOMap and the existing frontier points around the robot are detected.

[0051] During the operation of the unmanned vehicle, the candidate viewpoints are uniformly sampled around the robot in the free space where the unmanned vehicle can pass. The number of visible frontier points of each viewpoint is evaluated through visibility check, and the viewpoints that can observe multiple frontier points are retained. The number of frontier point units that can be observed by a viewpoint is used as the information gain measure of the viewpoint.

[0052] Furthermore, the method for updating the local unmanned vehicle viewpoint state after obtaining the information of other unmanned vehicles in step S9 is:

[0053] Define the global coordinate system as M and the body coordinate system of robot i as R i , the map coordinate system of robot i is M i , where i∈{0, 1, ..., n-1}, n is the number of robots. The coordinate transformation matrix from the robot i body coordinate system to the robot i map coordinate system can be obtained by the state estimation method. The coordinate transformation matrix from the map coordinate system of robot i to the global coordinate system is set by the preset method when the program is running are all the frontier points detected and extracted by robot i, where the viewpoint The reference coordinate system is M i .f i is one of the frontier points detected by robot i, and is transformed into the map coordinate system of robot j using the following formula:

[0054]

[0055] Among them, f j To f i Transform to the corresponding voxel point in the robot j map coordinate system. If at this time f j In the UFOMap of robot j, it is an unknown voxel. iis the frontier point in robot i, otherwise update f i Non-frontier point.

[0056] Finally, the number of frontier point units that can be observed by a viewpoint is used as the information gain measure of the viewpoint. v The calculation formula is:

[0057] G v =K×n v (10)

[0058] Among them, n v is the number of frontier points observed by the viewpoint, and K is the gain multiplier.

[0059] Furthermore, the method for merging the traversable topological graphs of multiple unmanned vehicles in step S9 is:

[0060] The premise of merging the traversable topology graphs of multiple unmanned vehicles is that there are already two or more robots in operation and have generated their own traversable topology graphs. It is known that the minimum spacing of the traversable topology graphs constructed by multiple unmanned vehicles is r. When the node v of the traversable topology graph of robot a is a The distance to any node in robot b is greater than When a There is no overlapping area. When the node v of the traversable topology graph of robot a a When the distance to any node in robot b is less than or equal to 0.2·r, it is considered that at node v a There is an overlapping area. When the node v of the traversable topology graph of robot a a The distance from any node in robot b is When the node v is between a The values ​​of α, β, and γ at:

[0061]

[0062] Among them, v a =(x P ,y P , z P ), (x A ,y A , z A ), (x B ,y B , z B ), (x C ,y C , z C ) is the distance v a The coordinates of the three nodes in the nearest robot b. After obtaining the values ​​of α, β, and γ, the node status is determined by the following formula:

[0063]

[0064] Among them, state(v a ) represents node v a After obtaining the set of overlapping points, the two traversable topological graphs are merged at the overlapping point positions.

[0065] Furthermore, the method for allocating tasks among multiple unmanned vehicles in step S9 is as follows:

[0066] First, the problem of a single robot exploring a bunch of viewpoints is abstracted into a single traveling salesman problem. By solving this problem, the optimal path sequence for the robot to traverse all viewpoints is obtained to improve the efficiency of the robot's exploration of viewpoints. In this process, all viewpoints V observed by the robot are considered, and finally a set of path points is generated. The formula is as follows:

[0067]

[0068] Where V = [V1, V2, ..., V n ] is the sequence of all viewpoints, and V0 is the current robot pose. k ) is the viewpoint V k The utility of G(V k ) is the viewpoint V k The information gain, E(V k )=exp(-C·L(V0,V k )) is the viewpoint V k The penalty factor is used to prevent the robot from cyclical motion. C is the penalty factor, L(V0, V k ) is the robot's path from V0 to V along the path obtained from the traversable topology k The cumulative moving distance. D(V q-1 , V q ) is the viewpoint V q-1 and V q The distance between them, v0 is the current movement speed of the robot. Then select the next viewpoint to be explored by the robot from the obtained path point set, denoted as V p .

[0069] Then, the distributed multi-robot task allocation method is used to allocate the viewpoint of the current robot. The formula is as follows:

[0070]

[0071] Among them, V is all the viewpoints observed by the robot, R represents the current position of all robots, and c ijrepresents the actual path distance from robot i to viewpoint j, which is calculated by the A* algorithm. Finally, the label j of the viewpoint that the robot wants to go to is obtained, and the viewpoint is recorded as V m .

[0072] Then, the dispersion between robots is calculated by the following formula, and the movement of robots is controlled according to the dispersion to ensure that multiple unmanned vehicles can be dispersed as much as possible when exploring large-scale unknown scenes:

[0073]

[0074] Where σ is the dispersion among robots and n represents the number of robots. i , P j (i, j<n) represents the positions of robots i and j respectively.

[0075] Finally, set σ u , σ l are the upper and lower thresholds of the dispersion, respectively. In order to avoid frequent switching of the algorithm, the following formula is used to select the final target point:

[0076]

[0077] Among them, S represents the single-vehicle viewpoint allocation mode, M represents the multi-vehicle target allocation mode, and S t-1 Indicates the viewpoint allocation mode of the previous algorithm. The final target point V T The destination point that the unmanned vehicle wants to go to.

[0078] The second aspect of the present invention relates to an autonomous exploration system for multiple unmanned vehicles for real large-scale and complex scenes, including a memory and one or more processors, wherein the memory stores executable code, and when the one or more processors execute the executable code, they are used to implement the autonomous exploration method for multiple unmanned vehicles for real large-scale and complex scenes of the present invention.

[0079] The present invention also relates to a computer-readable storage medium on which a program is stored. When the program is executed by a processor, the present invention implements the multi-unmanned vehicle autonomous exploration method for real large-scale complex scenes.

[0080] The beneficial effects of the present invention are:

[0081] 1. Efficient environment modeling: By establishing the UFOMap octree structure, the environment is voxelized, making the expression of the three-dimensional environment more accurate. At the same time, the map gives each voxel three states: free, occupied, and unknown, making the frontier point detection in the exploration process faster and more accurate.

[0082] 2. Accurate sensor data fusion: By using radar and IMU data registration, the position and posture of the unmanned vehicle and point cloud data in the environment can be obtained in real time, which effectively improves the accuracy of environmental perception and ensures the navigation and planning capabilities of the unmanned vehicle.

[0083] 3. Judgment of local environment passability: The radar point cloud data is segmented on the ground and then fused with obstacle information, converted into a two-dimensional grid map. Combined with the number and height information of radar points in the grid, a passable two-dimensional grid map within the local range of the unmanned vehicle is generated, ensuring the movement safety of the unmanned vehicle in complex environments.

[0084] 4. Collaboration among multiple unmanned vehicles: Through information sharing, multiple unmanned vehicles can share information such as position, viewpoint, and traversable topology to achieve collaborative exploration and task allocation. At the same time, the dispersion between robots and the exploration efficiency of a single robot are combined to propose a new method for multi-robot collaborative exploration task allocation, which improves the overall efficiency of the exploration task.

[0085] 5. Path planning optimization: Path planning based on the A* algorithm can generate the optimal path according to the current position of the unmanned vehicle and the target point, ensuring that the unmanned vehicle reaches the target in the shortest distance, thereby improving the efficiency and accuracy of path planning.

[0086] In general, this invention has significant improvements in environmental modeling, multi-vehicle collaboration, path planning, etc., and can ensure that multiple unmanned vehicles can complete exploration tasks efficiently and safely in unknown and complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0087] Figure 1 This is an architecture diagram of autonomous exploration of multiple unmanned vehicles for real large-scale complex scenarios in an embodiment of the present invention;

[0088] Figure 2 A schematic diagram of a grid map used for representing a three-dimensional environment in an embodiment of the present invention;

[0089] Figure 3 This is a graph showing the change in the exploration area of ​​one unmanned vehicle over time during the autonomous exploration of multiple unmanned vehicles in an embodiment of the present invention.

[0090] Figure 4 This is a graph showing the change in exploration area over time when two unmanned vehicles are exploring during the autonomous exploration of multiple unmanned vehicles in an embodiment of the present invention.

[0091] Figure 5 This is a graph showing the change in exploration area over time when three unmanned vehicles are exploring during the multi-unmanned vehicle autonomous exploration process in an embodiment of the present invention. DETAILED DESCRIPTION

[0092] In order to better understand the technical solution of the present application, the embodiments of the present application are described in detail below with reference to the accompanying drawings.

[0093] It should be clear that the described embodiments are only part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in the field without creative work are within the scope of protection of the present application.

[0094] The terms used in the embodiments of the present application are only for the purpose of describing specific embodiments, and are not intended to limit the present application. The singular forms "a", "an", "the" and "the" used in the embodiments of the present application and the appended claims are also intended to include plural forms, unless the context clearly indicates other meanings.

[0095] Example 1

[0096] like Figure 1 As shown, in this embodiment, a multi-unmanned vehicle autonomous exploration method for real large-scale complex scenes is provided, and the experimental scene is an indoor place of 100m×100m. The following steps are included:

[0097] S1, construct the initial UFOMap map, specifically:

[0098] First, establish a world coordinate system, with the position of the unmanned vehicle when it starts from a standstill as the origin of the world coordinate system, and set the coordinate system of the UFOMap map to be the same as the world coordinate system.

[0099] Secondly, initialize the occupancy state of each child node in the UFOMap map to an unknown state, that is, the occupancy probability is 0.5.

[0100] Finally, the UFOMap of the constructed three-dimensional environment is Figure 2 The black part in the figure represents obstacles in the three-dimensional environment, the gray part represents free space in the three-dimensional environment, and the transparent part represents the unknown part in the three-dimensional space. The colors of the free space, obstacle space, and unknown space can be changed to other colors without affecting the representation. The newly constructed UFOMap is fully transparent. In this example, the map resolution of UFOMap is set to 0.4m×0.4m×0.4m.

[0101] S1, construct the initial undirected traversable topological graph, specifically:

[0102] Initialize the undirected traversable topology graph G m = {V m , E m The root node of the robot is the current position before the robot starts exploring. In this example, the coordinates of the root node are (0, 0, 0).

[0103] S2, the method for obtaining the position and posture of the radar coordinate system relative to the world coordinate system and the robot coordinate system relative to the world coordinate system in real time, and registering the radar point cloud data in the world coordinate system is:

[0104] First, the robot receives radar point cloud data, and after a period of accumulation, it extracts features from the accumulated radar data to extract surface points and corner points. The method for extracting corner points and surface points is:

[0105] Calculate the curvature c at each point using the formula:

[0106]

[0107] Among them, S is the i-th point in the k-th frame point cloud data A set of consecutive points near is a point in S, and |S| represents the number of points in S.

[0108] After calculating all the points in the current frame point cloud, sort all the points according to the curvature value, and then select the points with large curvature as corner points and the points with small curvature as surface points. Note that when selecting feature points, the plane points of parallel laser beams, obscured plane points and outliers should be eliminated. These are unreliable point cloud data and cannot be obtained in two consecutive frames of point cloud data, and cannot be used as feature points.

[0109] Then, the IMU data is received for forward propagation, that is, the integration is used to obtain a rough estimate of the pose, and then back propagation is performed for motion compensation. The forward propagation derives the pose change formula through acceleration a and angular velocity ω as follows:

[0110]

[0111] in, It represents the posture transformation of the robot relative to the world coordinate system, including translation and rotation matrices. The function f(a, ω) represents the process of updating the posture through IMU data.

[0112] The error caused by the back-propagation compensation motion is corrected by combining the current posture with the posture obtained by IMU integration to obtain the corrected posture error T c , the motion compensation formula is:

[0113]

[0114] Among them, T I is a rough estimate of the pose, that is, obtained during the forward propagation process T e is the estimated error pose.

[0115] Finally, the residual of the radar odometer is calculated, and the pose transformation is estimated using iterative Kalman filtering until convergence, obtaining the pose of the radar relative to the world coordinate system. The radar point cloud data is then registered to the world coordinate system.

[0116] S3, method for obtaining radar point cloud data of ground points:

[0117] First, the received raw radar point cloud data is filtered for reflection noise. Removal method: remove data points that are close to the radar sensor, have a height less than a certain threshold, and have an intensity less than a certain threshold. These points are noise points.

[0118] Secondly, a height-based filtering method is used to consider points below a certain height as ground points. Height-based filtering method: All radar point clouds are projected into a 2D plane, the Z values ​​of the points in the point cloud are compared, and all radar points above a certain threshold are removed, and the remaining radar points are considered to be ground points.

[0119] Finally, these ground radar point data are aligned to the world coordinate system, and the alignment formula is:

[0120]

[0121] Among them, P M is the radar point cloud data after registration, is the coordinate transformation matrix from the radar coordinate system to the world coordinate system, P L is the current ground radar point cloud data. S6, the method of fusing the observed radar point cloud data into the existing three-dimensional UFOMap map and updating the occupancy status of the voxels in the UFOMap map is:

[0122] According to the point cloud data z observed by the radar sensor 1：t , update the occupancy probability P(n|z 1：t ) in the logarithmic form L(n|z 1：t ), use the following formula to update:

[0123]

[0124] Among them, the observation data of the sensor is z t , the prior occupancy probability of the node is P(n), and the node occupancy probability before the current observation is estimated to be P(n|z 1：t-1 ). P(n|z t ) represents a given measurement z t The occupancy probability of voxel n is obtained. Prior probability P(n) = 0.5. The logarithmic symbol representation of the above formula is:

[0125] L(n|z 1：t)=L(n|z 1:t-1 )+L(n|z t ) (6)

[0126]

[0127] That is, when updating the logarithmic value of the occupancy probability of the node in UFOMap, the logarithmic value of the occupancy probability of the current node history is L(n|z 1：t-1 ) and the logarithm of the occupancy probability of the node observed in the current frame L(n|z t ) are added together.

[0128] The nodes in the UFOMap grid map have three states: unknown, occupied, and free. They are updated according to the node probability values ​​calculated above. The update formula is as follows:

[0129]

[0130] Among them, occ(n) represents the occupancy probability value of node n, t f , t o The threshold is set manually.

[0131] S7, based on the UFOMap map, extract the frontier points in the environment, extract the viewpoints, and calculate the information gain of the viewpoints. The method is as follows:

[0132] First, during the operation of the unmanned vehicle, the newly updated voxel grid recorded in the UFOMap map and the existing frontier points around the current position of the robot are detected, and the voxels in the UFOMap map with an occupancy state of free state and adjacent to at least one voxel with an unknown state at the same height are extracted as frontier points. Secondly, during the operation of the unmanned vehicle, a certain range around the robot is uniformly sampled in the detected navigable free space to obtain candidate viewpoints, and the number of observable frontier points of each viewpoint is evaluated through visibility checks, and viewpoints that can observe multiple frontier points are retained.

[0133] Finally, the number of frontier point units that can be observed by a viewpoint is used as the information gain measure of the viewpoint. In this example, the information gain of the viewpoint G v The calculation formula is:

[0134] G v =K×n v (9)

[0135] Among them, n v is the number of frontier points observed by the viewpoint, in this example K=1000.

[0136] S9, in order to avoid multiple unmanned vehicles repeatedly exploring the same area during the exploration process, it is necessary to update the local unmanned vehicle viewpoint status based on the information obtained from other unmanned vehicles. The specific method is:

[0137] First, calculate the specific position of the local unmanned vehicle frontier point in the map formed by other unmanned vehicles. Define the global coordinate system as M and the body coordinate system of robot i as R i , the map coordinate system of robot i is M i , where i∈{0, 1, ..., n-1}, n is the number of robots. The coordinate transformation matrix from the robot i body coordinate system to the robot i map coordinate system can be obtained by the state estimation method. The coordinate transformation matrix from the map coordinate system of robot i to the global coordinate system is set by the preset method when the program is running are all the frontier points detected and extracted by robot i, where the viewpoint The reference coordinate system is M i .f i is one of the frontier points detected by robot i, and is transformed into the map coordinate system of robot j using the following formula:

[0138]

[0139] Among them, f j To f i Transform to the corresponding voxel point in the robot j map coordinate system.

[0140] Then, determine the state of the frontier point. If f j In the UFOMap of robot j, it is an unknown voxel. i In robot i, it is a frontier point, otherwise it is considered that f i Non-frontier point.

[0141] S9, in order to plan the paths of multiple unmanned vehicles, it is necessary to merge the traversable topological maps of multiple unmanned vehicles. The specific method is as follows:

[0142] First, in order to facilitate subsequent calculations, the traversable topological map constructed by other unmanned vehicles is transformed into the current unmanned vehicle map coordinate system.

[0143] Then, we calculate the points where the topological graphs can be merged and define them as overlapping points. We define the minimum spacing of multiple traversable topological graphs constructed by unmanned control as r. When the node v of the traversable topological graph of robot a is a The distance to any node in robot b is greater than When a There is no overlapping area. When the node v of the traversable topology graph of robot aa When the distance to any node in robot b is less than or equal to 0.2·r, it is considered that at node v a There is an overlapping area. When the node v of the traversable topology graph of robot a a The distance from any node in robot b is When the node v is between a The values ​​of α, β, and γ at:

[0144]

[0145] Among them, v a =(x P ,y P , z P ), (x A ,y A , z A ), (x B ,y B , z B ), (x C ,y C , z C ) is the distance v a The coordinates of the three nodes in the topological map of robot b after the recent topological map transformation. After obtaining the values ​​of α, β, and γ, the node status is judged by the following formula:

[0146]

[0147] Among them, state(v a ) represents node v a Whether it is an overlapping point.

[0148] Finally, the two traversable topological graphs are merged at the overlapping point. The distance between the overlapping point and the adjacent node in the other unmanned vehicle topological graph is calculated, that is, the weight edge in the topological graph. The corresponding nodes are connected using this edge to merge the topological graphs.

[0149] S9, in order to achieve efficient exploration of multiple unmanned vehicles, it is necessary to assign tasks to multiple unmanned vehicles in real time. The specific method is as follows:

[0150] First, obtain all viewpoints observed by the current robot itself V = [V1, V2, ..., V n ] and the robot's own position V0. The problem of a single robot exploring a bunch of viewpoints is abstracted into a single traveling salesman problem, and the viewpoint exploration priority set is calculated using the following formula:

[0151]

[0152] Among them, U(V k) is the viewpoint V k The utility of G(V k ) is the viewpoint V k The information gain, E(V k )=exp(-C·L(V0,V k )) is the viewpoint V k The penalty factor is used to prevent the robot from cyclical motion. C is the penalty factor, L(V0, V k ) is the robot's path from V0 to V along the path obtained from the traversable topology k The cumulative moving distance. D(V q-1 , V q ) is the viewpoint V q-1 and V q The distance between them, υ0 is the current movement speed of the robot.

[0153] Secondly, select the next viewpoint to be explored by the robot from the obtained viewpoint exploration priority set, denoted as V p .

[0154] Then, the distributed multi-robot task allocation method is used to allocate the viewpoint of the current robot. The formula is as follows:

[0155]

[0156] Among them, V is all the viewpoints observed by the robot, R represents the current position of all robots, and c ij represents the actual path distance from robot i to viewpoint j, which is calculated by the A* algorithm in the traversable topology graph. Finally, the label j of the viewpoint that the robot wants to go to is obtained, and the viewpoint is obtained according to the label, and the viewpoint is recorded as V m .

[0157] Then, the dispersion between robots is calculated by the following formula, and the movement of robots is controlled according to the dispersion to ensure that multiple unmanned vehicles can be dispersed as much as possible when exploring large-scale unknown scenes:

[0158]

[0159] Where σ is the dispersion among robots and n represents the number of robots. i , P j (i, j<n) represents the positions of robots i and j respectively.

[0160] Finally, set σ u , σ l are the upper and lower thresholds of the dispersion, respectively. In order to avoid frequent switching of the algorithm, the following formula is used to select the final target point:

[0161]

[0162] Among them, S represents the single-vehicle viewpoint allocation mode, M represents the multi-vehicle target allocation mode, and S t-1 Indicates the viewpoint allocation mode of the previous algorithm. The final target point V T is the target point that the unmanned vehicle is going to. In this example, σ l =7,σ u =15.

[0163] S10, in a two-dimensional traversable grid graph, a method for using the A* algorithm to perform path search to obtain a distance-optimal path is:

[0164] First, the A* algorithm needs to establish two sets: an open set and a close set. The open set stores grid points that have been explored but for which the optimal cost function has not yet been determined. The close set stores grid points for which the cost loss function has been determined. The starting grid point is placed in the open set.

[0165] Then, the A* algorithm performs the following loop: take the grid point with the smallest cost function from the open set as the current grid point. If it is the grid point of the target position, exit the loop, otherwise add it to the close set and delete it from the open set. Traverse all the adjacent grid points of the current node. In the present invention, the adjacent grid points are the eight neighborhood grid points of the current grid point, and add them to the open set. It should be noted here that the nodes in the close set do not need to be added, and the nodes already in the open set need to update the loss function.

[0166] The cost function of the A* algorithm is expressed as F(p)=G(p s ,p)+H(p,p t ), where p is the current position, p s represents the starting position, p t represents the target position, G(p s , p) represents the shortest path length from the starting position to the current position, H(p, p t ) represents the straight-line distance from the current position to the target position.

[0167] Finally, after searching for the target node, backtracking is performed to find the optimal path. The unmanned vehicle receives the path and moves to the target point, repeating the above steps until multiple unmanned vehicles have explored the entire unknown area. The figure below shows the changes in the explored area over time during the exploration process of multiple unmanned vehicles. Figure 3 ,4,5 show the changes of exploration area over time for 1 robot, 2 robots and 3 robots in the same scene respectively.

[0168] It should also be noted that the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, commodity or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, commodity or device. In the absence of more restrictions, the elements defined by the sentence "comprises a ..." do not exclude the existence of other identical elements in the process, method, commodity or device including the elements.

[0169] Example 2

[0170] This embodiment relates to an autonomous exploration system for multiple unmanned vehicles for real large-scale complex scenes, including a memory and one or more processors, wherein the memory stores executable code, and when the one or more processors execute the executable code, they are used to implement the autonomous exploration method for multiple unmanned vehicles for real large-scale complex scenes of this embodiment 1.

[0171] Example 3

[0172] This embodiment relates to a computer-readable storage medium on which a program is stored. When the program is executed by a processor, the multi-unmanned vehicle autonomous exploration method for real large-scale complex scenes of Example 1 is implemented.

[0173] The above describes specific embodiments of the present specification. Other embodiments are within the scope of the appended claims. In some cases, the actions or steps recited in the claims can be performed in an order different from that in the embodiments and still achieve the desired results.

[0174] The above description is merely a preferred embodiment of one or more embodiments of the present specification and is not intended to limit one or more embodiments of the present specification. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of one or more embodiments of the present specification shall be included in the scope of protection of one or more embodiments of the present specification.

Claims

1. An autonomous exploration method for multiple unmanned vehicles in real large-scale complex scenes, characterized by: The following steps are involved: S1: Establish a world coordinate system, voxelize the three-dimensional environment to be explored by the unmanned vehicle, construct an initial UFOMap map, which is an octree structure, and at the same time, construct an initialized undirected traversable topology map; S2: Read the observation data of the radar and IMU, thereby obtaining the position and posture of the radar coordinate system relative to the world coordinate system and the position and posture of the robot coordinate system relative to the world coordinate system in real time, as well as the radar point cloud data registered to the world coordinate system; S3: Obtain the radar observation data obtained at the current observation position, process it to obtain the ground radar data and the radar point cloud data of the obstacle, and convert it to the world coordinate system; merge the ground radar data with the obstacle radar data to obtain the registered radar point cloud data within a period of time within the local range of the unmanned vehicle; S4: Based on the radar point cloud data within a period of time in the local range of the unmanned vehicle obtained in step S3, a two-dimensional grid map is constructed, the radar point cloud data is projected onto the two-dimensional grid map, the number of radar points in each grid and the height information of the radar points are recorded, and the passability of each grid is generated by comparing with a predetermined threshold, thereby constructing a passable two-dimensional grid map; S5: Fusing the radar pose and radar point cloud data obtained in step S2 into the existing three-dimensional voxel UFOMap map, and updating the occupancy status of the voxels in the UFOMap map; S6: based on the passable two-dimensional grid map obtained in step S4, sampling the passable points at the current position of the unmanned vehicle and merging them into the passable topology; S7: Based on the UFOMap obtained in step S5 and the traversable topology map obtained in step S6, frontier point extraction and viewpoint extraction in the environment and information gain calculation of the viewpoints are performed; S8: Information sharing: sharing the robot’s current position, viewpoint, traversable topology map, and constructed UFOMap, and accepting information shared by other unmanned vehicles; S9: Based on the information of other unmanned vehicles obtained in step S8, the local viewpoint state is updated, the traversable topological maps of multiple unmanned vehicles are merged, and then the tasks of multiple unmanned vehicles are allocated to obtain the target point to which the current unmanned vehicle is heading; S10: Based on the target point obtained in step S9 and the two-dimensional passable grid map obtained in step S4, the A* algorithm is used to perform path planning to obtain a distance-optimal path; S11: The unmanned vehicle performs autonomous movement based on the path obtained in step S10, continuously senses the surrounding environment during the movement, and repeats steps S2-S11 until there is no unexplored area in the current environment, thus completing the exploration task.

2. The method according to claim 1, characterized in that In step S1, the initial UFOMap is constructed, including: The UFOMap map is essentially an occupancy octree map. The position of the unmanned vehicle when starting the algorithm is used as the starting point of the world coordinate system, and the coordinate system of the UFOMap is set to be the same as the world coordinate system. The occupancy state of each child node in the UFOMap map is initialized to an unknown state, that is, the occupancy probability is 0.

5. The corresponding construction of the initial undirected traversable topological graph in step S1 specifically includes: Construct an undirected traversable topological graph G m = {V m , E m },in, represents the node of the graph, which is composed of the traversable points in the robot's current environment; in order to ensure the sparsity of the topological graph, the traversable points are obtained by uniform sampling; E m ={w ij } represents the edge of the graph, w ij is the connection weight between any two nodes in the graph; the root node of the undirected topological graph is the current starting position of the robot.

3. The method according to claim 1, characterized in that In step S2, the position and posture of the radar coordinate system relative to the world coordinate system and the position and posture of the robot coordinate system relative to the world coordinate system are obtained in real time, and the radar point cloud data is registered to the world coordinate system, which specifically includes: First, the robot receives radar point cloud data, and after a period of accumulation, it extracts features from the accumulated radar data to extract surface points and corner points. The method for extracting corner points and surface points is as follows: Calculate the curvature c at each point using the formula: Among them, S is the i-th point in the k-th frame point cloud data A set of consecutive points near is a point in S, and |S| represents the number of points in S. After calculating all the points in the point cloud of the current frame, all points are sorted according to the curvature value, and then the points with large curvature are selected as corner points, and the points with small curvature are selected as surface points. Then, the IMU data is received for forward propagation, that is, the integration is used to obtain a rough estimate of the pose, and then back propagation is performed for motion compensation; the forward propagation derives the pose change formula through acceleration a and angular velocity ω as follows: in, Represents the posture transformation of the robot relative to the world coordinate system, including translation and rotation matrices. The function f(a, ω) represents the process of updating the posture through IMU data. The error caused by the back-propagation compensation motion is corrected by combining the current posture with the posture obtained by IMU integration to obtain the corrected posture error T c , the motion compensation formula is: Among them, T I is a rough estimate of the pose, that is, obtained during the forward propagation process T e is the estimated error pose; Finally, the residual of the radar odometer is calculated, and the pose transformation is estimated using iterative Kalman filtering until convergence, obtaining the pose of the radar relative to the world coordinate system; then, the radar point cloud data is aligned to the world coordinate system based on the radar pose.

4. The method according to claim 1, characterized in that: The radar point cloud data of the ground point is obtained in step S3, specifically including: First, the received raw radar point cloud data is filtered for reflected noise; then, a height-based filtering method is used to consider points below a certain height as ground points; finally, these ground radar point data are aligned to the world coordinate system, and the alignment formula is: Among them, P M is the radar point cloud data after registration, is the coordinate transformation matrix from the radar coordinate system to the world coordinate system, P L It is the current ground radar point cloud data.

5. The method according to claim 1, characterized in that In step S6, the radar point cloud data is integrated into the existing three-dimensional voxel UFOMap map, and the occupancy status of the voxels in the UFOMap map is updated, which specifically includes: Integrate the point cloud data read by the radar into the UFOMap; according to the observation data of the sensor 1：t , update the occupancy probability P(n|z 1：t ), use the following formula to update: Among them, the observation data of the sensor is z t , the prior occupancy probability of the node is P(n), and the node occupancy probability before the current observation is estimated to be P(n|z 1：t-1 ); P(n|z t ) represents a given measurement z t The occupancy probability of voxel n obtained by the prior probability P(n) = 0.5; the above formula is converted into the following logarithmic symbol representation: L(n|z 1：t )=L(n|z 1：t-1 )+L(n|z t ) (6) The logarithmic notation allows for faster updates because multiplication is replaced by addition; logarithmic values ​​and probability values ​​can be converted to each other, and the logarithmic values ​​are stored for each voxel in the UFOMap grid. The states of nodes in the UFOMap grid graph can be unknown, occupied, and free. They are updated according to the probability values ​​of the nodes. The update formula is as follows: Among them, occ(n) represents the occupancy probability value of node n, t f , t o The threshold is set manually.

6. The method according to claim 1, characterized in that Step S7 extracts frontier points in the environment, extracts viewpoints, and calculates information gain of viewpoints based on the UFOMap map, specifically including: During the operation of the unmanned vehicle, the voxels in the UFOMap whose occupancy state is free and adjacent to at least one voxel whose state is unknown at the same height are extracted as frontier points; in order to improve the detection efficiency of frontier points, only the newly updated voxel grid recorded in the UFOMap and the existing frontier points around the robot are detected; During the operation of the unmanned vehicle, candidate viewpoints are obtained by uniformly sampling around the robot in the free space where the unmanned vehicle can pass. The number of visible frontier points of each viewpoint is evaluated through visibility check, and viewpoints that can observe multiple frontier points are retained; the number of frontier point units that can be observed by a viewpoint is used as the information gain measure of the viewpoint.

7. The method according to claim 1, characterized in that The information of other unmanned vehicles obtained in step S9 is used to update the local unmanned vehicle viewpoint state, specifically including: Define the global coordinate system as M and the body coordinate system of robot i as R i , the map coordinate system of robot i is M i , where i∈{0, 1, ..., n-1}, n is the number of robots; the coordinate transformation matrix from the robot i body coordinate system to the robot i map coordinate system can be obtained by the state estimation method The coordinate transformation matrix from the map coordinate system of robot i to the global coordinate system is set by the preset method when the program is running are all the frontier points detected and extracted by robot i, where the viewpoint The reference coordinate system is M i ;f i is one of the frontier points detected by robot i, and is transformed into the map coordinate system of robot j using the following formula: Among them, f j To f i Transform to the corresponding voxel point in the robot j map coordinate system; if at this time f j In the UFOMap of robot j, it is an unknown voxel. i is the frontier point in robot i, otherwise update f i is a non-frontier point; The number of frontier point units that can be observed by a viewpoint is used as the information gain measure of the viewpoint; the information gain G of the viewpoint v The calculation formula is: G v =K×n v (10) Among them, n v is the number of frontier points observed by the viewpoint, and K is the gain multiplier.

8. The method according to claim 1, characterized in that The merging of the traversable topological graphs of multiple unmanned vehicles in step S9 specifically includes: The premise of merging the traversable topology graphs of multiple unmanned vehicles is that there are already two or more robots in operation and have generated their own traversable topology graphs; it is known that the minimum spacing of the traversable topology graphs constructed by multiple unmanned vehicles is r. When the node v of the traversable topology graph of robot a is a The distance to any node in robot b is greater than When a There is no overlapping area; when the node v of the traversable topology graph of robot a a When the distance to any node in robot b is less than or equal to 0.2·r, it is considered that at node v a There is an overlapping area; when the node v of the traversable topology graph of robot a a The distance from any node in robot b is When the node v is between a The values ​​of α, β, and γ at: Among them, v a =(x P ,y P , z P ), (x A ,y A , z A ), (x B ,y B , z B ), (x C ,y C , z C ) is the distance v a The coordinates of the three nodes in the nearest robot b; after obtaining the values ​​of α, β, and γ, the node status is judged by the following formula: Among them, state(v a ) represents node v a After obtaining the set of overlapping points, the two traversable topological graphs are merged at the overlapping point positions.

9. The method according to claim 1, characterized in that: In step S9, task allocation for multiple unmanned vehicles is performed, which specifically includes: First, the problem of a single robot exploring a bunch of viewpoints is abstracted into a single traveling salesman problem. By solving this problem, the optimal path sequence for the robot to traverse all viewpoints is obtained to improve the efficiency of the robot's exploration of viewpoints. In this process, all viewpoints V observed by the robot are considered, and finally a set of path points is generated. The formula is as follows: Where V = [V1, V2, ..., V n ] is the sequence of all viewpoints, V0 is the current robot posture; U(V k ) is the viewpoint V k The utility of G(V k ) is the viewpoint V k The information gain, E(V k )=exp(-C·L(V0,V k )) is the viewpoint V k The penalty factor is used to prevent the robot from cyclical motion. C is the penalty factor, L(V0, V k ) is the robot's path from V0 to V along the path obtained from the traversable topology k Cumulative moving distance; D(V q-1 , V q ) is the viewpoint V q-1 and V q The distance between them, v0 is the current movement speed of the robot; then the next viewpoint to be explored by the robot is selected from the obtained path point set, recorded as V p ; Then, the distributed multi-robot task allocation method is used to allocate the viewpoint of the current robot. The formula is as follows: Among them, V is all the viewpoints observed by the robot, R represents the current position of all robots, and c ij represents the actual path distance from robot i to viewpoint j, which is calculated by the A* algorithm; finally, the label j of the viewpoint that the robot wants to go to is obtained, and the viewpoint is recorded as V m ; Then, the dispersion between robots is calculated by the following formula, and the movement of robots is controlled according to the dispersion to ensure that multiple unmanned vehicles can be dispersed as much as possible when exploring large-scale unknown scenes: Where σ is the dispersion among robots, n represents the number of robots; P i , P j (i, j<n) represents the positions of robots i and j respectively; Finally, set σ u , σ l are the upper and lower thresholds of the dispersion, respectively. In order to avoid frequent switching of the algorithm, the following formula is used to select the final target point: Among them, S represents the single-vehicle viewpoint allocation mode, M represents the multi-vehicle target allocation mode, and S t-1 Indicates the viewpoint allocation mode of the previous algorithm; the final target point V T The destination point that the unmanned vehicle wants to go to.

10. The multi-unmanned vehicle autonomous exploration system for real large-scale complex scenes is characterized by: It includes a memory and one or more processors, wherein the memory stores executable code, and when the one or more processors execute the executable code, it is used to implement the multi-unmanned vehicle autonomous exploration method for real large-scale complex scenes as described in any one of claims 1-9.

Images