Real-time Dense Reconstruction Method for Swarm UAVs

Abstract

The present invention proposes a method for real-time dense reconstruction of a swarm of drones. Relative positioning is carried out based on GNSS and VIO. Multiple drones equipped with cameras, IMUs, and GNSS are used to scan the target area to complete collaborative reconstruction. The relative positioning method is implemented through a dynamic distributed network (DDN). The DDN uses prior relative position information to adjust the structure of the distributed network in real time. For each sub-module of the distributed structure, dense reconstruction is completed using accurate local relative pose information. In each sub-module, a central node is set, and the poses between the virtual central nodes of each sub-module are estimated to fuse the point clouds. And a hierarchical point cloud fusion algorithm is adopted to perform point cloud registration between the drones in each sub-module and between different sub-modules of the entire distributed network, so as to obtain a registered high-precision point cloud map and the accurate relative pose between the drones. This point cloud registration method is more robust than general point cloud registration methods, and the registration accuracy and the accuracy of the relative pose recovered between the drones are higher.

Description

Technical Field

[0001] The present invention relates to the field of real-time three-dimensional dense reconstruction of unmanned aerial vehicles, and specifically relates to a method for real-time three-dimensional dense reconstruction of a cluster of unmanned aerial vehicles. Background Art

[0002] The demand for real-time three-dimensional dense reconstruction technology in disaster relief, agricultural monitoring, urban planning, etc. is increasing day by day, and it is one of the basic technologies for the future development of digital twin cities.

[0003] To implement this technology, first of all, it is required to achieve accurate self-position and attitude estimation of the unmanned aerial vehicle, and generate dense point cloud information from sensor data through a reconstruction algorithm. For reconstruction tasks within a small range, a single unmanned aerial vehicle can be used for global route planning, collecting sensor information, and completing the reconstruction. However, limited by the limited flight time of a single unmanned aerial vehicle, this reconstruction method is difficult to reconstruct a large-scale scene in a single flight. Moreover, since it is a single unmanned aerial vehicle performing the task, this method has disadvantages such as poor mobility and low efficiency under complex tasks, and the time to complete the reconstruction task is limited by the time of the flight route of a single unmanned aerial vehicle. Currently, most methods that can achieve collaborative three-dimensional dense reconstruction of multiple unmanned aerial vehicles rely on the SfM (Structure from Motion) algorithm. This algorithm can only perform dense reconstruction offline after collecting data and cannot perform real-time three-dimensional dense reconstruction.

[0004] The method of collaborative mapping of a large-scale scene by a cluster of unmanned aerial vehicles based on the SLAM (simultaneous localization and mapping) technology can perform three-dimensional reconstruction in real time during the flight of the unmanned aerial vehicle, which can greatly improve the efficiency of dense reconstruction. VIO (visual-inertial Odometry) is a SLAM system that fuses visual sensor and IMU (Inertial Measurement Unit) data. Although this positioning method can achieve high positioning accuracy locally, in a large-scale scene without repeated paths, it is difficult for this positioning method to eliminate cumulative errors.

[0005] At present, most of the real-time three-dimensional dense reconstruction technologies for a single drone are based on the flight path planning and dense reconstruction algorithms of the drone. The main defect of such algorithms is that the efficiency of data collection and mapping by a single drone is limited by its own standby time and the number of devices, and the real-time requirement cannot be met in most cases. The current swarm drone technology focuses on positioning and path planning, and often cannot generate a dense point cloud map in real time or can only generate a sparse point cloud map. A sparse point cloud map often cannot fully represent the environment and cannot meet the requirements in most cases. Although the three-dimensional reconstruction method of SfM can use multiple drones to collect data and obtain an accurate three-dimensional point cloud map, the computing power consumption is huge and the real-time requirement cannot be met. The existing SLAM algorithms also consume huge computing power in the application of multi-device collaborative positioning and mapping, and cannot meet the requirement of incrementally adding devices for multi-drone three-dimensional dense reconstruction.

[0006] "CN201910800208.4 - Multi-agent collaborative positioning and mapping method and device [ZH]" proposes a multi-agent collaborative reconstruction scheme. This scheme relies on the precise positioning and point cloud fusion technology of a single agent, does not make full use of the co-visibility information between agents, and has problems such as poor real-time reconstruction and poor robustness of the point cloud fusion method.

[0007] The core of this problem is to ensure the real-time performance of multi-drone collaborative dense reconstruction while ensuring the accuracy of dense reconstruction. How to extend from the dense reconstruction of a single drone to the collaborative and real-time reconstruction of multiple drones and give full play to the advantages of swarm drones is an urgent problem to be solved. Summary of the Invention

[0008] To solve the problems existing in the prior art, the present invention proposes a method for real-time dense reconstruction of swarm drones, which includes the following steps:

[0009] Step 1: After multiple drones in the swarm drone receive a three-dimensional dense reconstruction task and enter the flight path, they separately perform the initialization of VIO and autonomous positioning to obtain the VIO pose estimation information of each drone; GNSS, visual sensors and IMUs are carried on the drones.

[0010] The co-visibility relationship between drones is deduced based on the GNSS information of each drone, and the drones are divided into mapping subgroups according to the co-visibility relationship; each mapping subgroup contains several drones with co-visibility relationships; then the relative poses of the drones in the mapping subgroup are initialized according to the GNSS information, and the central drone is determined using the adjacent relationship.

