CN116295426A - UAV autonomous exploration method and device based on 3D reconstruction quality feedback - Google Patents

Abstract

The invention relates to an unmanned aerial vehicle autonomous exploration method and device based on three-dimensional reconstruction quality feedback, wherein the method comprises the following steps: collecting laser radar point cloud data, and constructing a truncated signed distance field map based on the laser radar point cloud data; searching candidate target points in the exploration space, and obtaining the shortest path from each candidate target point to the current position of the unmanned aerial vehicle; judging the voxel state through the truncated signed distance field map, and calculating the exploration gain of the candidate target point; and evaluating the shortest path in combination with the exploration gain to obtain an optimal path. The method aims at improving the problem that the traditional large-range environment exploration algorithm does not consider the uncertainty of the scene reconstruction map, when an alternative target point is selected, not only the unknown information gain of the point is calculated, but also the consideration of the surface reconstruction effect is added, and finally a high-quality scene reconstruction model of the underground unknown environment is established.

Description

UAV autonomous exploration method and device based on 3D reconstruction quality feedback

technical field

The invention relates to the technical field of surveying and mapping, in particular to an autonomous exploration method and device for unmanned aerial vehicles based on three-dimensional reconstruction quality feedback.

Background technique

In applications such as regular inspections of large industrial building structures, the requirements for 3D modeling accuracy and system autonomy are getting higher and higher. Traditional 3D reconstruction tasks are performed by operators with handheld measurement equipment. In recent years, some researchers have tried to use unmanned vehicles to realize autonomous exploration and 3D reconstruction of small objects or small area environments. However, most of the current research work in the field of autonomous environment coverage and exploration planning focuses on how to find the optimal trajectory that can cover more unknown areas with less time and movement costs by analyzing the environment map, so as to drive the carrier along the While moving along the path, sensor perception data is collected, and finally a map expression that completely covers the environment is obtained. The effectiveness of this type of exploration algorithm is only measured according to the specific gravity of the volume of the known region in space, and does not take into account the uncertainty of the reconstruction model. In scenes with unstructured environmental features and a large range of height changes, traditional algorithms tend to lose detailed information in local areas, reducing the confidence and integrity of the environmental model. In order to complete the local details, it is often necessary to repeat the operation many times, resulting in redundant collected information and low efficiency of task completion.

Contents of the invention

The purpose of the present invention is to improve the problem that the traditional large-scale environment exploration algorithm does not consider the uncertainty of the scene reconstruction map. When selecting a candidate target point, it not only calculates the unknown information gain of the point, but also adds the effect of surface reconstruction. Consider, and finally build a high-quality scene reconstruction model of the underground unknown environment.

To achieve the above object, the present invention provides the following scheme:

The UAV autonomous exploration method based on 3D reconstruction quality feedback is characterized in that it includes:

Gather lidar point cloud data, build a truncated signed distance field map based on the lidar point cloud data;

Construct a random graph in the exploration space, search for candidate target points, and obtain the shortest path from each candidate target point to the current position of the drone;

Judging the voxel state by the truncated signed distance field map, and calculating the exploration gain of the candidate target point;

Evaluate the shortest path in combination with the exploration gain to obtain the best path.

Optionally, constructing a truncated signed distance field map according to the lidar point cloud data includes:

Projecting each laser reflection point in the new point cloud to the voxel grid of the global map, merging all laser rays whose laser reflection point falls into the same voxel, and obtaining a laser ray group;

Perform ray tracing based on the laser ray group at the voxel grid, traverse each voxel from the end point of the traced ray to the centroid of the sensor, update the projection distance and weight of the voxel by weighting, and construct the truncated signed Distance field map.

Optionally, the weighted method for updating the projection distance and weight of the voxel is:

W _i+1 (x,p)=min(W _i (x)+w(x,p),W _max )

Among them, x is the three-dimensional position of the voxel center in the navigation system, p is the three-dimensional position of a certain laser reflection point in the new point cloud in the navigation system, s is the three-dimensional position of the sensor in the navigation system, D _i is the currently stored projection distance of the voxel in the map, W _i is the stored weight, W _max is the upper limit of the weight of the voxel, d is the distance from the laser ray to the surface of the object obtained according to the new frame of point cloud data, w is the weight obtained from the new frame of point cloud data, D _i+1 is the updated projection distance, W _i+1 is the updated weight, and i is the number of voxel data currently updated.

