Rapid three-dimensional reconstruction method for cluster-type equipment

Through communication and collaboration between devices in the cluster system, combined with 3D LiDAR and IMU data, decentralized state estimation and map construction are realized, solving the problem of insufficient state estimation in complex environments in the existing technology and improving the environment Adaptability and robustness ensure accurate state estimation and map construction in complex environments.

CN118980370APending Publication Date: 2024-11-19SHENZHEN LIUXING TECHNOLOGY LTD
View PDF 0 Cites 1 Cited by

Patent Information

Application Number
CN202411097960.4
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-08-09
Publication Date
2024-11-19

Smart Images

  • Figure CN118980370A_ABST
    Figure CN118980370A_ABST
Patent Text Reader

Abstract

The invention relates to the technical field of cluster equipment navigation and three-dimensional map construction, and discloses a cluster equipment rapid three-dimensional reconstruction method, which is applied to three-dimensional map construction equipment, and specifically comprises the following steps: S101, receiving a data signal originally input by a laser radar, and sending the data signal to a teammate detection and temporary tracking module, the teammate detection and temporary tracking module detects potential teammates through reflectivity filtering and clustering extraction, and performs temporary tracking through a Kalman filter; s102, the obtained temporary tracking information is sent to a teammate recognition module, and the teammate recognition module recognizes the teammate by matching with the track of the teammate. According to the method, the state is accurate or fails due to the fact that the anti-interference capacity of communication and cooperation between devices in a cluster system is high, and accurate state estimation in a complex and dynamic environment is ensured through a robust mutual observation and state estimation method and through detection, recognition and tracking of the 3D LiDAR unmanned aerial vehicle.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the technical field of cluster equipment navigation and three-dimensional map construction, and in particular to a method for rapid three-dimensional reconstruction of cluster equipment. Background Art

[0002] With the rapid development of multi-robot system swarm technology, swarm systems have 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. Existing technologies mainly focus on the state estimation and navigation of a single device, but in a swarm 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.

[0003] Current cluster state estimation methods mainly rely on 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 indoors or in signal-blocked environments, and visual methods have significantly reduced performance in poor lighting or lack of texture. Existing LIO methods are computationally heavy and rely on centralized architectures, which are prone to single-point failures. In addition, most existing methods require expensive and cumbersome external equipment, such as RTK-GPS, motion capture systems, etc., which increases deployment costs and complexity. Summary of the invention

[0004] The object of the present invention is to provide a method for rapid three-dimensional reconstruction of clustered equipment, which has strong anti-interference ability through communication and collaboration between equipment in the cluster system, resulting in accurate state or failure, and ensures accurate state estimation in complex and dynamic environments through robust mutual observation and state estimation methods, drone detection, identification and tracking through 3D LiDAR, aiming to solve the problems in the prior art.

[0005] The present invention is implemented as follows: a cluster device fast three-dimensional reconstruction method, applied to a three-dimensional map building device, specifically comprising the following steps:

[0006] S101: receiving data signals originally input by the laser radar and sending them to a teammate detection and temporary tracking module, wherein the teammate detection and temporary tracking module detects potential teammates through reflectivity filtering and clustering extraction, and performs temporary tracking through a Kalman filter;

[0007] S1 02: sending the acquired temporary tracking information to a teammate identification module, wherein the teammate identification module identifies the teammate by matching the temporary tracking information with the teammate's trajectory;

[0008] S103: After matching and identifying teammates, a signal is sent to a teammate tracking module, and after detecting and identifying teammates, the teammate tracking module uses the teammate's speed information and the latest global external transformation to perform state prediction and update;

[0009] S104: the teammate tracking module sends a status instruction to the status prediction module after the prediction and update information are completed. The status prediction module uses the ESIKF framework to complete the status prediction and sends the data to its own device;

[0010] S105: The state prediction module also receives the IMU raw data input data and performs state prediction. After the IMU raw data prediction is completed, it is sent to the error state iterative update module to complete motion compensation and calculate the distance from the point to the plane to generate a point cloud residual, and complete data transmission with other devices.

[0011] Further, in S101, the teammate detection and temporary tracking module detects potential teammates through reflectivity filtering and cluster extraction, and performs temporary tracking through a Kalman filter, including:

[0012] Extract high reflectivity points from the new LiDAR scan, identified by reflectivity values ​​exceeding a preset threshold;

[0013] Euclidean clustering of high reflectivity points to detect new potential teammates;

[0014] The detected objects are tracked using a Kalman filter, their positions are predicted, and their states are predicted based on a constant velocity model.

[0015] Further, identification is performed by the reflectivity value exceeding a preset threshold, including:

[0016] Get the reflectivity values ​​of the extracted high reflectivity points in the new LiDAR scan;

[0017] The obtained multiple reflectivity values ​​are placed in a data recognition model, and the values ​​of the data recognition model are sorted in order from small to large;

[0018] A reflectivity threshold preset by the system is obtained, and the endpoint value of the reflectivity threshold is directly embedded into the data recognition model, so as to obtain a reflectivity value set that is unexpected based on the reflectivity threshold in the data recognition model.

[0019] Further, in S102, the teammate identification module identifies the teammate by matching the teammate's trajectory, including:

[0020] Trajectory accumulation: accumulates the trajectories of temporary trackers;

[0021] Trajectory matching: Use the trajectory transmitted by teammates for matching. If the matching error is less than a given threshold, the object is considered to be the corresponding teammate, and the temporary tracker is transformed into a teammate tracker.

[0022] Furthermore, the trajectory transmitted by the teammate is used for matching. If the matching error is less than a given threshold, the object is considered to be the corresponding teammate, including:

[0023] Get the given threshold recognition model;

[0024] The trajectory transmitted by the teammate is obtained, and the trajectory transmitted by the teammate is input into a fixed threshold recognition model, and the fixed threshold recognition model performs matching error determination on the trajectory transmitted by the current teammate.

[0025] Further, in S103, after detecting and identifying the teammates, the teammate tracking module uses the teammate's speed information and the latest global external transformation to perform state prediction and update, including:

[0026] State prediction: Use the speed information transmitted by teammates and the latest global external transformation to predict the state;

[0027] Status update: When no teammate observations are available, the status is updated using teammate odometry data received from the network.

[0028] Further, in S104, the state prediction module uses the ESIKF framework to complete the state prediction, and the completed state prediction equation includes position, attitude, velocity, bias and gravity vector.

[0029] Furthermore, the state prediction module uses the ESIKF framework to complete state prediction and send data to its own device, including:

[0030] State prediction equation: Use IMU data to predict the motion state of the device;

[0031] State Propagation: When new IMU measurements are received, state propagation is performed using a discrete state transition model.

[0032] Furthermore, in S105, the error state iterative update module performs motion compensation and calculates the distance from the point to the plane to generate a point cloud residual at the end of each new LiDAR scan, and constructs new constraints using the mutual observation measurement of the 3D LiDAR to improve the accuracy and robustness of the state estimation.

[0033] Furthermore, motion compensation is performed and the distance from the point to the plane is calculated to generate the point cloud residual, including:

[0034] Point cloud residual calculation: Calculate the displacement of each feature point relative to the end time of scanning, and correct the position of the feature point according to the displacement;

[0035] Mutual Observations: Leverage active and passive observations to build new constraints and improve the accuracy of state estimation.

[0036] Compared with the prior art, the method for rapid 3D reconstruction of clustered equipment provided by the present invention has the following beneficial effects:

[0037] 1. Through the cooperation between multiple devices, decentralized data processing is achieved, avoiding the problem of single point failure, and it has strong environmental adaptability. It is not easy to lose features in complex environments (such as narrow spaces and areas with large lighting changes), so that the constructed map is completely preserved and the positioning is quite accurate. By combining LiDAR and IMU data, the robustness of feature recognition and state estimation in various environments is improved.

[0038] 2. No mutual interference: The communication and collaboration between devices in the cluster system have strong anti-interference capabilities, resulting in accurate state or failure. Through robust mutual observation and state estimation methods, drone detection, identification and tracking through 3D LiDAR ensure accurate state estimation in complex and dynamic environments, effectively reducing the impact of mutual interference on system performance. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] Figure 1 A flowchart of a cluster equipment rapid 3D reconstruction method proposed by the present invention;

[0040] Figure 2 A flowchart of a teammate identification module in a cluster equipment rapid three-dimensional reconstruction method proposed by the present invention identifying teammates by matching with the teammate's trajectory;