[0011] Step 2: According to the VIO pose estimation information of each drone obtained in Step 1, jointly optimize the global pose with GNSS information, update the scale of the local positioning information and the position of the global coordinates in real time, align the positioning information with the restored scale with the image timestamp; and use the initial coordinates of each drone to update the relative poses between the drones in each subgroup.

[0012] Step 3: Detect the overlap rate of the images obtained by each drone in the mapping subgroup. If the overlap rate meets the set threshold requirement, use the relative pose result jointly optimized with GNSS information as the initial value, and use the common observations as constraints to further optimize the relative poses between the drones.

[0013] Step 4: Use the VIO pose estimation information of a single drone, the relative poses between the drones after being processed in Step 3, and the image information to perform dense three-dimensional reconstruction to generate a dense three-dimensional point cloud map.

[0014] Step 5: After each drone obtains the dense point cloud map in Step 4, according to the relative poses of each drone, transform the dense point clouds generated by each drone into the same coordinate system to achieve point cloud fusion.

[0015] Step 6: Use the hierarchical point cloud fusion registration algorithm. First, perform point cloud registration on the dense point cloud maps constructed by different drones within each mapping subgroup; after registering the point cloud maps in the subgroup, update the relative poses of the drones in each subgroup; then perform fusion registration on the point cloud maps constructed and fused in each subgroup, and update the relative poses of the central drones between different subgroups.

[0016] Step 7: After completing the precise registration, use the registration information to update the relative poses of each drone, and use the relative pose information to determine whether it is necessary to re-divide the subgroups; if it is necessary to re-divide the subgroups, return to Step 1 and use these relative poses to re-divide and initialize the dense reconstruction subgroups.

[0017] Furthermore, the process of initializing VIO and performing autonomous positioning in Step 1 is as follows: First, align the initial coordinate system of VIO with gravity using the IMU information on the aircraft, and solve the attitude of the drone at this time using the transformation between the initial stationary IMU information and the gravity vector; then form multiple frames of image information into a sliding window for local batch optimization, perform pure vision SfM in the sliding window to recover the unscaled camera trajectory, and obtain the IMU trajectory by integrating the IMU information; finally, align the camera trajectory and the IMU trajectory.

[0018] Further, the specific process of step 2 is as follows: First, obtain the local pose and GNSS information of the drone and align their timestamps; then use the local pose and GNSS information of VIO to construct a global optimization problem to minimize the residuals constructed by the two with the global pose, where the variables to be optimized include the global pose of the drone and the global scale of the VIO estimated pose; then solve the optimization problem and obtain the transformation between the local coordinate system and the global coordinate system through calculation; convert the VIO estimated pose after scale recovery to the camera pose using the extrinsic matrix and align the timestamps of the camera pose and the received images.

[0019] Further, in step 3, the specific method for calculating the overlap rate of the images collected by the two drones at the same moment is to calculate the homography matrix through the matching points of the images, so as to recover the transformation between the two cameras, and then transform the vertices of one image to the other image to calculate the proportion of the area of the polygon intersection in the image.

[0020] Further, in step 3, compare the overlap rates of the images collected by the drones in the same subgroup. If the overlap rate meets the set threshold requirements, use the co-visibility information to further optimize the relative pose between the cameras: construct a Sim3 problem and then optimize it in combination with the relative pose obtained in step 2; and use the coordinates of three pairs of well-matched points in the two coordinate systems to recover the transformation and scale between the two coordinate systems.

[0021] Further, in step 4, use the dense stereo vision matching algorithm to perform fast stereo matching on adjacent frames and frames at the same moment with overlap rates within a set range to obtain a depth map; then filter the obtained depth map and perform continuous image consistency checks to obtain a depth map with higher accuracy; after the consistency check, remove the redundant points in the adjacent frames to generate a dense point cloud map.

[0022] Further, in step 5, receive the dense three-dimensional point cloud maps constructed by each drone and the real-time relative poses between the drones respectively, and rely on the relative positioning between the drones to perform point cloud fusion:

[0023] For any point in the dense point cloud reconstructed by the k-th drone Fuse it into the point cloud map constructed by the central drone, and there is Use voxelization to downsample the fused dense point cloud map.

[0024] Further, in step 5, use the positioning information to perform two-dimensional point cloud position segmentation, store the point clouds into different point cloud blocks respectively; rely on the following formula to find the index of the point cloud block:

[0025] x = P x / d cell

[0026] y = P y / d cell

[0027] where P x , P x is the position of the coordinates of the point cloud on the x,y plane in the central coordinate system; d cell is the width of the point cloud block.

[0028] Furthermore, in step 6, the hierarchical point cloud fusion registration algorithm is as follows: First, determine the gravity direction of the point cloud, and then divide the point cloud into blocks in the gravity direction of the point cloud; for each block of point cloud, each point is projected onto the same plane perpendicular to the gravity direction. First, use the line-plane features of the two-dimensional compressed point clouds in different layers for rough registration, cluster the transformation results after registration to obtain the most likely transformation result; use this result to perform a rough transformation on the two dense point clouds, and then use the ICP algorithm combined with the three-dimensional point information of the corresponding feature points of the image to further perform fine registration of the dense point clouds.

[0029] Furthermore, in step 6, when the IMU and GNSS information is missing or the information obtained by the IMU and GNSS is unreliable, rely on the point cloud information to calculate the gravity direction: First, calculate the curvature of the two groups of point clouds respectively, and select the normal direction of the direction with the most surface features as the gravity direction.

[0030] Beneficial effects

[0031] Compared with the prior art, the present invention has the following beneficial effects:

[0032] 1. The present invention improves the VIO algorithm. Compared with the past methods, due to the adoption of the pre-optimized outlier removal strategy, it is more robust in the construction and solution process of the optimization problem, and the feature point selection and variable sliding window strategy can be applied to the occasions with limited computing resources. In large-scale scenarios, the unmanned aerial vehicle can effectively and real-time complete the estimation of its own pose. To meet the requirements of the computing platform, the improved algorithm of the present invention can either centrally process the pose estimation of each unmanned aerial vehicle with multiple instances on one device, or independently estimate the pose on each unmanned aerial vehicle.

[0033] 2. The multi-unmanned aerial vehicle relative positioning method used in the present invention relies on local accurate visual inertial odometry, GNSS information, and the co-visibility relationship between unmanned aerial vehicles. Compared with the existing multi-unmanned aerial vehicle relative positioning methods, the proposed method can incrementally increase the number of unmanned aerial vehicles used for mapping, and can globally optimize the pose of each unmanned aerial vehicle and solve the relative pose in real time in the occasion with very limited computing resources. When facing the dense reconstruction task, it can provide accurate positioning on the premise of relatively small computational cost.

[0034] 3. The hierarchical point cloud registration algorithm proposed by the present invention can accurately register dense point cloud maps. Even in scenarios with relatively high noise in the dense point cloud map, it can still work and exhibit strong robustness.

[0035] 4. Compared with the dense reconstruction method of a single drone in the past, the dense reconstruction method of the drone swarm proposed by the present invention greatly improves the reconstruction efficiency and can perform collaborative dense reconstruction of multiple drones in real time. When facing relatively complex dense reconstruction tasks, it has higher flexibility and can improve the mapping efficiency and accuracy by reasonably allocating tasks to each drone.

[0036] Additional aspects and advantages of the present invention will be given in part in the following description, become apparent in part from the following description, or be learned through the practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] The above and / or additional aspects and advantages of the present invention will become apparent and be readily understood from the description of the embodiments in conjunction with the following drawings, wherein:

[0038] Figure 1 is the overall process of the present invention;

[0039] Figure 2 is a schematic diagram of dividing the dense reconstruction sub-module (sub-group) of the present invention;

[0040] Figure 3 is a schematic diagram of the process of real-time large-scale dense reconstruction of the swarm drones in a dense reconstruction sub-group of the present invention;

[0041] Figure 4 is a schematic diagram of the process of realizing autonomous positioning of a single drone of the present invention;

[0042] Figure 5 is a schematic diagram of the process of realizing real-time dense reconstruction of the present invention;

[0043] Figure 6 is the process of hierarchical point cloud registration of the present invention; DETAILED DESCRIPTION OF THE EMBODIMENTS

[0044] In view of the problem of how to ensure the real-time performance of collaborative dense reconstruction of multiple drones while ensuring the accuracy of dense reconstruction, the present invention proposes a method for real-time dense reconstruction of a large scene by a swarm of drones. This method can realize collaborative real-time large-scale three-dimensional dense reconstruction of multiple drones, effectively improving the efficiency of dense reconstruction of large-scale scenes while ensuring accuracy.

[0045] The swarm drones in the present invention perform relative positioning based on GNSS (Global Navigation Satellite System) and VIO, and use multiple drones equipped with cameras, IMUs, and GNSS to scan the target area to complete collaborative reconstruction. The relative positioning method that combines GNSS signals and co-view information here is implemented through a dynamic distributed network DDN (Dynamic Distributed Network). The dynamic distributed network DDN uses prior relative position information to adjust the structure of the distributed network in real time. For each sub-module of the distributed structure, dense reconstruction is completed using accurate local relative pose information. In each sub-module, a central node is set, and the poses between the virtual central nodes of each sub-module are estimated to fuse the point clouds. And to ensure the consistency of the global point cloud map, a hierarchical point cloud fusion algorithm is adopted to perform point cloud registration between the drones in the sub-module and between different sub-modules of the entire distributed network respectively, so as to obtain a registered high-precision point cloud map and the accurate relative pose between drones. This point cloud registration method is more robust than general point cloud registration methods, and the registration accuracy and the accuracy of the relative pose recovered between drones are higher.

[0046] Specifically, it includes the following steps:

[0047] Step 1: After multiple drones receive the three-dimensional dense reconstruction task and enter the flight path, VIO is initialized separately and autonomous positioning is performed to obtain the VIO pose estimation information of each drone; the process of initializing VIO separately and performing autonomous positioning here is: an optimization problem is constructed using the matching information of feature points between adjacent image frames and IMU integration information, and the ceres solver is used to solve it to obtain the VIO pose estimation information of each drone.

[0048] The specific process is as follows: First, the initial coordinate system of VIO is aligned with gravity using the IMU information on the aircraft, and the attitude of the drone at this time is solved using the transformation between the initial static IMU information and the gravity vector, and the initial coordinate system is set as the FLU (Front Left Up) coordinate system. Immediately afterwards, 10-frame image information is formed into a sliding window for local batch optimization, and pure vision SfM is performed in the sliding window to recover the unscaled camera trajectory, and the IMU trajectory is obtained by integrating the IMU information. Finally, the camera trajectory and the IMU trajectory are aligned.

[0049] The key technology of this step is the precise positioning of a single drone:

[0050] First, in order to address the scenario of insufficient on-board computing power of drones, a strategy based on FAST feature points is added at the front end. After comparison, the positioning algorithm of such feature points can multiply the processing speed. The corner points detected by the FAST method are sometimes densely distributed, and the constraints provided by feature points that are too close to each other in BA optimization are very similar. For the optimization problem, the constraints provided by such feature points have a relatively low "cost performance" compared to the computing power consumption. Therefore, in the present invention, an optimal point cluster strategy is preferably given. For a single-channel image (grayscale image), after finding the feature points, a cluster is formed by the feature point and a certain number of its adjacent points. The quality of this cluster of points is compared, and the point with the highest quality is selected. Then, with the centroid of this group of points as the center and the point farthest from the center in this cluster as the radius, a mask is drawn, and other points within the mask are discarded. By relying on this method, the quality and uniformity of the feature points are effectively guaranteed. In the feature point screening of multi-channel images, feature points are extracted for each channel of the image respectively, and then the optimal point cluster strategy is executed centrally.

[0051] In the VIO optimization part, a visual-inertial bundle adjustment equation is used. In the construction of the optimization residuals, three types of residuals are mainly constructed: visual residuals, IMU residuals, and prior residual terms formed by marginalization of the sliding window. Let the window size be k + 1, and the overall optimization formula is as follows:

[0052]

[0053] Among them, e p and H p are the prior information from marginalization. J is the set of visual observation information, k 1 , k 2 ∈K, K is the size of the sliding window, is the residual generated by reprojection of the common observations between two image frames, W r is the information matrix of the visual residuals. represents the residual constructed from the observation information of the IMU, W s k is the information matrix of the IMU residuals. The specific construction of the residuals will be elaborated in detail later.

[0054] The variable X to be optimized in the optimization can be expressed as follows

[0055] X = [X 0 , X 1 , … X n , λ 0 , λ 1 , … λ m , t d

[0056] ​

[0057] x in the variables to be optimized k State parameters representing each frame, including the position of each frame within the window Velocity Rotation Acceleration bias and angular velocity bias b of the IMU a ,b g 。λ m Represents the inverse depth of all feature points within the window. t d Represents the time offset, mainly caused by the asynchronous timestamps of IMU information and image information.

[0058] To reduce the influence of outliers on the algorithm and further improve the robustness of the algorithm, a Huber robust kernel function is added to the visual residual term. The definition of the Huber norm is as follows:

[0059]

[0060] Visual residual:

[0061] Plane points are all assumed to be on the normalized plane. The same three-dimensional point observed in two frames is respectively on the normalized plane Then, according to the reprojection formula, The point can be reprojected to The camera normalized plane of the frame where it is located, thereby constructing a residual function. To obtain the three-dimensional point coordinates It is necessary to recover The coordinates of the point in the camera system of the i-th frame and project it to the camera coordinate system of the j-th frame:

[0062]

[0063] Among them, T bc Represents the extrinsic parameters of the camera, that is, the transformation between the camera and the IMU, Represents the transformation from the body coordinate system of the i-th frame to the initial world coordinate system, that is, the pose of the i-th frame body in the world coordinate system, Represents the transformation from the world coordinate system of the j-th frame to the body coordinate system, Represents the transformation from the body system to the camera coordinate system. The visual residual can finally be expressed as:

[0064]

[0065] IMU residual:

[0066] The IMU can obtain acceleration and angular velocity information through the accelerometer and gyroscope, and the rotation and displacement transformation between two frames can be obtained through integration. When performing non-linear optimization in the backend, the pose needs to be optimized. Each time the pose is adjusted, the IMU measurement values need to be re-transmitted between them, and re-integration is required, which will be very time-consuming. To avoid re-transmitting the measurement values, a pre-integration strategy is adopted. The pre-integration strategy is to avoid repeated integration of the IMU observation information after the variables to be optimized are adjusted. According to the IMU pre-integration formula, there is

[0067]

[0068]

[0069]

[0070] where, are the integrals of the displacement, velocity, and attitude transformation between two frames respectively. is the measurement value of the IMU, is the bias of the IMU, n a ,n w is the white noise of the IMU, is the attitude of the body coordinate system at time t relative to the previous frame, is the quaternion form of the attitude of the body coordinate system at time t relative to the previous frame. Therefore, the IMU residual can be constructed as follows:

[0071]

[0072] In the formula, are the displacement residual, velocity residual, attitude residual, and the residual of the IMU bias respectively. The variables to be estimated have been introduced before. Here, there are:

[0073]

[0074] The bias is used in the pre-integration term. If the bias is updated once during optimization, then in the next iteration, the pre-integration term in the residual term has to be recalculated, which needs to be avoided as much as possible. To solve this problem, the error propagation model of ESKF is used. Through this error propagation model, not only the problem of bias update is solved, but also the covariance matrix (information matrix) is calculated by the way.

[0075] Prior term:

[0076] After each optimization, when the sliding window slides, the data of the oldest frame or the second newest frame will be discarded. If these constraints are directly discarded, the accumulation of residuals will lead to a large cumulative error. Extracting the constraint information as a prior term and adding it to the subsequent optimization can greatly suppress the cumulative error while reducing the computational complexity.

[0077] The marginalization operation utilizes the Schur complement. We construct a new prior based on all marginalization metrics related to the removed states. Perform the Schur complement operation on the removed frames, and thus the constraint information is included in the incremental equation where is the matrix after performing the Schur complement operation on the Hessian matrix, and it is still a symmetric matrix.

[0078] Using the Jacobian matrix and the residual e 0 can be recovered.

[0079]

[0080]

[0081] To update Through derivation, we can obtain

[0082]

[0083] where dx is the difference between the updated state quantity and the state quantity at the marginalization.

[0084] To obtain better robustness, a pre-optimization strategy is adopted before optimization. Visual features with excessive residuals are removed through pre-optimization. In addition, some methods are taken to prevent the VIO system from failing. For the IMU observation information, estimate whether the numerical value of each observed information exceeds the threshold. If it exceeds the threshold, it is considered as a frame of relatively bad data and is directly discarded. For visual features, if the number of continuously tracked features is too small, it is considered that the VIO system has failed, and at this time, the VIO system will be restarted.

[0085] The significance of determining the dense reconstruction subgroup is to determine whether to further optimize the relative pose and perform three-dimensional dense reconstruction using the co-view information. Therefore, the co-view relationship between the drones is inferred based on the GNSS information of each drone, and the drones are divided into mapping subgroups according to the co-view relationship. Each mapping subgroup contains several drones with co-view relationships. Subsequently, the relative poses of the drones in the mapping subgroup are initialized according to the GNSS information, and the central drone is selected by voting using the adjacent relationship. The central drone is the drone with the most co-view relationships in the subgroup.

[0086] Step 2: According to the VIO pose estimation information of each drone obtained in Step 1, jointly optimize the global pose with GNSS information, update the scale of the local positioning information and the position of the global coordinates in real time, and align the scaled positioning information with the image timestamp.

[0087] Moreover, using the initial coordinates of each drone, update the relative poses between the drones in each subgroup. For example, update the relative pose between UAV No. 1 and UAV No. 2 in a certain subgroup. The formula is where respectively represent the poses of the two drones in the world coordinate system, represents the relative pose of the two drones in the world coordinate system.

[0088] This step is to obtain more accurate global poses of the drones and the relative poses between the drones. First, obtain the VIO local pose of the drone and the GNSS information, and align their timestamps. Then, use the VIO local pose and the GNSS information to construct a global optimization problem to minimize the residuals constructed by the two with the global pose. The variables to be optimized include the global pose of the drone and the global scale of the VIO estimated pose. Then, use ceres to solve the optimization problem and obtain the transformation between the local coordinate system and the global north-east-down coordinate system (ENU coordinate system) through calculation. The scaled VIO estimated pose is converted to the camera pose using the extrinsic matrix, and the camera pose is timestamp-aligned with the received pictures.

[0089] The key technology in this step is the GNSS loose coupling optimization.

[0090] 1) Optimize the pose and scale

