Rapid three-dimensional reconstruction method for swarm devices

Abstract

A rapid three-dimensional reconstruction method for swarm devices. The method is applied to a three-dimensional map construction device, and comprises: receiving data signals originally input by a lidar, and sending the data signals to a teammate detection and temporary tracking module; the teammate detection and temporary tracking module performing reflectivity filtering and clustering extraction on the data signals to detect potential teammates, and performing temporary tracking on the potential teammates by means of a Kalman filter to obtain temporary tracking information (S101); and sending the obtained temporary tracking information to a teammate identification module; the teammate identification module matching the temporary tracking information against teammate trajectories to identify the potential teammates (S102).

Description

Clustered device fast three-dimensional reconstruction method

[0001] The present application claims priority to the Chinese patent application No. 202411097960.4 filed on August 9, 2024 with the China Patent Office, the content of which is incorporated herein by reference in its entirety. TECHNICAL FIELD

[0002] The present application relates to the technical field of clustered device navigation and three-dimensional map construction, for example, to a clustered device fast three-dimensional reconstruction method. BACKGROUND

[0003] With the rapid development of multi-robot system clustering technology, the cluster system has shown great potential in autonomous detection, target tracking, search and rescue, etc. In order to complete these tasks, accurate self-positioning and relative state estimation are crucial. Related technologies mainly focus on state estimation and navigation of a single device, but in a clustered environment, each device not only needs to perform high-precision self-positioning, but also needs to estimate the state of other devices to achieve collaborative tasks.

[0004] Existing cluster state estimation methods mainly rely on Global Positioning System (GPS), Visual-Inertial Odometry (VIO), or LiDAR-Inertial Odometry (LIO), but these methods have many shortcomings in complex environments. For example, GPS cannot be used in indoor or signal blocked environments, visual methods perform poorly in poor lighting or lack of texture, and existing LIO methods are computationally heavy and rely on centralized architecture, which is prone to single point failure problems. In addition, most existing methods require expensive and cumbersome external devices such as Real-Time Kinematic Global Positioning System (RTK-GPS), motion capture systems, etc., increasing deployment costs and complexity. SUMMARY

[0005] The present application provides a clustered device fast three-dimensional reconstruction method, which has strong anti-interference ability in communication and cooperation between devices in the cluster system, and through robust mutual observation and state estimation methods, as well as unmanned aerial vehicle detection, identification and tracking of 3D LiDAR, accurate state estimation in complex and dynamic environments is ensured.

[0006] The present application provides a clustered device fast three-dimensional reconstruction method, applied to a three-dimensional map construction device, comprising:

[0007] receiving a data signal of a laser radar raw input, and sending the data signal to a teammate detection and temporary tracking module, the teammate detection and temporary tracking module performing reflectivity filtering and cluster extraction on the data signal to detect potential teammates, and temporarily tracking the potential teammates through a Kalman filter to obtain temporary tracking information;

[0008] sending the obtained temporary tracking information to a teammate identification module, the teammate identification module matching the temporary tracking information with a trajectory of a teammate to identify the potential teammates;

[0009] after identifying the potential teammates, sending an identification completion state signal to a teammate tracking module, after detecting and identifying the potential teammates as corresponding teammates, the teammate tracking module performing state prediction and update using speed information of the teammates and a latest global external transformation;

[0010] the teammate tracking module sending a state instruction to a state prediction module after the state prediction and update are completed, the state prediction module performing state prediction using an ESIKF framework to obtain state prediction data, and sending the state prediction data to a self device;

[0011] the state prediction module also receiving raw input data of an IMU and performing state prediction, after the state prediction is completed, sending the raw input data of the IMU to an error state iterative update module, the error state iterative update module performing motion compensation and calculating a distance from a feature point in the raw input data of the IMU to a plane to generate a point cloud residual, and performing data transmission with other devices.

[0012] the teammate detection and temporary tracking module performing reflectivity filtering and cluster extraction on the data signal to detect potential teammates, and temporarily tracking the potential teammates through a Kalman filter to obtain temporary tracking information, including:

[0013] extracting high reflectivity points from a new LiDAR scan, and identifying potential teammates when reflectivity values of the high reflectivity points exceed a preset threshold;

[0014] performing Euclidean clustering on the high reflectivity points to detect the potential teammates;

[0015] temporarily tracking the detected potential teammates through a Kalman filter, predicting positions of the detected potential teammates, and performing state prediction on the detected potential teammates based on a constant speed model.

[0016] identifying potential teammates when reflectivity values of the high reflectivity points exceed a preset threshold, including:

[0017] acquiring reflectivity values of the high reflectivity points extracted in the new LiDAR scan;

[0018] placing the acquired plurality of reflectivity values in a data recognition model, the data recognition model ordering the plurality of reflectivity values in ascending order;

[0019] acquiring a reflectivity threshold value preset by the cluster system, and directly embedding an endpoint value of the reflectivity threshold value into the data recognition model, and acquiring a set of reflectivity values outside the data recognition model based on the reflectivity threshold value.

[0020] The temporary tracking information includes a trajectory of the potential teammate, and the teammate identification module matches the temporary tracking information with the trajectory of the teammate to identify the potential teammate, including:

[0021] accumulating the trajectory of the potential teammate of the temporary tracker in the teammate detection and temporary tracking module;

[0022] matching the trajectory of the teammate transmission with the trajectory of the potential teammate, and in response to a matching error being less than a given threshold value, the potential teammate is the corresponding teammate, and the temporary tracker is converted into a teammate tracker.

[0023] The matching of the trajectory of the teammate transmission with the trajectory of the potential teammate, in response to a matching error being less than a given threshold value, the potential teammate is the corresponding teammate, including:

[0024] acquiring a given threshold value identification model;

[0025] acquiring a trajectory of a teammate transmission, inputting the trajectory of the teammate transmission into the given threshold value identification model, and the given threshold value identification model determining a matching error between the trajectory of the teammate transmission and the potential teammate.

[0026] After detecting and identifying the potential teammate as the corresponding teammate, the teammate tracking module uses the speed information of the teammate and the latest global external transformation to perform state prediction and update, including:

[0027] using the speed information of the teammate transmission and the latest global external transformation to perform state prediction;

[0028] When no teammate observation is available, using teammate odometry data received from the network to perform state update.

[0029] The state prediction module uses an ESIKF framework to perform state prediction to obtain state prediction data, and the state prediction equation of the ESIKF framework includes position, attitude, velocity, bias, and gravity vector.

[0030] The state prediction module uses an ESIKF framework to perform state prediction to obtain state prediction data, and sends the state prediction data to the ego device, including:

[0031] The state prediction equation predicts the motion state of the ego device using the raw input data of the IMU.

[0032] Upon receiving new measurements of the IMU, state propagation is performed using a discrete state transition model.

[0033] The error state iterative update module performs motion compensation and calculates the distance from the feature points in the raw input data of the IMU to the plane to generate point cloud residuals at the end of each new LiDAR scan, while constructing new constraints using mutual observation measurements of the 3D LiDAR.

[0034] The error state iterative update module performs motion compensation and calculates the distance from the feature points in the raw input data of the IMU to the plane to generate point cloud residuals at the end of each new LiDAR scan, while constructing new constraints using mutual observation measurements of the 3D LiDAR, including:

[0035] Calculate the displacement of each feature point relative to the time at the end of each new LiDAR scan, and correct the position of each feature point according to the displacement;

[0036] Construct new constraints using active and passive observation measurements. BRIEF DESCRIPTION OF DRAWINGS

[0037] Figure 1 is a flowchart of a cluster device fast three-dimensional reconstruction method according to the present application;

[0038] Figure 2 is a flowchart of the teammate identification module identifying teammates by matching with the trajectories of teammates in a cluster device fast three-dimensional reconstruction method according to the present application;

[0039] Figure 3 is a system connection diagram of a three-dimensional map construction device in a cluster device fast three-dimensional reconstruction method according to the present application;

[0040] Figure 4 is a connection diagram of a three-dimensional map construction device and an external device in a cluster device fast three-dimensional reconstruction method according to the present application. DETAILED DESCRIPTION

[0041] The present application will be described below in conjunction with the accompanying drawings and examples. It should be understood that the examples described herein are only used to explain the present application and do not limit the present application.

[0042] The implementation of the present application will be described below in conjunction with the examples.

[0043] The same or similar reference numerals in the drawings of the embodiments correspond to the same or similar components; in the description of the present application, it is understood that if the orientations or positional relationships indicated by the terms "upper", "lower", "left", "right" and the like are based on the orientations or positional relationships shown in the drawings, they are only for the convenience of describing the present application and simplifying the description, and do not indicate or imply that the devices or elements referred to must have a particular orientation, be constructed and operated in a particular orientation, therefore the terms describing the positional relationship in the drawings are only used for exemplary illustration, and cannot be understood as a limitation of the present application, for those skilled in the art, the meaning of the above terms can be understood according to the situation.

[0044] Referring to FIGS. 1-4, a cluster-based fast three-dimensional reconstruction method is applied to a three-dimensional map construction device, comprising the following steps:

[0045] S101: receiving a data signal of a laser radar original input, and sending the data signal to a teammate detection and temporary tracking module, the teammate detection and temporary tracking module performing reflectivity filtering and cluster extraction on the data signal to detect potential teammates, and performing temporary tracking on the potential teammates through a Kalman filter to obtain temporary tracking information.

[0046] The teammate detection and temporary tracking module performs reflectivity filtering and cluster extraction on the data signal to detect potential teammates, and performs temporary tracking on the potential teammates through a Kalman filter to obtain temporary tracking information, including:

[0047] Extracting high reflectivity points from a new radar LiDAR scan, and identifying potential teammates when the reflectivity value of the high reflectivity points exceeds a preset threshold;

[0048] Performing Euclidean clustering on the high reflectivity points to detect potential teammates;

[0049] Temporarily tracking the detected potential teammates through a Kalman filter, predicting the position of the detected potential teammates, and predicting the state of the detected potential teammates based on a constant speed model.

[0050] In this embodiment, the basic idea of Euclidean clustering is to determine the similarity between data points by calculating their Euclidean distance, which is one of the most common distance measures, measuring the straight-line distance between two points in a multi-dimensional space. In the clustering process, the algorithm calculates the Euclidean distance between each data point and other data points, and groups these points into different clusters according to the set threshold or a similarity measure. In addition, conditional Euclidean clustering is an extension of Euclidean clustering, which allows users to customize the conditions of clustering. This clustering method not only considers the Euclidean distance between data points, but also allows users to define additional conditions (such as smoothness, color, etc.), which can further constrain the clustering process, making the clustering results more consistent with specific needs or application scenarios. Conditional Euclidean clustering is particularly useful when dealing with clustering problems with specific constraints or requirements.

[0051] When the reflectivity value of the high reflectivity point exceeds the preset threshold, identify potential teammates, including:

[0052] Obtain the reflectivity value of the high reflectivity point extracted in the new LiDAR scan;

[0053] Place the obtained multiple reflectivity values in the data recognition model, and the data recognition model sorts the multiple reflectivity values in order from small to large;

[0054] Obtain the reflectivity threshold preset by the cluster system, and directly embed the endpoint value of the reflectivity threshold into the data recognition model, and obtain the reflectivity value set outside the data recognition model based on the reflectivity threshold.

[0055] S102: Send the obtained temporary tracking information to the teammate identification module, and the teammate identification module matches the temporary tracking information with the trajectory of the teammate to identify potential teammates.

[0056] The temporary tracking information includes the trajectory of the potential teammate, and the teammate identification module matches the temporary tracking information with the trajectory of the teammate to identify potential teammates, including:

[0057] Trajectory accumulation: accumulate the trajectory of the potential teammate in the teammate detection and temporary tracking module;

[0058] Trajectory matching: match the trajectory of the potential teammate with the trajectory of the teammate transmitted, and in response to the matching error being less than a given threshold, the potential teammate is the corresponding teammate, and the temporary tracker is converted into a teammate tracker.

[0059] S103: After matching and identifying potential teammates, send an identification completion state signal to the teammate tracking module. After detecting and identifying potential teammates as corresponding teammates, the teammate tracking module uses the speed information of the teammates and the latest global external transformation to perform state prediction and update.

[0060] After detecting and identifying potential teammates, the teammate tracking module uses the speed information of the teammates and the latest global external transformation to perform state prediction and update, including:

[0061] State prediction: using the speed information transmitted by the teammates and the latest global external transformation to perform state prediction;

[0062] State update: when no teammate observation is available, use the teammate odometer data received from the network to perform state update.

[0063] S104: After the state prediction and update are completed, the teammate tracking module sends a state instruction to the state prediction module. The state prediction module uses the ESIKF framework to perform state prediction to obtain state prediction data, and sends the state prediction data to the device itself.

[0064] The state prediction module uses the Error-state Iterated Kalman Filter (ESIKF) framework to complete state prediction. The state prediction equation of the ESIKF framework includes position, attitude, velocity, bias, and gravity vector.

[0065] In this embodiment, the state prediction module uses the ESIKF framework to perform state prediction to obtain state prediction data, and sends the state prediction data to the device itself, including:

[0066] State prediction equation: using the raw input data of the Inertial Measurement Unit (IMU) to predict the motion state of the device itself;

[0067] State propagation: when receiving new IMU measurements, use the discrete state transition model to perform state propagation.