Optionally, constructing a random graph in the exploration space includes:

The current position of the drone is added to the random graph as the root vertex, a random point is generated by sampling the Sample() function in space, and the nearest vertex to the random point is searched in the random graph;

According to the environment map, it is judged whether the connection from the random point to the vertex is passable. If the connection has no collision, the random point is used as a new vertex, that is, the latest point, and the latest point points to the connection of the random point. join the random graph as a new connection;

All vertices in the neighborhood of the latest point in the random graph are connected to the latest point, and if the connection has no collision, the vertices in the neighborhood of the latest point are connected to the latest point and added to the In the random graph, the final random graph is obtained.

Optionally, searching for candidate target points and obtaining the shortest path from each of the candidate target points to the current position of the drone includes:

Each vertex in the random graph is a candidate target point, and based on the connection relationship added to each point in the random graph, the Dijkstra algorithm is used to search for the shortest path from each of the candidate target points to the current position of the drone.

Optionally, judging the voxel state through the truncated signed distance field map includes:

The voxel state is divided into unknown, known with obstacles and known without obstacles;

If the voxel state is determined to be unknown, it is determined whether there are known obstacle pixels in a preset number of voxels around the specified voxel in the preset area.

Optionally, calculating the exploration gain of the candidate target point includes:

If the voxel state is unknown and there is no known obstacle pixel in the surrounding area, then accumulating the number of voxels as the unknown voxel gain of the candidate target point;

If the state of the voxel is unknown and there are known obstacles in the surrounding area, the voxel is a boundary voxel, which is included in the reconstruction surface gain of the candidate target point, if the state of the voxel is known There are obstacles, when the weight of the voxel is lower than a certain threshold, it is also included in the reconstruction surface gain of the candidate target point;

Perform weighted calculation according to the unknown voxel gain and the reconstructed surface gain to obtain the exploration gain of the candidate target point.

Optionally, obtaining the best path includes:

Evaluate all the shortest paths according to the sum of the path length, angle deviation and the exploration gain, and obtain the shortest path with the highest path exploration gain as the optimal path.

In order to achieve the above object, the present invention provides a UAV autonomous exploration device based on 3D reconstruction quality feedback, including:

A composition module for constructing a truncated signed distance field map based on lidar point cloud data;

Screening point module for searching candidate target points near the carrier;

A path optimization module, configured to obtain the shortest path from each of the candidate target points to the current position of the drone;

A voxel state judging module, configured to judge the voxel state through the truncated signed distance field map, and calculate the exploration gain of the candidate target point;

The path evaluation module is used to calculate the path exploration gains of all the shortest paths, and obtain the shortest path and the best path with the highest exploration gain.

The beneficial effects of the present invention are:

The present invention first constructs a truncated signed distance field map according to the laser radar point cloud data, and then searches for candidate target points through the RRG algorithm in the exploration space. Secondly, the Dijkstra algorithm is used to search the shortest path from each candidate target point to the current location, and then the voxel state is judged based on the map data, and the exploration gain of the candidate target point is calculated. Finally, all shortest paths are evaluated, and path exploration is performed to find the best path with the highest gain. By adopting the technical solution of the present invention, the UAV can judge the uncertainty of the three-dimensional reconstruction map when performing autonomous exploration tasks, and finally can improve the quality and confidence of scene reconstruction.

Description of drawings

In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the accompanying drawings required in the embodiments. Obviously, the accompanying drawings in the following description are only some of the present invention. Embodiments, for those of ordinary skill in the art, other drawings can also be obtained according to these drawings without paying creative labor.

Fig. 1 is the flow chart of the UAV autonomous exploration method based on the three-dimensional reconstruction quality feedback of the embodiment of the present invention;

FIG. 2 is a structural diagram of a UAV autonomous exploration device based on 3D reconstruction quality feedback according to an embodiment of the present invention.

Detailed ways