[0091] To achieve relative positioning of multiple drones and restore the positioning and scale of the point cloud of multiple drones, a GNSS loose coupling algorithm is introduced. In the previous step, the local pose has been obtained. By coupling the local pose solved by the VIO part with the observation information obtained by the GNSS, the scale of the relative pose can be restored and the current global pose can be optimized. In addition, to achieve collaborative dense reconstruction of multiple devices, the GNSS information will also be used to restore the relative poses of multiple drones.

[0092] First, initialize the GNSS loose coupling system. Use the first frame of GNSS information obtained as the initial ENU coordinates and record the VIO initial local coordinate system at the same time. All subsequent GNSS information will be converted to the transformation to the original GNSS coordinate system using GeographicLib. All VIO local pose information will be converted to the transformation to the original local coordinate system.

[0093] After obtaining the VIO information and GNSS information, first align the timestamps of the two pieces of information, and then construct an optimization problem to optimize the global pose and recover the absolute scale information. Assume that the scale is represented by s. Before optimization, the initial value of s needs to be solved first.

[0094]

[0095] Among them, represents the displacement from the current frame GNSS coordinate to the initial ENU coordinate. represents the displacement of the current frame in the VIO local coordinate system, represents the initial position in the VIO local coordinate system.

[0096] Construct the GNSS loose coupling optimization function. The optimization function consists of two parts. One part is the constraint of the frame - to - frame transformation in the VIO local coordinate system

[0097]

[0098] Among them, e q is the rotation constraint brought by the VIO - estimated pose; e t represents the displacement constraint brought by the VIO - estimated pose; respectively represent the pose transformation between two adjacent coordinates after alignment with the GNSS timestamp in the VIO local coordinate system; s is the scale to be optimized; respectively represent the pose transformation between two adjacent coordinates in the northeast - up - east coordinate system, and are also the quantities to be optimized; is the operation on the quaternion error state.

[0099] The other part is the constraint brought by the GNSS observation information.

[0100]

[0101] Among them, is the displacement from the current frame to the GNSS initial coordinate system, is the displacement of the current frame in the world coordinate system, and s is the scale to be optimized.

[0102] 2) Use the GNSS loose coupling algorithm to complete the relative positioning between multiple UAVs

[0103] To determine the relative positions between multiple UAVs, the relative positions of the initial coordinate systems between multiple UAVs need to be determined. At this time, the pose of the central UAV is expressed as represents the rotation from the VIO local coordinate system of the central UAV to the world coordinate system (ENU coordinate system), which can be obtained by the formula Calculated. Represent the longitude, latitude and altitude of the central UAV in the GNSS original coordinate system (ENU coordinate system). For any UAV k, its pose is defined as Then the rotation of this UAV relative to the central UAV is The displacement is t 0k , which is calculated by using GeographicLib. For the convenience of subsequent derivation, use T 0k to represent the transformation from the initial coordinate system of the k-th UAV to the initial coordinate system of the central UAV.

[0104] During large-scale and long-term operation, the GNSS loose coupling algorithm can effectively reduce positioning drift. By updating the relative positioning between multiple UAVs where in the mapping part, the dense point cloud map can be fused according to the real-time relative position and the initial relative position of the UAVs to achieve multi-UAV collaborative three-dimensional dense reconstruction.