[0068] S105: The state prediction module also receives the raw input data of the IMU and performs state prediction. After the state prediction is completed, the raw input data of the IMU is sent to the error state iterative update module. The error state iterative update module performs motion compensation and calculates the distance from the feature point to the plane in the raw input data of the IMU to generate a point cloud residual, and transmits data with other devices. Through the cooperation between multiple devices, decentralized data processing is realized, the problem of single point failure is avoided, and the environmental adaptability is strong. In a complex environment (such as a narrow space or a large area with changing light), features are not easily lost, resulting in a complete map constructed and quite accurate positioning. By using the raw input data of the LiDAR and the IMU, the robustness of feature recognition and state estimation in various environments is improved.

[0069] In S102 of the embodiment, the trajectory transmitted by the teammate is matched with the trajectory of the potential teammate. If the matching error is less than a given threshold, the potential teammate is the corresponding teammate, including:

[0070] Obtain a given threshold identification model;

[0071] Obtain the trajectory transmitted by the teammate, input the trajectory transmitted by the teammate into the given threshold identification model, and the given threshold identification model determines the matching error between the trajectory transmitted by the teammate and the potential teammate.

[0072] In S105 of the embodiment, the error state iterative update module performs motion compensation and calculates the distance from the feature point to the plane in the raw input data of the IMU to generate a point cloud residual at the end of each new LiDAR scan, and constructs new constraints using mutual observation measurements of the 3D LiDAR to improve the accuracy and robustness of state estimation,

[0073] In the embodiment, the error state iterative update module performs motion compensation and calculates the distance from the feature point to the plane in the raw input data of the IMU to generate a point cloud residual at the end of each new LiDAR scan, and constructs new constraints using mutual observation measurements of the 3D LiDAR, including:

[0074] Point cloud residual calculation: calculate the displacement of each feature point relative to the time when each new LiDAR scan ends, and correct the position of each feature point according to the displacement;

[0075] Mutual observation measurement: new constraints are constructed using active and passive observation measurements to improve the accuracy of state estimation.

[0076] The technical scheme realizes decentralized data processing through cooperation between multiple devices, avoids single point failure problems, and has strong environmental adaptability, and is not easy to lose features in complex environments (such as narrow spaces and areas with large changes in illumination), so that the constructed map is complete and positioning is quite accurate, the original input data of LiDAR and IMU are used to improve the robustness of feature recognition and state estimation in various environments, and mutual interference can be avoided: the communication and cooperation between devices in the cluster system have strong anti-interference ability, and through robust mutual observation and state estimation methods, as well as unmanned aerial vehicle detection, identification and tracking of 3D LiDAR, accurate state estimation in complex and dynamic environments is ensured, and the influence of mutual interference on system performance is effectively reduced.

[0077] In the embodiment, robustness refers to the ability of a system to survive in abnormal and dangerous situations, for example, whether computer software can not crash and not crash in the case of input errors, disk failures, network overloads or intentional attacks. The robustness of the software is the robustness of the software. The so-called "robustness" also refers to the characteristics of a control system to maintain other performances under certain (structure, size) parameter perturbations. According to different definitions of performance, it can be divided into stability robustness and performance robustness.

[0078] The technical scheme relates to the technical field of cluster device navigation and three-dimensional map construction, for example, a cluster type rapid three-dimensional reconstruction method combining laser radar and inertial measurement unit (IMU) and a device thereof, mainly applied to cooperative detection, target tracking, search and rescue and other tasks of cluster devices such as unmanned aerial vehicles and robots in complex environments.

[0079] In the embodiment, the entire operation process can be controlled by a computer to realize automatic operation control, and in multiple operation links, signal feedback can be performed through the setting of sensors to realize the sequential performance of steps. These are all conventional knowledge of automatic control, and will not be described one by one in the embodiment.

Claims

1. A method for fast 3D reconstruction of a swarm of devices, applied to a 3D map construction device, comprising: receiving a data signal of a raw input of a laser radar, and sending the data signal to a teammate detection and temporary tracking module, the teammate detection and temporary tracking module performing reflectivity filtering and cluster extraction on the data signal to detect potential teammates, and performing temporary tracking on the potential teammates by a Kalman filter to obtain temporary tracking information; sending the obtained temporary tracking information to a teammate identification module, the teammate identification module matching the temporary tracking information with a trajectory of a teammate to identify the potential teammates; after the identification of the potential teammates is completed by matching, sending an identification completion state signal to a teammate tracking module, after detecting and identifying the potential teammates as corresponding teammates, the teammate tracking module performing state prediction and update using speed information of the teammates and a latest global external transformation; after the state prediction and update are completed, the teammate tracking module sends a state instruction to a state prediction module, the state prediction module performing state prediction using an error state iterative Kalman filter (ESIKF) framework to obtain state prediction data, and sending the state prediction data to a device of the state prediction module; the state prediction module also receives raw input data of an inertial measurement unit (IMU) and performs state prediction, after the state prediction is completed, the raw input data of the IMU is sent to an error state iterative update module, the error state iterative update module performs motion compensation and calculates a distance from a feature point in the raw input data of the IMU to a plane to generate a point cloud residual, and transmits data with other devices.