[0041] Figure 3 A schematic diagram of system connections of a 3D map construction device in a cluster-type device rapid 3D reconstruction method proposed by the present invention;

[0042] Figure 4 This is a schematic diagram of the connection between a 3D map building device and external devices in a cluster device fast 3D reconstruction method proposed by the present invention. DETAILED DESCRIPTION

[0043] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0044] The implementation of the present invention is described in detail below in conjunction with specific embodiments.

[0045] The same or similar numbers in the drawings of this embodiment correspond to the same or similar parts; in the description of the present invention, it should be understood that if the terms "upper", "lower", "left", "right" and the like indicate directions or positional relationships based on the directions or positional relationships shown in the drawings, it is only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific direction, be constructed and operated in a specific direction. Therefore, the terms describing the positional relationship in the drawings are only used for illustrative purposes and cannot be understood as limitations on the present invention. For ordinary technicians in this field, the specific meanings of the above terms can be understood according to specific circumstances.

[0046] Reference Figure 1-4 As shown, a cluster device fast 3D reconstruction method is applied to a 3D map building device, specifically comprising the following steps:

[0047] S101: receiving the data signal originally input by the laser radar and sending it to the teammate detection and temporary tracking module. The teammate detection and temporary tracking module detects potential teammates through reflectivity filtering and clustering extraction, and performs temporary tracking through the Kalman filter;

[0048] Among them, the teammate detection and temporary tracking module detects potential teammates through reflectivity filtering and clustering extraction, and performs temporary tracking through the Kalman filter, including:

[0049] Extract high reflectivity points from the new LiDAR scan, identified by reflectivity values ​​exceeding a preset threshold;

[0050] Euclidean clustering of high reflectivity points to detect new potential teammates;

[0051] Perform Kalman filter tracking on detected objects, predict their positions, and make state predictions based on a constant velocity model;

[0052] In this embodiment, the basic idea of ​​Euclidean clustering is to determine the similarity between data points by calculating the Euclidean distance between them. Euclidean distance is one of the most common distance metrics, which measures the straight-line distance between two points in multidimensional space. During 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 a set threshold or some similarity metric. In addition, conditional Euclidean clustering is an extension of Euclidean clustering, which allows users to customize clustering conditions. 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 so that the clustering results are more in line with specific needs or application scenarios. Conditional Euclidean clustering is particularly useful when dealing with clustering problems with specific constraints or requirements.

[0053] Specifically, identification is performed by the reflectivity value exceeding a preset threshold, including:

[0054] Get the reflectivity values ​​of the extracted high reflectivity points in the new LiDAR scan;

[0055] The obtained multiple reflectivity values ​​are all placed in the data recognition model, and the values ​​of the data recognition model are sorted in order from small to large;

[0056] Obtaining a reflectivity threshold preset by the system, and directly embedding the endpoint value of the reflectivity threshold into the data recognition model, and obtaining a reflectivity value set located outside the data recognition model based on the reflectivity threshold;

[0057] S102: Sending the acquired temporary tracking information to a teammate identification module, and the teammate identification module identifies the teammate by matching the temporary tracking information with the teammate's trajectory;

[0058] The teammate identification module identifies teammates by matching their trajectories, including:

[0059] Trajectory accumulation: accumulates the trajectories of temporary trackers;

[0060] Trajectory matching: Use the trajectory transmitted by teammates for matching. If the matching error is less than a given threshold, the object is considered to be the corresponding teammate, and the temporary tracker is transformed into a teammate tracker;

[0061] S103: After matching and identifying teammates, a signal is sent to a teammate tracking module. After detecting and identifying teammates, the teammate tracking module uses the teammate's speed information and the latest global external transformation to predict and update the state;

[0062] Among them, after detecting and identifying teammates, the teammate tracking module uses the teammate's speed information and the latest global external transformation to predict and update the state, including:

[0063] State prediction: Use the speed information transmitted by teammates and the latest global external transformation to predict the state;

[0064] Status update: When no teammate observations are available, the status is updated using teammate odometry data received from the network.

[0065] S104: After the prediction and update information are completed, the teammate tracking module sends a status instruction to the status prediction module. The status prediction module uses the ESIKF framework to complete the status prediction and sends the data to its own device;

[0066] Among them, the state prediction module uses the ESIKF framework to complete the state prediction, and the completed state prediction equation includes position, attitude, velocity, bias and gravity vector;