[0105] Step 3: Detect the overlap rate of the images obtained by each UAV in the mapping subgroup. If the overlap rate meets the set threshold requirement, use the relative pose result jointly optimized with GNSS information as the initial value, and use the common observation as the constraint to further optimize the relative pose between UAVs.

[0106] The specific process is as follows: Calculate the overlap rate of the pictures collected by different UAVs in the same subgroup at the same moment. If the overlap rate is relatively high, find the common observation of the two pictures through the feature point matching algorithm, and use the common observation (more than three pairs of three-dimensional points matched in two frames) to solve the Sim3 transformation, so as to further optimize the relative pose and relative scale between UAVs.

[0107] The key technology in this step is to couple the co-visibility information:

[0108] The specific method to calculate the overlap rate of the images collected by two UAVs at the same moment is to calculate the homography matrix through the matching points of the images, so as to restore the transformation between the two cameras, and then transform the vertices of one image to the other image, and calculate the proportion of the area of the polygon intersection in the image.

[0109] Compare the overlap rates of the images collected by UAVs in the same subgroup. When the overlap rate meets a certain threshold requirement, use the co-visibility information to further optimize the relative pose between cameras. Construct a Sim3 problem, and then optimize it jointly with the relative pose obtained in the previous step. Use the coordinates of three pairs of well-matched points in two coordinate systems to restore the transformation and scale between the two coordinate systems. The scale and transformation obtained in the Sim3 problem are s sim , T sim . The pose after joint optimization with GNSS information is represented as T w . Construct the optimization problem as:

[0110]

[0111] The relative pose and images obtained using the co-visibility relationship will also be added as the next step's consistency constraints to the data queue for dense reconstruction.

[0112] Step 4: Using the VIO pose estimation information of a single UAV and the relatively accurate relative pose and image information between UAVs after being processed in Step 3, perform dense three-dimensional reconstruction to generate a dense three-dimensional point cloud map.

[0113] Here, for adjacent frames and frames at the same moment with an overlap rate within a certain range, use the dense stereo vision matching algorithm to perform fast stereo matching to obtain a depth map; then filter the obtained depth map and perform continuous image consistency checking to obtain a depth map with higher accuracy; after consistency checking, remove redundant points in adjacent frames to generate a dense point cloud map.

[0114] The key technology in this step is dense reconstruction:

[0115] In the previous steps of VIO-GNSS coupled collaborative positioning (Step 1 and Step 2) and relative positioning using co-visibility information (Step 3), accurate relative positioning between image frames can already be obtained. Based on the relative positioning results, perform dense mapping. The dense mapping of a single UAV is mainly divided into three parts, Denser, Cleaner, and Pruner. Denser performs reconstruction, and Cleaner and Pruner process the point cloud to eliminate inconsistent points or redundant points. Use the estimated local accurate pose and the front and back two-frame images to construct a virtual stereo pair, thereby generating a depth map. Then use the depth map to recover a large-scale dense point cloud. To improve the matching efficiency, the method of maximizing the common area of the left and right views is applied. In the part of constructing the dense disparity map, the classic method of fast stereo matching, ELAS, is used.

[0116] In the above method, in order to further reduce the error of dense mapping, the consistency constraints between different frames are fully utilized. The higher the consistency, the higher the similarity between pixels, and the more accurate the recovered depth map. In the consistency detection part, a producer-consumer model is constructed. After the number of images in the image queue reaches a certain number, convert the recovered depth map into three-dimensional points and reprojection them onto other frames of pictures. If the residual is less than a certain threshold, it is considered that the three-dimensional points are consistent in other views and accept the three-dimensional points. The unaccepted three-dimensional points will be removed. This operation can effectively remove outliers, but cannot remove duplicate points. To further remove redundant points to save memory consumption, project the accepted pixel points onto other depth maps to remove duplicate points. After obtaining a dense point cloud with better quality, transfer it to the point cloud fusion module.

[0117] Step 5: After each drone obtains the dense point cloud map through Step 4, according to the relative poses of each drone, the dense point clouds generated by each drone are transformed into the same coordinate system, here it is transformed into the coordinate system of the central drone. For a point in the point cloud The transformation to the central drone coordinate system is

[0118] The key technology in this step is point cloud fusion:

[0119] In the point cloud fusion part, the dense three-dimensional point cloud maps constructed by each drone and the real-time relative poses between the drones are respectively received. To ensure the real-time performance of dense reconstruction, point cloud fusion is performed relying on the relative positioning between each drone. For any point in the dense point cloud reconstructed by the k-th drone Fusing it into the point cloud map constructed by the central drone, there is Due to problems such as perspective overlap in the fused dense point cloud map, there will be a stacking phenomenon, which greatly increases the memory consumption and the computational complexity of point cloud processing. To eliminate the stacking, a voxelization method is used to downsample the point cloud map, which can suppress the scale of the point cloud to a certain extent.

[0120] The method proposed by the present invention is applicable to large-scale scenarios. To facilitate writing the point cloud in the "inactive" part to achieve the purpose of further saving memory, two-dimensional point cloud position segmentation is performed using the positioning information, and the point clouds are stored in different point cloud blocks respectively. The index of the point cloud block is found relying on the following formula:

[0121] x = P x / d cell

[0122] y = P y / d cell

[0123] Where P x , P x is the position of the coordinate of the point cloud on the x, y plane in the central coordinate system. d cell is the width of the point cloud block. By storing the point cloud information in this way, not only can memory be saved, but also the scale of the point cloud can be effectively limited in operations such as point cloud voxelization at the back end.