2. The cluster-based rapid 3D reconstruction method of claim 1, wherein, the teammate detection and temporary tracking module performs reflectivity filtering and cluster extraction on the data signal to detect potential teammates, and performs temporary tracking on the potential teammates by a Kalman filter to obtain temporary tracking information, comprising: extracting high reflectivity points from a new LiDAR scan, when reflectivity values of the high reflectivity points exceed a preset threshold, identifying the potential teammates; performing Euclidean clustering on the high reflectivity points to detect potential teammates; performing temporary tracking on the detected potential teammates by a Kalman filter, predicting positions of the detected potential teammates, and performing state prediction on the detected potential teammates based on a constant speed model.

3. The cluster-based rapid 3D reconstruction method of claim 2, wherein, when reflectivity values of the high reflectivity points exceed a preset threshold, identifying potential teammates, comprising: obtaining reflectivity values of the high reflectivity points extracted from a new LiDAR scan; placing the obtained multiple reflectivity values in a data identification model, the data identification model sorting the multiple reflectivity values in order from small to large; obtaining a reflectivity threshold preset by a swarm system, and directly embedding endpoint values of the reflectivity threshold into the data identification model, and obtaining a set of reflectivity values outside the data identification model based on the reflectivity threshold.

4. The cluster-based rapid 3D reconstruction method of claim 3, wherein, The temporary tracking information includes a trajectory of the potential teammate, and the teammate identification module matches the temporary tracking information with a trajectory of a teammate to identify the potential teammate, including: accumulating the trajectory of the potential teammate of the temporary tracker in the teammate detection and temporary tracking module; matching a trajectory of a teammate transmission with the trajectory of the potential teammate, and in response to a matching error being less than a given threshold, the potential teammate being a corresponding teammate, and converting the temporary tracker into a teammate tracker.

5. The cluster-based rapid 3D reconstruction method of claim 4, wherein, The matching of the trajectory of the teammate transmission with the trajectory of the potential teammate, and in response to a matching error being less than a given threshold, the potential teammate being a corresponding teammate, includes: obtaining a given threshold identification model; obtaining a trajectory of a teammate transmission, and inputting the trajectory of the teammate transmission into the given threshold identification model, and the given threshold identification model performing a matching error determination between the trajectory of the teammate transmission and the potential teammate.

6. The cluster-based rapid 3D reconstruction method of claim 5, wherein, After detecting and identifying the potential teammate as a corresponding teammate, the teammate tracking module uses speed information of the teammate and a latest global external transformation to perform state prediction and update, including: using the speed information of the teammate transmission and the latest global external transformation to perform state prediction; when no teammate observation is available, using teammate odometry data received from a network to perform state update.

7. The cluster-based rapid 3D reconstruction method of claim 6, wherein, The state prediction module uses an ESIKF framework to perform state prediction to obtain state prediction data, and a state prediction equation of the ESIKF framework includes position, attitude, velocity, bias, and gravity vector.

8. The cluster-based rapid 3D reconstruction method of claim 7, wherein, The state prediction module uses an ESIKF framework to perform state prediction to obtain state prediction data, and sends the state prediction data to a self device, including: The state prediction equation uses raw input data of the IMU to predict a motion state of the self device; when receiving new measurements of the IMU, using a discrete state transition model to perform state propagation.

9. The cluster-based rapid 3D reconstruction method of claim 8, wherein, The error state iterative update module, at the end of each new LiDAR scan, performs motion compensation and calculates a distance from a feature point in the raw input data of the IMU to a plane to generate a point cloud residual error, and simultaneously constructs a new constraint using mutual observation measurements of a three-dimensional radar 3D LiDAR.

10. The cluster-based rapid 3D reconstruction method of claim 9, wherein, The error state iterative update module, at the end of each new LiDAR scan, performs motion compensation and calculates a distance from a feature point in the raw input data of the IMU to a plane to generate a point cloud residual error, and simultaneously constructs a new constraint using mutual observation measurements of a three-dimensional radar 3D LiDAR, including: calculating a displacement of each feature point relative to a time at the end of each new LiDAR scan, and correcting a position of the each feature point according to the displacement; constructing a new constraint using active and passive observation measurements.

Images