[0067] In this embodiment, the state prediction module uses the ESIKF framework to complete state prediction and send data to its own device, including:

[0068] State prediction equation: Use IMU data to predict the motion state of the device;

[0069] State Propagation: When new IMU measurements are received, state propagation is performed using a discrete state transition model.

[0070] S105: The state prediction module also receives the IMU raw data input data and performs state prediction. After the IMU raw data prediction is completed, it is sent to the error state iterative update module to complete motion compensation and calculate the distance from the point to the plane to generate the point cloud residual, and complete the data transmission with other devices. Through the cooperation between multiple devices, decentralized data processing is realized, avoiding the problem of single point failure, and it has strong environmental adaptability. It is not easy to lose features in complex environments (such as narrow spaces and areas with large lighting changes), so that the constructed map is completely preserved and the positioning is quite accurate. By combining LiDAR and IMU data, the robustness of feature recognition and state estimation in various environments is improved.

[0071] In S102 of this embodiment, the trajectory transmitted by the teammate is used for matching. If the matching error is less than a given threshold, the object is considered to be the corresponding teammate, including:

[0072] Get the given threshold recognition model;

[0073] The trajectory transmitted by the teammate is obtained, and the trajectory transmitted by the teammate is input into the fixed threshold recognition model, and the fixed threshold recognition model determines the matching error of the trajectory transmitted by the current teammate.

[0074] In S105 of this embodiment, the error state iterative update module performs motion compensation and calculates the distance from the point to the plane to generate a point cloud residual at the end of each new LiDAR scan, and constructs new constraints using the mutual observation measurement of the 3D LiDAR to improve the accuracy and robustness of the state estimation.

[0075] In this embodiment, motion compensation is performed and the distance from the point to the plane is calculated to generate a point cloud residual, including:

[0076] Point cloud residual calculation: Calculate the displacement of each feature point relative to the end time of scanning, and correct the position of the feature point according to the displacement;

[0077] Mutual Observations: Leverage active and passive observations to build new constraints and improve the accuracy of state estimation.

[0078] This technical solution realizes decentralized data processing through cooperation between multiple devices, avoids the problem of single point failure, and has strong environmental adaptability. It is not easy to lose features in complex environments (such as narrow spaces and areas with large changes in lighting), so that the constructed map is completely preserved and the positioning is quite accurate. By combining LiDAR and IMU data, the robustness of feature recognition and state estimation in various environments is improved, and at the same time, non-interference can be achieved: through the strong anti-interference ability of communication and collaboration between devices in the cluster system, the state is accurate or invalid. Through robust mutual observation and state estimation methods, drone detection, identification and tracking of 3D LiDAR are used to ensure accurate state estimation in complex and dynamic environments, effectively reducing the impact of mutual interference on system performance.

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

[0080] The present technical solution relates to the technical field of cluster equipment navigation and three-dimensional map construction, in particular to a cluster-type rapid three-dimensional reconstruction method and equipment combining lidar and inertial measurement unit (IMU), which is mainly used for collaborative detection, target tracking, search and rescue and other tasks of cluster equipment such as drones and robots in complex environments.

[0081] In this embodiment, the entire operation process can be controlled by a computer to achieve automatic operation control, and in each operation link, sensors can be set to provide signal feedback to achieve sequential execution of steps. These are all common knowledge of current automatic control and will not be described in detail in this embodiment.

[0082] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A method for rapid three-dimensional reconstruction of clustered equipment, characterized in that: Applied to a three-dimensional map construction device, specifically comprising the following steps: S101: receiving data signals originally input by the laser radar and sending them to a teammate detection and temporary tracking module, wherein the teammate detection and temporary tracking module detects potential teammates through reflectivity filtering and clustering extraction, and performs temporary tracking through a Kalman filter; S102: sending the acquired temporary tracking information to a teammate identification module, wherein the teammate identification module identifies the teammate by matching the temporary tracking information with the teammate's trajectory; S103: After matching and identifying teammates, a signal is sent to a teammate tracking module, and after detecting and identifying teammates, the teammate tracking module uses the teammate's speed information and the latest global external transformation to perform state prediction and update; S104: the teammate tracking module sends a status instruction to the status prediction module after the prediction and update information are completed. The status prediction module uses the ESIKF framework to complete the status prediction and sends the data to its own device; S105: The state prediction module also receives the IMU raw data input data and performs state prediction. After the IMU raw data prediction is completed, it is sent to the error state iterative update module to complete motion compensation and calculate the distance from the point to the plane to generate a point cloud residual, and complete data transmission with other devices.