[0124] Step 6: Using the hierarchical point cloud fusion registration algorithm, first perform point cloud registration on the dense point cloud maps constructed by different drones within each mapping subgroup. After registering the point cloud maps in the subgroup, update the relative poses of the drones in each subgroup. Subsequently, perform fusion registration on the point cloud maps constructed and fused by each subgroup, and update the relative poses of the central drones between different subgroups.

[0125] For the hierarchical point cloud fusion registration algorithm here, first, the gravity direction of the point cloud is determined, and then the point cloud is segmented in the gravity direction of the point cloud. For each block of point cloud, each point will be projected onto the same plane perpendicular to the gravity direction. First, rough registration is performed using the line-plane features of the two-dimensional compressed point clouds of different layers. The transformation results after registration are clustered to obtain the most likely transformation result. Using this result, the two dense point clouds are roughly transformed, and then the ICP algorithm is used to further perform fine registration of the dense point clouds in combination with the three-dimensional point information of the corresponding feature points of the image. After fusing the point clouds in the subgroup, the relative poses between the UAVs are updated.

[0126] For the registration between the point clouds generated by the subgroup, the process is similar to the above, except that in order to reduce the computing power consumption, the point cloud is downsampled, and the downsampled point cloud is registered.

[0127] The key technology in this step is point cloud registration:

[0128] After point cloud fusion, more refined point cloud registration is required. The hierarchical point cloud registration method proposed by the present invention first divides the point cloud into layers in the direction perpendicular to gravity, and then compresses it into the corresponding plane along the gravity direction. Using the method of line-plane features to register the two-dimensional point cloud map, a relatively good rough registration result can be obtained. For the line features of the two-dimensional point cloud image, the Hough line detection algorithm is directly used. For the plane features, the curvature information of the points is used to obtain. Subsequently, based on this registration result, an improved ICP method is used for more refined registration.

[0129] For the determination of the gravity direction in the above steps, there are two methods. In the case of having IMU information, the direction of the static measurement value of the IMU can be used as the gravity direction. In the case of lacking IMU information, it will be more difficult to determine the gravity direction. At this time, relying on GNSS information to obtain the gravity direction. The specific method is to first transform the GNSS information into the displacement in the ENU coordinate system. Using the relationship between the ENU coordinate system and the camera coordinate system, the Z-axis (gravity direction) in the GNSS coordinate system is transformed into the camera coordinate system to obtain the gravity direction. When both IMU and GNSS information are missing or the information obtained by IMU and GNSS is unreliable, it will be more difficult to find the gravity direction.

[0130] The present invention proposes a calculation of the "pseudo-gravity" direction relying only on point cloud information. First, the curvature of two groups of point clouds is calculated respectively, and the normal direction of the direction with the most plane features is selected as the gravity direction. After obtaining the gravity direction, the above steps are carried out.

[0131] In the improved ICP method of the present invention, the main improvement point is to assign a greater weight to the three-dimensional points restored from the matched image features. The point pairs obtained by using the image feature matching algorithm are generally more reliable than the nearest neighbor points found by the traditional ICP method.

[0132] Step 7: After completing the precise registration, use the registration information to update the relative pose of each drone, and use the relative pose information to determine whether it is necessary to re-divide the sub-groups. Here, it is mainly to judge whether there is a co-visibility relationship between the collected pictures according to the relative pose to determine whether it is necessary to re-divide the sub-groups, so that the drones in the sub-group can obtain the best co-visibility relationship. If it is necessary to re-divide the sub-groups, return to Step 1 and use these relative poses to re-divide and initialize the dense reconstruction sub-groups.

[0133] Although the embodiments of the present invention have been shown and described above, it can be understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those of ordinary skill in the art can make changes, modifications, substitutions, and variations to the above embodiments within the scope of the present invention without departing from the principles and purposes of the present invention.

Claims

1. A method for real-time dense reconstruction of a swarm of drones, characterized in that: It includes the following steps: Step 1: After multiple drones in the swarm of drones receive a three-dimensional dense reconstruction task and enter the flight path, they individually initialize VIO and perform autonomous positioning to obtain the VIO pose estimation information of each drone; GNSS, visual sensors, and IMUs are carried on the drones; The co-visibility relationship between the drones is deduced based on the GNSS information of each drone, and the drones are divided into mapping subgroups according to the co-visibility relationship; each mapping subgroup contains several drones with a co-visibility relationship; Subsequently, the relative poses of the drones in the mapping subgroup are initialized based on the GNSS information, and the central drone is determined using the adjacency relationship; Step 2: According to the VIO pose estimation information of each drone obtained in Step 1, the global pose is optimized by combining the GNSS information, the scale of the local positioning information and the position of the global coordinates are updated in real time, and the localized information with the restored scale is aligned with the image timestamp; and using the initial coordinates of each drone, the relative poses between the drones in each subgroup are updated; Step 3: Detect the overlap rate of the images obtained by each drone in the mapping subgroup. If the overlap rate meets the set threshold requirements, the relative pose result optimized by combining with the GNSS information is used as the initial value, and the common observations are used as constraints to further optimize the relative poses between the drones; Step 4: Using the VIO pose estimation information of a single drone, the relative poses between the drones and the image information after being processed in Step 3, perform dense three-dimensional reconstruction to generate a dense three-dimensional point cloud map; Step 5: After each drone obtains the dense point cloud map through Step 4, according to the relative poses of each drone, the dense point clouds generated by each drone are transformed into the same coordinate system to achieve point cloud fusion; Step 6: Using a hierarchical point cloud fusion registration algorithm, first perform point cloud registration on the dense point cloud maps constructed by different drones within each mapping subgroup; after registering the point cloud maps in the subgroup, update the relative poses between the drones in each subgroup; subsequently, perform fusion registration on the point cloud maps constructed and fused in each subgroup, and update the relative poses of the central drones between different subgroups; Step 7: After completing the precise registration, use the registration information to update the relative poses of each drone, and use the relative pose information to determine whether it is necessary to re-divide the subgroups; If it is necessary to re-divide the subgroups, return to Step 1 and use these relative poses to re-divide and initialize the dense reconstruction subgroups.