The following will clearly and completely describe the technical solutions in the embodiments of the present invention with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only some, not all, embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by persons of ordinary skill in the art without making creative efforts belong to the protection scope of the present invention.

In order to make the above objects, features and advantages of the present invention more comprehensible, the present invention will be further described in detail below in conjunction with the accompanying drawings and specific embodiments.

UAV autonomous exploration method based on 3D reconstruction quality feedback, including: collecting lidar point cloud data, constructing a truncated signed distance field map based on lidar point cloud data; constructing a random graph in the exploration space, searching for candidate target points, Obtain the shortest path from each candidate target point to the current position of the UAV; judge the voxel state through the truncated signed distance field map, and calculate the exploration gain of the candidate target point; combine the exploration gain to evaluate the shortest path to obtain the best path.

Constructing a truncated signed distance field map from lidar point cloud data includes: projecting each laser reflection point in the new point cloud into the voxel grid of the global map, and merging all laser reflection points falling into the same voxel Laser ray, get the laser ray group; perform ray tracing based on the laser ray group at the voxel grid, traverse each voxel from the end point of the traced ray to the sensor centroid, update the projection distance and weight of the voxel weightedly, and construct a truncated signed Distance field map.

Constructing a random graph in the exploration space includes: adding the current position of the drone to the random graph as the root vertex, generating random points by sampling in the space through the Sample() function, and searching for the closest vertex to the random point in the random graph ;According to the environment map, it is judged whether the connection from the random point to the vertex is passable. If the connection has no collision, the random point is used as the new vertex, that is, the latest point, and the connection from the latest point to the random point is added to the random graph as a new connection; All the vertices in the neighborhood of the latest point in the random graph are connected to the latest point. If the connection has no collision, the vertices in the neighborhood of the latest point and the latest point are connected to the random graph to obtain the final random graph.

Searching for candidate target points and obtaining the shortest path from each candidate target point to the current position of the UAV includes: each vertex in the random graph is a candidate target point, based on the connection relationship of each point added to the final random graph, through the Dijkstra algorithm Search the shortest path from each candidate target point to the current position of the UAV.

Judging the voxel state through the truncated signed distance field map includes: the voxel state is divided into unknown, known obstacles and known obstacles; if the voxel state is judged as unknown, then judge the specified voxel as the center Whether there are known obstacle pixels in the preset number of surrounding voxels in the preset area.

Calculating the exploration gain of candidate target points includes:

If the voxel state is unknown and there is no known obstacle pixel in the surrounding area, accumulating the number of voxels of this type as the unknown voxel gain of the candidate target point;

If the state of the voxel is unknown and there are known obstacles in the surrounding area, the voxel is a boundary voxel, which is included in the reconstruction surface gain of the candidate target point, if the state of the voxel is known There are obstacles, when the weight of the voxel is lower than a certain threshold, it is also included in the reconstruction surface gain of the candidate target point;

The weighted calculation is performed according to the unknown voxel gain and the reconstructed surface gain to obtain the exploration gain of the candidate target point.

Obtaining the best path includes: evaluating all shortest paths according to the sum of path length, angle deviation and exploration gain, and obtaining the shortest path with the highest path exploration gain as the best path.

In order to achieve the above purpose, this embodiment also provides an autonomous exploration device for UAVs based on 3D reconstruction quality feedback, including: a composition module, which is used to construct a truncated signed distance field map based on lidar point cloud data; a screening point module, It is used to search for candidate target points near the carrier; the path optimization module is used to obtain the shortest path from each candidate target point to the current position of the UAV; the voxel state judgment module is used to judge by truncated signed distance field map The voxel state calculates the exploration gain of the candidate target point; the path evaluation module is used to calculate the path exploration gain of all the shortest paths, and obtains the shortest path and the best path with the highest exploration gain.

As shown in Figure 1, the present invention provides a UAV autonomous exploration method based on 3D reconstruction quality feedback, including:

Step 1. The point cloud data is the data about the environment space of the exploration mission. Start the lidar, and the sensor can collect the data by transmitting the point cloud data to the processor through the network cable. According to the lidar point cloud data, construct a truncated signed distance field map ( The map is composed of square voxels, and each voxel stores truncated signed distance and weight data. Signed means: if the voxel is on the side close to the sensor, the distance is positive, otherwise, it is negative. The truncated type means that if the distance is too far or too close, the distance is truncated as a fixed value);