2. A method for rapid three-dimensional reconstruction of clustered equipment as claimed in claim 1, characterized in that: In S101, the teammate detection and temporary tracking module detects potential teammates through reflectivity filtering and cluster extraction, and performs temporary tracking through a Kalman filter, including: Extract high reflectivity points from the new LiDAR scan, identified by reflectivity values ​​exceeding a preset threshold; Euclidean clustering of high reflectivity points to detect new potential teammates; The detected objects are tracked using a Kalman filter, their positions are predicted, and their states are predicted based on a constant velocity model.

3. A cluster equipment rapid 3D reconstruction method as claimed in claim 2, characterized in that: Identified by reflectivity values ​​exceeding preset thresholds, including: Get the reflectivity values ​​of the extracted high reflectivity points in the new LiDAR scan; The obtained multiple reflectivity values ​​are placed in a data recognition model, and the values ​​of the data recognition model are sorted in order from small to large; A reflectivity threshold preset by the system is obtained, and the endpoint value of the reflectivity threshold is directly embedded into the data recognition model, so as to obtain a reflectivity value set that is unexpected based on the reflectivity threshold in the data recognition model.

4. A cluster equipment rapid 3D reconstruction method as claimed in claim 3, characterized in that: In S102, the teammate identification module identifies the teammate by matching the teammate's trajectory, including: Trajectory accumulation: accumulates the trajectories of temporary trackers; Trajectory matching: Use the trajectory transmitted by teammates for matching. If the matching error is less than a given threshold, the object is considered to be the corresponding teammate, and the temporary tracker is transformed into a teammate tracker.

5. A method for rapid 3D reconstruction of clustered equipment as claimed in claim 4, characterized in that: Use the trajectory transmitted by the teammate for matching. If the matching error is less than a given threshold, the object is considered to be the corresponding teammate, including: Get the given threshold recognition model; The trajectory transmitted by the teammate is obtained, and the trajectory transmitted by the teammate is input into a fixed threshold recognition model, and the fixed threshold recognition model performs matching error determination on the trajectory transmitted by the current teammate.

6. A method for rapid 3D reconstruction of clustered equipment as claimed in claim 5, characterized in that: In S103, after detecting and identifying teammates, the teammate tracking module uses the teammate's speed information and the latest global external transformation to perform state prediction and update, including: State prediction: Use the speed information transmitted by teammates and the latest global external transformation to predict the state; Status update: When no teammate observations are available, the status is updated using teammate odometry data received from the network.

7. A method for rapid 3D reconstruction of clustered equipment as claimed in claim 6, characterized in that: In S104, the state prediction module uses the ESIKF framework to complete the state prediction, and the completed state prediction equation includes position, attitude, velocity, bias and gravity vector.

8. A cluster equipment rapid 3D reconstruction method as claimed in claim 7, characterized in that: The state prediction module uses the ESIKF framework to complete state prediction and send data to its own device, including: State prediction equation: Use IMU data to predict the motion state of the device; State Propagation: When new IMU measurements are received, state propagation is performed using a discrete state transition model.

9. A method for rapid three-dimensional reconstruction of clustered equipment as claimed in claim 8, characterized in that: In S105, the error state iterative update module performs motion compensation and calculates the distance from the point to the plane to generate a point cloud residual at the end of each new LiDAR scan, and constructs new constraints using the mutual observation measurement of the 3D LiDAR to improve the accuracy and robustness of the state estimation.

10. The method for rapid 3D reconstruction of clustered equipment according to claim 9, characterized in that: Perform motion compensation and calculate the distance from the point to the plane to generate the point cloud residual, including: Point cloud residual calculation: Calculate the displacement of each feature point relative to the end time of scanning, and correct the position of the feature point according to the displacement; Mutual Observations: Leverage active and passive observations to build new constraints and improve the accuracy of state estimation.

Citation Information

Cited By

  • Rapid three-dimensional reconstruction method for swarm devices

    WO2026032106A1