2. The method for real-time dense reconstruction of a swarm of drones according to claim 1, characterized in that: The process of initializing VIO and performing autonomous positioning in Step 1 is as follows: First, align the initial coordinate system of VIO with gravity using the IMU information on the aircraft, and solve the attitude of the drone at this time using the transformation between the initial static IMU information and the gravity vector; Then, multiple frames of image information are formed into a sliding window for local batch optimization. Perform pure-vision SfM in the sliding window to recover the unscaled camera trajectory, and use the IMU information integration to obtain the IMU trajectory. Finally, align the camera trajectory and the IMU trajectory.

3. The method for real-time dense reconstruction of a cluster of unmanned aerial vehicles according to claim 1, characterized in that: The specific process of step 2 is as follows: First, obtain the local pose of the VIO of the unmanned aerial vehicle and the GNSS information, and align their timestamps; then use the local pose of the VIO and the GNSS information to construct a global optimization problem to minimize the residuals constructed by the two with the global pose, where the variables to be optimized include the global pose of the unmanned aerial vehicle and the global scale of the VIO estimated pose; After that, solve the optimization problem and obtain the transformation between the local coordinate system and the global coordinate system through calculation; Convert the VIO estimated pose after scale recovery to the camera pose using the extrinsic matrix, and align the timestamps of the camera pose and the received pictures.

4. The method for real-time dense reconstruction of a cluster of unmanned aerial vehicles according to claim 1, characterized in that: The specific method for calculating the overlap rate of the images collected by two unmanned aerial vehicles at the same moment in step 3 is to calculate the homography matrix through the matching points of the images, so as to recover the transformation between the two cameras, and then transform the vertices of one image to the other image, and calculate the ratio of the area of the polygon intersection to the image.

5. The method for real-time dense reconstruction of a cluster of unmanned aerial vehicles according to claim 1 or 4, characterized in that: In step 3, compare the overlap rates of the images collected by the unmanned aerial vehicles in the same subgroup. If the overlap rate meets the set threshold requirements, further optimize the relative pose between the cameras using the co-visibility information: construct a Sim3 problem, and then optimize it jointly with the relative pose obtained in step 2; and use the coordinates of three pairs of well-matched points in the two coordinate systems to recover the transformation and scale between the two coordinate systems.

6. The method for real-time dense reconstruction of a cluster of unmanned aerial vehicles according to claim 1, characterized in that: In step 4, use the dense stereo vision matching algorithm to perform fast stereo matching on adjacent frames and frames at the same moment with the overlap rate within a set range to obtain a depth map; then filter the obtained depth map and perform continuous image consistency check to obtain a depth map with higher accuracy; after the consistency check, remove the redundant points in the adjacent frames to generate a dense point cloud map.

7. The method for real-time dense reconstruction of a cluster of unmanned aerial vehicles according to claim 1, characterized in that: In step 5, respectively receive the dense three-dimensional point cloud maps constructed by each unmanned aerial vehicle and the real-time relative poses between the unmanned aerial vehicles, and perform point cloud fusion depending on the relative positioning between the unmanned aerial vehicles: For any point in the dense point cloud reconstructed by the k-th UAV Fuse it into the point cloud map constructed by the central UAV, and there is Use the voxelization method to downsample the fused dense point cloud map.

8. The method for real-time dense reconstruction of a cluster of unmanned aerial vehicles according to claim 7, characterized in that: In step 5, use the positioning information to perform two-dimensional point cloud position segmentation, and store the point clouds into different point cloud blocks respectively; Rely on the following formula to find the index of the point cloud block: x = P x / d cell y = P y / d cell where P x , P x is the position of the coordinates of the point cloud on the x and y planes in the central coordinate system; d cell is the width of the point cloud block.

9. The method for real-time dense reconstruction of a cluster of unmanned aerial vehicles according to claim 1, characterized in that: In step 6, the hierarchical point cloud fusion registration algorithm is as follows: First, determine the gravity direction of the point cloud, and then divide the point cloud into blocks along the gravity direction of the point cloud; for each block of point cloud, each point is projected onto the same plane perpendicular to the gravity direction. First, use the line-plane features of the two-dimensional compressed point clouds in different layers for rough registration, cluster the transformation results after registration to obtain the most likely transformation result; use this result to perform a rough transformation on the two dense point clouds, and then use the ICP algorithm combined with the three-dimensional point information of the corresponding feature points of the image to further perform fine registration of the dense point clouds.

10. The method for real-time dense reconstruction of a cluster of unmanned aerial vehicles according to claim 9, characterized in that: In step 6, when the IMU and GNSS information is missing or the information obtained by the IMU and GNSS is unreliable, rely on the point cloud information to calculate the gravity direction: First, calculate the curvature of the two groups of point clouds respectively, and select the normal direction of the direction with the most surface features as the gravity direction.

Images