Step 2, search for candidate target points by RRG algorithm in the exploration task area (exploration space);

Step 3, search for the shortest path from each candidate target point to the current position through the Dijkstra algorithm;

Step 4. Determine the voxel state based on the map data, and calculate the exploration gain of the candidate target point;

Step 5. Evaluate all the shortest paths, and perform path search for the best path with the highest gain.

Further, step 1 specifically includes:

Step 1.1. Project each laser reflection point in a new frame of point cloud to the voxel grid of the global map, and then merge the laser rays whose reflection points fall into the same voxel into a set of laser rays;

Step 1.2: Perform ray tracing only once based on this group of laser rays at the voxel grid, traverse each voxel from the end point of the traced ray to the sensor centroid, and weight update the projection distance D _i+1 and weight W _{i of} the voxel ₊₁ ,

W _i+1 (x,p)=min(W _i (x)+w(x,p),W _max )

Among them, x is the three-dimensional position of the voxel center in the navigation system, p is the three-dimensional position of a certain laser reflection point in the new point cloud in the navigation system, s is the three-dimensional position of the sensor in the navigation system, D _i+1 is the updated projection distance, W _i+1 is the updated weight, and i is the number of times the voxel data has been updated currently. D _i is the currently stored projection distance of the voxel in the map, W _i is the stored weight, and W _max is the upper limit of the weight of the voxel. d is the distance along the laser ray to the surface of the object obtained according to the new frame of point cloud data, w is the weight obtained according to the new frame of point cloud data, and the calculation formula is as follows:

d(x,p,s)=‖p-x‖sign((p-x)·(p-s))

Among them, sign() represents the sign function, which is the point multiplication operation symbol of the vector, w _t is the update weight of the voxel whose projection distance is greater than the truncation distance and close to the sensor side, 0<w _t <1, dis _trun is the truncation distance, size is the map voxel size.

Further, step 2 specifically includes:

Step 2.1, add the current position of the drone to the random graph as the root vertex;

Step 2.2, generate a random point X _rand by sampling the Sample() function in the exploration space;

Step 2.3, searching for the vertex X _nearest closest to the random point in the random graph;

Step 2.4. According to the currently constructed environment map, it is judged whether the connection from the random point to the nearest point is passable. If the connection is collision-free, then the random point is used as a new vertex, the connection from the random point to the nearest point and the connection from the nearest point to the random point are added to the random graph as a new connection (the connection is a vector with direction) , all vertices in the random graph are candidate target points;

Step 2.5. All vertices in the random graph within the neighborhood of the new vertex also form connections with the new vertex. If the connections are safe, these connections are also added to the random graph.

Further, step 4 specifically includes:

Step 4.1. Based on the constructed truncated signed distance field map, determine the Status (m) of the voxel m. The status is divided into three types: unknown, known with obstacles and known without obstacles:

Among them, W(m) and D(m) are the weight and projection distance of the voxel, respectively, and sur _thre is the upper limit of the projection distance of the surface voxel;

Step 4.2, if the state of the voxel is unknown, determine whether there are voxels in the surrounding 26 voxels in the cube area centered on a certain voxel as known obstacle pixels;

Step 4.2.1. If there is no known obstacle pixel in the surrounding voxel, this voxel needs to be included in the unknown voxel gain Gain _ukn (ξ) of the candidate target point ξ:

Among them, Vis(ξ) is all the voxels that can be perceived within the 3D lidar field of view at the candidate point ξ, Ukn is the unknown voxel set, Occup is the known obstacle pixel set, Free is the known barrier-free collection of object voxels;

Step 4.2.2. If there are known obstacle pixels in the surrounding voxels, the voxel is a boundary voxel, which needs to be included in the reconstruction surface gain Gain _rec (ξ) of the candidate target point ξ;

Step 4.3. If the state of the voxel is known to have obstacles, when the confidence level is low, the reconstruction surface gain Gain _rec (ξ) of the candidate target point ξ needs to be included in the same boundary voxel:

Among them, U is the boundary voxel set, S is the surface voxel set, η _thre is the threshold value of the influence factor, and η(m) is the voxel influence factor, which is used to measure the impact of surface voxels on the reconstruction effect, and its calculation formula is :

ang _span = 2tan(size,2z)

Among them, z is the distance information from the sensor to the voxel center, N _rays (m) is the number of laser rays that can pass through the voxel, and ang _span is the angle range that the voxel can span in the X and Y directions, angres _x and angres _y are the angular resolutions of the 3D lidar in the X and Y directions, respectively.

Step 4.4. Based on the unknown voxel gain Gain _ukn (ξ) and the reconstructed surface gain Gain _rec (ξ), the weighted calculation obtains the exploration gain ExpGain (ξ) of the candidate target point.

Further, step 5 specifically includes:

Step 5.1. Evaluate all the shortest paths according to the path length, angle deviation and the sum of the exploration gains of each point on the path. The formula for calculating the path exploration gain of the jth shortest path Path _j is as follows:

是路径Path_j上的第k个顶点，/>

是沿着Path_j从/>

到根顶点/>

的距离。Among them, γ _S and γ _D are adjustable coefficients greater than 0, Path _exp is a path whose length is the same as Path _j but whose direction is consistent with the current heading of the UAV, S(Path _j , Path _exp ) is Path _j and Path The distance matrix between _exp . m _j is the total number of vertices on the path,

is the kth vertex on the path Path _j , />

is along Path _j from />

to the root vertex />

distance.

Step 5.2: The shortest path with the highest path search gain is the best path, and the best path is executed.

The present invention first constructs a truncated signed distance field map according to the laser radar point cloud data, and then searches for candidate target points through the RRG algorithm in the exploration space. Secondly, the Dijkstra algorithm is used to search the shortest path from each candidate target point to the current location, and then the voxel state is judged based on the map data, and the exploration gain of the candidate target point is calculated. Finally, all shortest paths are evaluated, and path exploration is performed to find the best path with the highest gain. By adopting the technical solution of the present invention, the UAV can judge the uncertainty of the three-dimensional reconstruction map when performing autonomous exploration tasks, and finally can improve the quality and confidence of scene reconstruction.

As shown in Figure 2, the present invention also provides a UAV autonomous exploration device based on 3D reconstruction quality feedback, including:

A composition module for constructing an environmental map based on lidar point cloud data;

Screening point module for searching candidate target points near the carrier;

Path optimization module, for searching the shortest path from each candidate target point to the current position;

The path evaluation module is used to calculate the path exploration gains of all the shortest paths, and finally execute the best path with the highest path exploration gain.

By adopting the technical solution of the present invention, the UAV can judge the uncertainty of the three-dimensional reconstruction map when performing autonomous exploration tasks, and finally can improve the quality and confidence of scene reconstruction.

The above-mentioned embodiments are only descriptions of the preferred modes of the present invention, and do not limit the scope of the present invention. Variations and improvements should fall within the scope of protection defined by the claims of the present invention.

Claims (9)

1. An autonomous exploration method for unmanned aerial vehicles based on 3D reconstruction quality feedback, characterized in that it comprises: Gather lidar point cloud data, build a truncated signed distance field map based on the lidar point cloud data; Construct a random graph in the exploration space, search for candidate target points, and obtain the shortest path from each candidate target point to the current position of the drone; Judging the voxel state by the truncated signed distance field map, and calculating the exploration gain of the candidate target point; Evaluate the shortest path in combination with the exploration gain to obtain the best path. 2. the unmanned aerial vehicle autonomous exploration method based on three-dimensional reconstruction quality feedback according to claim 1, is characterized in that, according to described lidar point cloud data, builds the truncated signed distance field map comprising: Projecting each laser reflection point in the new point cloud to the voxel grid of the global map, merging all laser rays whose laser reflection point falls into the same voxel, and obtaining a laser ray group; Perform ray tracing based on the laser ray group at the voxel grid, traverse each voxel from the end point of the traced ray to the centroid of the sensor, update the projection distance and weight of the voxel by weighting, and construct the truncated signed Distance field map. 3. the unmanned aerial vehicle autonomous exploration method based on three-dimensional reconstruction quality feedback according to claim 2, is characterized in that, the method for weighting the projection distance and the weight of updating described voxel is:

W _i+1 (x,p)=min(W _i (x)+w(x,p),W _max ) Among them, x is the three-dimensional position of the voxel center in the navigation system, p is the three-dimensional position of a certain laser reflection point in the new point cloud in the navigation system, s is the three-dimensional position of the sensor in the navigation system, D _i is the currently stored projection distance of the voxel in the map, W _i is the stored weight, W _max is the upper limit of the weight of the voxel, d is the distance from the laser ray to the surface of the object obtained according to the new frame of point cloud data, w is the weight obtained from the new frame of point cloud data, D _i+1 is the updated projection distance, W _i+1 is the updated weight, and i is the number of voxel data currently updated. 4. the unmanned aerial vehicle autonomous exploration method based on three-dimensional reconstruction quality feedback according to claim 1, is characterized in that, constructing random graph in described exploration space comprises: The current position of the drone is added to the random graph as the root vertex, a random point is generated by sampling the Sample() function in space, and the nearest vertex to the random point is searched in the random graph; According to the environment map, it is judged whether the connection from the random point to the vertex is passable. If the connection has no collision, the random point is used as a new vertex, that is, the latest point, and the latest point points to the connection of the random point. join the random graph as a new connection; All vertices in the neighborhood of the latest point in the random graph are connected to the latest point, and if the connection has no collision, the vertices in the neighborhood of the latest point are connected to the latest point and added to the In the random graph, the final random graph is obtained. 5. the autonomous exploration method of unmanned aerial vehicle based on three-dimensional reconstruction quality feedback according to claim 4, is characterized in that, search candidate target point, obtain each described candidate target point to the shortest path of unmanned aerial vehicle current position comprising: Each vertex in the random graph is a candidate target point. Based on the connection relationship of each point added to the final random graph, the Dijkstra algorithm is used to search for the shortest path from each of the candidate target points to the current position of the drone. 6. the unmanned aerial vehicle autonomous exploration method based on three-dimensional reconstruction quality feedback according to claim 4, is characterized in that, judge voxel state by described truncated signed distance field map comprising: The voxel state is divided into unknown, known with obstacles and known without obstacles; If the voxel state is determined to be unknown, it is determined whether there are known obstacle pixels in a preset number of voxels around the specified voxel in the preset area. 7. the UAV autonomous exploration method based on three-dimensional reconstruction quality feedback according to claim 6, is characterized in that, calculating the exploration gain of described candidate target point comprises: If the voxel state is unknown and there is no known obstacle pixel in the surrounding area, then accumulating the number of voxels as the unknown voxel gain of the candidate target point; If the state of the voxel is unknown and there are known obstacles in the surrounding area, the voxel is a boundary voxel, which is included in the reconstruction surface gain of the candidate target point, if the state of the voxel is known There are obstacles, when the weight of the voxel is lower than a certain threshold, it is also included in the reconstruction surface gain of the candidate target point; Perform weighted calculation according to the unknown voxel gain and the reconstructed surface gain to obtain the exploration gain of the candidate target point. 8. the unmanned aerial vehicle autonomous exploration method based on three-dimensional reconstruction quality feedback according to claim 1, is characterized in that, obtaining described optimal path comprises: Evaluate all the shortest paths according to the sum of the path length, angle deviation and the exploration gain, and obtain the shortest path with the highest path exploration gain as the optimal path. 9. The UAV autonomous exploration device based on 3D reconstruction quality feedback, characterized in that it includes: A composition module for constructing a truncated signed distance field map based on lidar point cloud data; Screening point module for searching candidate target points near the carrier; A path optimization module, configured to obtain the shortest path from each of the candidate target points to the current position of the drone; A voxel state judging module, configured to judge the voxel state through the truncated signed distance field map, and calculate the exploration gain of the candidate target point; The path evaluation module is used to calculate the path exploration gains of all the shortest paths, and obtain the shortest path and the best path with the highest exploration gain.

Images