Multi-robot cooperation SLAM method and system based on cross validation and index threshold

Abstract

The invention relates to the technical field of positioning and map construction, in particular to a multi-robot cooperation SLAM method and system based on cross validation and an index threshold, and the method comprises the steps: S1, collecting image data; s2, performing cross validation feature matching processing on the acquired image data to obtain matched feature point pairs; s3, realizing pose tracking based on the matched feature point pair and a previous frame pose predicted value, and obtaining a current frame pose; s4, screening key frames through an index threshold function; s5, constructing a single-machine dense map based on the key frame; s6, performing coarse-to-fine multi-map fusion processing on the single-machine dense map of the multiple robots; and S7, outputting a final global dense map. According to the method, the problems of high mismatching rate of feature matching, unreasonable key frame selection, poor multi-robot map fusion effect, insufficient map precision and density and the like in the existing SLAM technology can be solved, and the positioning precision, the mapping quality and the operation efficiency of an SLAM system are improved.

Description

Technical Field

[0001] This invention relates to the field of localization and mapping technology, and in particular to a multi-robot collaborative SLAM method and system based on cross-validation and exponential threshold. Background Technology

[0002] The rapid development of mobile robots, autonomous driving, augmented reality (AR), and virtual reality (VR) has created an increasingly urgent need for real-time environmental perception and precise autonomous navigation. Simultaneous Localization and Mapping (SLAM) technology, as a core support in these fields, enables robots to determine their own position and build environmental maps in real time in unknown environments, and is a key technology for intelligent systems to achieve autonomous decision-making.

[0003] In SLAM technology, Visual SLAM (VSLAM) based on visual sensors (such as monocular, binocular, and RGB-D cameras) has become a key research area due to its advantages such as low cost, rich information, and lightweight equipment. Compared to other sensor solutions such as LiDAR, it is more suitable for consumer devices and small mobile robots. However, existing VSLAM technologies still face many challenges, and mainstream VSLAM algorithms (such as ORB-SLAM3, VINS-Mono, and DSO) have significant technical bottlenecks in practical applications. In terms of feature matching, traditional feature matching typically employs single-step verification (such as preliminary matching based on Hamming distance), which is susceptible to noise, occlusion, and changes in illumination, leading to an increased mismatch rate. In some scenarios, the mismatch rate can reach 50%, directly reducing positioning accuracy. Regarding keyframe selection, existing methods (such as fixed thresholds and empirical rules) lack quantitative evaluation, easily resulting in keyframe redundancy or insufficiency, thereby increasing computational burden or reducing trajectory accuracy. In terms of multi-robot collaboration, single-robot SLAM has limited coverage, low task efficiency, and insufficient robustness, while existing multi-robot collaborative SLAM (CSLAM) schemes often ignore the pose probability distribution of each robot in the global coordinate system at startup, resulting in complex map fusion calculations and poor global consistency. In terms of map quality, most SLAM systems output sparse point cloud maps, which are difficult to meet the requirements of dense maps in high-precision scenarios (such as 3D reconstruction and fine navigation), and registration errors are prone to occur during multi-robot map fusion.

[0004] ORB-SLAM3, as a current advanced VSLAM solution, supports monocular, binocular, RGB-D, and vision-inertial fusion modes and has multi-map management capabilities. However, it relies on single-step verification in the feature matching stage, resulting in limited filtering of mismatches. Its keyframe selection strategy relies on empirical thresholds and lacks quantitative modeling of multiple factors (such as time, translation, and rotation). Furthermore, it has not designed an efficient map fusion mechanism for multi-robot collaborative scenarios, making it difficult to directly apply to multi-robot systems.

[0005] Therefore, it is urgent to improve existing SLAM technology, solve the above-mentioned technical problems, and enhance its performance and applicability in practical applications. Summary of the Invention

[0006] One objective of this invention is to propose a multi-robot collaborative SLAM method based on cross-validation and exponential thresholding, which solves problems such as high feature matching mismatch rate, unreasonable keyframe selection, poor multi-robot map fusion effect, and insufficient map accuracy and density in existing SLAM technologies, thereby improving the positioning accuracy, mapping quality and operating efficiency of SLAM systems.

[0007] Another objective of this invention is to propose a multi-robot collaborative SLAM system based on cross-validation and exponential thresholding, employing a multi-robot collaborative SLAM method based on cross-validation and exponential thresholding as described above.

[0008] To achieve this objective, the present invention adopts the following technical solution: The multi-robot collaborative SLAM method based on cross-validation and exponential thresholding includes the following steps: S1. Acquire image data; S2. Perform cross-validation feature matching on the acquired image data to obtain matching feature point pairs; S3. Based on the matching feature point pairs and the pose prediction value of the previous frame, pose tracking is achieved to obtain the pose of the current frame. S4. Filter keyframes using the exponential threshold function; S5. Construct a single-machine dense map based on keyframes; S6. Perform coarse-to-fine multi-map fusion processing on the dense maps of multiple robots on a single machine; S7. Output the final global dense map.

[0009] Preferably, in S2, the cross-validation feature matching process specifically includes the following steps: S21. Obtain two frames of images to be matched and their feature point sets, denoted as the feature point set PA of the first frame image and the feature point set PB of the second frame image, respectively; where PA={ai|i=1,2,…,Na} and PB={bi|i=1,2,…,Nb}, ai=(Uai,Vai), bi=(Ubi,Vbi). S22, By transforming the matrix Projecting the feature point set PB onto the coordinate system of the first frame image yields the projected feature point set. :

[0010] S23, in the set of feature points Within the adjacent grid region, the optimal matching point is determined, and the initial matching point sets PA' and PB' are obtained by filtering; where PA'={ai'|i=1,2,…,N'} and PB'={bi'|i=1,2,…,N'}; S24. By transforming the matrix Project the initial set of matching points PA' onto the coordinate system of the second frame image to obtain the projected set of points. :

[0011] S25, in the set of points Find the optimal matching point within the neighboring grid region to obtain the final matching point set PA'' and PB''; where PA"={ai"|i=1,2...,N"} and PB"={bi"|i=1,2...,N"}.

[0012] Preferably, in S24, the transformation matrix The following can be derived from formula (3):

[0013] in, This is the transformation matrix from the world coordinate system to the coordinate system of the first frame image. This is the transformation matrix from the world coordinate system to the coordinate system of the second frame image. , These are the rotation matrices from the world coordinate system to the coordinate systems of the first and second frames, respectively. , These are the translation vectors from the world coordinate system to the coordinate systems of the first and second frames, respectively; when the coordinate system of the first frame is set as the origin of the world coordinate system... , It is the identity matrix, at this time .

[0014] Preferably, in S4, the keyframe selection using the exponential threshold function specifically includes the following steps: S41. Constructing the input vector ;in, For the cost of time, For the sake of shared quality, As a result of the translation cost, For the cost of rotation, , , , These are weighting coefficients, and + + + =1; S42. Constructing the exponential threshold function ,in, This is the weight matrix; S43, Preset threshold Q, if the current frame's If the value exceeds Q, the frame is determined to be a key frame, sent to the loop closure detection process, and updated as a reference key frame for subsequent processing; wherein, the loop closure detection process refers to comparing the current key frame with historical key frames to identify whether the robot has returned to a previously reached position, and then outputting loop closure constraint information or a processing process without additional constraints.

[0015] Preferably, in S6, the coarse-to-fine multi-image fusion processing specifically includes the following steps: S61. Obtain the dense point cloud map generated by each robot in its own coordinate system, and record the transformation matrix of each robot from the world coordinate system to its own coordinate system. S62. Project the point cloud map of each robot onto the world coordinate system using the transformation matrix. The point cloud in the world coordinate system satisfies:

[0016] And set the matching radius to filter and obtain the matching point sets PAw' and PBw'; in, Point cloud collected by the first robot The point cloud collected for the second robot is PAw'={awi|i=1,2...,N'} and PBw'={bwi|i=1,2...,N'}; S63. If both PAw' and PBw' are non-empty sets, use the generalized iterative nearest-point algorithm to perform a fine matching of PAw' and PBw' and solve for the exact transformation matrix. S64. The matched point cloud is processed using a uniform sampling method, and then the point cloud data is smoothed by a three-dimensional Gaussian filter.

[0017] Preferably, in S63, the implementation of the generalized iterative nearest point algorithm specifically includes the following steps: S631. Assume that point sets PA' and PB' follow a Gaussian distribution:

[0018] in, , These are the means of a Gaussian distribution, respectively. , These are the covariance matrices for each point; S632, Calculate the matching error :

[0019] in, Let be the transformation matrix from the first robot coordinate system to the second robot coordinate system, and ; S633, Matching Error Follows a Gaussian distribution:

[0020] S634. Constructing the cost function:

[0021] The transformation matrix is ​​solved by singular value decomposition. .

[0022] Preferably, in S64, the uniform sampling method constructs a three-dimensional voxel grid, in which all points within each voxel are replaced by the point closest to the voxel center.

[0023] Preferably, in S64, the three-dimensional Gaussian filter satisfies:

[0024] in, for The set of coordinates of the six nearest neighboring points. It is a 3x3 covariance matrix; The covariance matrix The diagonal elements satisfy:

[0025] The covariance matrix Off-diagonal elements satisfy:

[0026] in, The number of neighboring points involved in the calculation; For the g-th neighborhood point in the th... Coordinates on the axis; Corresponding to the x, y, and z axes respectively; In the Coordinates on the axis; for In the Coordinates on the axis; For the first The coordinates of the neighboring points on the k-th axis; This represents two different coordinate axes, namely x and y, x and z, and y and z.

[0027] A multi-robot collaborative SLAM system based on cross-validation and exponential thresholding employs the multi-robot collaborative SLAM method based on cross-validation and exponential thresholding as described above, including: The data acquisition module is used to acquire image data; The cross-validation feature matching module is used to perform cross-validation feature matching on the acquired image data to obtain matching feature point pairs; The pose tracking module is used to achieve pose tracking based on the matching feature point pairs and the pose prediction value of the previous frame, and to obtain the pose of the current frame. The exponential threshold keyframe selection module is used to filter keyframes using an exponential threshold function. The stand-alone dense mapping module is used to build a stand-alone dense map based on keyframes; The coarse-to-fine multi-map fusion module is used to perform coarse-to-fine multi-map fusion processing on dense maps of multiple robots. The post-processing and output module is used to output the final global dense map.

[0028] Preferred options also include: The local mapping module is used to build local maps based on keyframes and matching point pairs; The loop closure detection module is used to perform the loop closure detection process. It compares the current keyframe with the historical keyframes to identify whether the robot has returned to a previously reached position, and then outputs loop closure constraint information or no additional constraints.

[0029] One of the above technical solutions offers the following advantages: This solution constructs a complete and optimized multi-robot collaborative SLAM technology framework, comprehensively addressing the core pain points of traditional SLAM technology in areas such as feature matching, keyframe selection, and multi-map fusion. Through collaborative optimization of each stage, it ensures both positioning accuracy and map quality while maintaining system operating efficiency and reducing computational and storage resource consumption. It has wide applicability, deploying on various platforms such as unmanned vehicles, drones, and mobile robot swarms. It can achieve efficient and accurate collaborative positioning and mapping in complex scenarios such as farmland inspection, warehousing and logistics, and search and rescue, significantly improving the adaptability and reliability of SLAM technology in practical applications. Attached Figure Description

[0030] Figure 1 This is a flowchart illustrating a multi-robot collaborative SLAM method based on cross-validation and exponential thresholding. Figure 2 This is a schematic diagram illustrating the feature matching performance of the cross-validation matching algorithm in a multi-robot collaborative SLAM method based on cross-validation and exponential thresholding. Figure 3This is a schematic diagram of the feature matching representation of the cross-validation matching algorithm in the multi-robot collaborative SLAM method based on cross-validation and exponential threshold. Detailed Implementation

[0031] Embodiments of the present invention are described in detail below. Examples of these embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and should not be construed as limiting the present invention.

[0032] The multi-robot collaborative SLAM method based on cross-validation and exponential thresholding includes the following steps: S1. Acquire image data; S2. Perform cross-validation feature matching on the acquired image data to obtain matching feature point pairs; S3. Based on the matching feature point pairs and the pose prediction value of the previous frame, pose tracking is achieved to obtain the pose of the current frame. S4. Filter keyframes using the exponential threshold function; S5. Construct a single-machine dense map based on keyframes; S6. Perform coarse-to-fine multi-map fusion processing on the dense maps of multiple robots on a single machine; S7. Output the final global dense map.

[0033] like Figure 1 As shown, this technical solution follows a complete technical process of "data acquisition - feature matching - pose tracking - keyframe filtering - single-machine mapping - multi-map fusion - result output," forming a closed-loop collaborative SLAM system. First, environmental image data is acquired through sensors, providing raw input for subsequent processing. Then, a cross-validation mechanism is used to accurately match image features, obtaining reliable feature point pairs. Next, based on the matched feature point pairs and the pose prediction value of the previous frame, real-time tracking and calculation of the pose of the current frame are achieved. Subsequently, frame data is quantized and filtered using an exponential threshold function to determine keyframes, balancing accuracy and efficiency. Based on the selected keyframes, a dense point cloud map of a single robot is constructed. For the single-machine dense map of multiple robots, a coarse-to-fine fusion strategy is used to complete global map integration. Finally, a global dense map that meets high-precision requirements is output, covering all core aspects of SLAM technology and ensuring seamless connection and smooth data flow between steps.

[0034] To further explain, in S2, the cross-validation feature matching process specifically includes the following steps: S21. Obtain two frames of images to be matched and their feature point sets, denoted as the feature point set PA of the first frame image and the feature point set PB of the second frame image, respectively; where PA={ai|i=1,2,…,Na} and PB={bi|i=1,2,…,Nb}, ai=(Uai,Vai), bi=(Ubi,Vbi). S22, By transforming the matrix Projecting the feature point set PB onto the coordinate system of the first frame image yields the projected feature point set. :

[0035] S23, in the set of feature points Within the adjacent grid region, the optimal matching point is determined, and the initial matching point sets PA' and PB' are obtained by filtering; where PA'={ai'|i=1,2,…,N'} and PB'={bi'|i=1,2,…,N'}; S24. By transforming the matrix Project the initial set of matching points PA' onto the coordinate system of the second frame image to obtain the projected set of points. :

[0036] S25, in the set of points Find the optimal matching point within the neighboring grid region to obtain the final matching point set PA'' and PB''; where PA"={ai"|i=1,2...,N"} and PB"={bi"|i=1,2...,N"}.

[0037] like Figure 2-3 As shown, the above cross-validation feature matching process adopts a "two-way projection-two-stage matching" cross-validation mechanism to ensure matching accuracy. First, the feature point sets PA and PB of the two frames to be matched are clearly defined, where PA={ai|i=1,2,…,Na} and PB={bi|i=1,2,…,Nb}, and ai=(Uai,Vai) and bi=(Ubi,Vbi), clearly defining the coordinate attributes of the feature points. The first stage involves transforming the matrix... Projecting PB onto the coordinate system of the first frame image yields... (satisfy ),exist Within the neighboring grid region, the optimal matching points are selected using the Hamming distance between descriptors, forming an initial set of matching points PA' and PB'. The second stage involves transforming the matrix... Backprojecting PA' onto the coordinate system of the second frame image, we obtain... (satisfy ),exist The optimal matching point is searched again within the adjacent grid area, resulting in two final matching point pairs, PA'' and PB''. During this process, the grid area size can be dynamically adjusted, and a smaller matching radius can be used in the second stage to further improve the screening effect.

[0038] Compared to traditional single-step verification feature matching methods, this bidirectional cross-validation mechanism effectively eliminates mismatched points caused by noise, occlusion, and illumination variations, significantly reducing the mismatch rate by more than 50%. Simultaneously, through two-stage matching and grid region constraints, a uniform distribution of matching points is ensured, providing high-precision input data for subsequent pose tracking, thereby improving the accuracy of pose estimation and the stability of map construction. Furthermore, dynamically adjusted grid size and matching radius enhance the method's adaptability to different scenarios, maintaining stable matching performance even in complex environments.

[0039] To further explain, in S24, the transformation matrix The following can be derived from formula (3):

[0040] in, This is the transformation matrix from the world coordinate system to the coordinate system of the first frame image. This is the transformation matrix from the world coordinate system to the coordinate system of the second frame image. , These are the rotation matrices from the world coordinate system to the coordinate systems of the first and second frames, respectively. , These are the translation vectors from the world coordinate system to the coordinate systems of the first and second frames, respectively; when the coordinate system of the first frame is set as the origin of the world coordinate system... , It is the identity matrix, at this time .

[0041] The above transformation matrix It is the core parameter for bidirectional projection in cross-validation feature matching, and its derivation is based on the pose transformation relationship between the world coordinate system and the image coordinate system. First, define... This is the transformation matrix from the world coordinate system to the coordinate system of the first frame image. This is the transformation matrix from the world coordinate system to the second frame image coordinate system, both of which contain rotation matrices. , Translation vector , ), in matrix form respectively and The result is derived through matrix operations. Further development into The calculation logic for rotation and translation components was clarified. When the coordinate system of the first frame image is set to the origin of the world coordinate system, (Identity matrix), at this time This simplifies the computation process in specific scenarios.

[0042] The above transformation matrix The derivation process strictly follows the mathematical principles of coordinate system transformation, ensuring the accuracy and reliability of the transformation matrix TBA and providing precise mathematical support for bidirectional projection in cross-validation. By clarifying the matrix composition and operational logic, the projection transformation of feature points between different coordinate systems is made more accurate, avoiding matching deviations caused by transformation errors and further improving the accuracy of feature matching. Simultaneously, the simplified calculation rules set for the initial frame reduce computational complexity without affecting accuracy, while also considering the real-time performance of the system, enabling the method to run efficiently on resource-constrained devices.

[0043] To further explain, in S4, the keyframe selection using the exponential threshold function specifically includes the following steps: S41. Constructing the input vector ;in, For the cost of time, For the sake of shared quality, As a result of the translation cost, For the cost of rotation, , , , These are weighting coefficients, and + + + =1; S42. Constructing the exponential threshold function ,in, This is the weight matrix; S43, Preset threshold Q, if the current frame's If the value exceeds Q, the frame is determined to be a key frame, sent to the loop closure detection process, and updated as a reference key frame for subsequent processing; wherein, the loop closure detection process refers to comparing the current key frame with historical key frames to identify whether the robot has returned to a previously reached position, and then outputting loop closure constraint information or a processing process without additional constraints.

[0044] The keyframe selection method described above achieves dynamic and adaptive keyframe selection through quantization modeling. First, an input vector is constructed. Comprehensive coverage of time costs ( Common quality () ), translation cost ( ), rotation cost ( Four core influencing factors, among which , , , Normalized weighted coefficients ( + + + =1), allowing for dynamic adjustment of the influence weights of each factor based on different scenarios. Subsequently, an exponential threshold function is constructed. ,in A weight matrix of predefined size, optimized during model training, is used to achieve non-linear dynamic priority scheduling of influencing factors in an exponential form. Finally, a preset threshold Q is set; if the current frame's... If the value exceeds Q, it is determined to be a keyframe and sent to the loop closure detection process (comparing the current keyframe with historical keyframes to identify whether it has returned to a previously reached position, outputting loop closure constraint information or no additional constraints), and updated as the reference keyframe for subsequent processing.

[0045] The aforementioned keyframe selection method breaks away from the limitations of traditional keyframe selection, which relies on fixed thresholds or empirical rules. By comprehensively considering multi-dimensional influencing factors through quantitative modeling, it achieves adaptive keyframe selection. The design of the exponential threshold function enables keyframe selection to dynamically respond to environmental changes and robot motion states, effectively reducing redundant keyframes (by 32.89%) and lowering computational and storage resource consumption. Simultaneously, it improves positioning accuracy (by 12.90%), achieving an optimal balance between system efficiency and mapping accuracy. The integration of loop closure detection further ensures global map consistency, avoids trajectory drift, and allows the SLAM system to maintain stable performance during long-term, large-scale operation.

[0046] To further explain, in S6, the coarse-to-fine multi-image fusion processing specifically includes the following steps: S61. Obtain the dense point cloud map generated by each robot in its own coordinate system, and record the transformation matrix of each robot from the world coordinate system to its own coordinate system. S62. Project the point cloud map of each robot onto the world coordinate system using the transformation matrix. The point cloud in the world coordinate system satisfies:

[0047] And set the matching radius to filter and obtain the matching point sets PAw' and PBw'; in, Point cloud collected by the first robot The point cloud collected for the second robot is PAw'={awi|i=1,2...,N'} and PBw'={bwi|i=1,2...,N'}; S63. If both PAw' and PBw' are non-empty sets, use the generalized iterative nearest-point algorithm to perform a fine matching of PAw' and PBw' and solve for the exact transformation matrix. S64. The matched point cloud is processed using a uniform sampling method, and then the point cloud data is smoothed by a three-dimensional Gaussian filter.

[0048] The aforementioned multi-map fusion method employs a three-stage process of "coarse matching - fine matching - optimization" to achieve efficient integration of dense maps from multiple robots. The first stage (coarse matching) involves acquiring dense point cloud maps in each robot's own coordinate system, recording the transformation matrix from the world coordinate system to each robot's coordinate system, and then performing matrix operations (…). , The first stage involves projecting all point clouds onto a unified world coordinate system, setting a matching radius to filter out matching point sets PAw' and PBw', thus completing the initial localization of potential overlapping areas. The second stage (fine matching) involves using the Generalized Iterative Closest Point (GICP) algorithm to precisely match the two point sets if both PAw' and PBw' are non-empty sets, solving for the optimal transformation matrix, and eliminating residual errors from the coarse matching stage. The third stage (optimization) involves using a uniform sampling method to simplify the matched point cloud data, and then smoothing the point cloud data using a 3D Gaussian filter to reduce noise interference and improve map quality.

[0049] The aforementioned fusion method fully utilizes prior information from the robot's initial pose, quickly identifying overlapping regions through coarse matching, significantly reducing the computational complexity of subsequent fine matching and improving fusion efficiency. The introduction of the GICP algorithm achieves high-precision fine matching of point clouds, and combined with optimized processing using uniform sampling and 3D Gaussian filtering, effectively reduces fusion errors (by more than 20%), ultimately resulting in a globally consistent, low-noise, and highly dense point cloud map. This method solves the problems of poor global consistency and large registration errors in traditional multi-robot SLAM map fusion, while simultaneously balancing computational efficiency and map quality, providing reliable technical support for multi-robot collaborative mapping.

[0050] To further explain, in S63, the implementation of the generalized iterative nearest point algorithm specifically includes the following steps: S631. Assume that point sets PA' and PB' follow a Gaussian distribution:

[0051] in, , These are the means of a Gaussian distribution, respectively. , These are the covariance matrices for each point; S632, Calculate the matching error :

[0052] in, Let be the transformation matrix from the first robot coordinate system to the second robot coordinate system, and ; S633, Matching Error Follows a Gaussian distribution:

[0053] S634. Constructing the cost function:

[0054] The transformation matrix is ​​solved by singular value decomposition. .

[0055] The Generalized Iterative Closest Point (GICP) algorithm described above achieves high-precision matching of point clouds through probabilistic modeling and optimization. First, it is assumed that the point sets PA' and PB' to be matched follow a Gaussian distribution (…). , ),in , As the mean of the ideal coordinates, , To characterize the covariance matrix of the measurement error, the probabilistic characteristics of the point cloud data were clarified. The matching error was then calculated. ( Let be the transformation matrix from the coordinate system of the first robot to the coordinate system of the second robot, satisfying ),because and Independent of each other, matching error obey The error distribution is Gaussian. A cost function is constructed based on this error distribution. The optimal transformation matrix is ​​solved by singular value decomposition (SVD). This completes the precise registration of the point cloud.

[0056] Compared to the traditional ICP algorithm, the Generalized Iterative Closest Point (GICP) algorithm introduces a covariance matrix to model measurement errors, effectively eliminating the influence of outlier points and improving the robustness of matching. By constructing a cost function through rigorous probabilistic derivation, the optimization objective is made more closely aligned with the actual data characteristics, resulting in a more accurate transformation matrix and significantly reducing point cloud registration errors. The singular value decomposition (SVD) solution ensures the algorithm's stability and convergence speed, making the fine-matching process efficient and reliable. This provides a high-precision coordinate transformation foundation for multi-map fusion, further enhancing the consistency and accuracy of the global map.

[0057] To further explain, in S64, the uniform sampling method constructs a three-dimensional voxel grid, in which all points within each voxel are replaced by the point closest to the voxel center.

[0058] The aforementioned uniform sampling method achieves efficient simplification of point cloud data through spatial grid partitioning. The core step involves constructing a three-dimensional voxel grid, dividing the matched point cloud data into different voxels based on spatial location. Each voxel contains multiple point cloud data points. By calculating the voxel's center coordinates, the point closest to these coordinates is selected as the representative point of that voxel, replacing all original points within the voxel, thus achieving downsampling of the point cloud data. This method does not rely on local features of the point cloud; it samples solely based on spatial distribution, ensuring the uniformity of the sampled point cloud.

[0059] The uniform sampling method described above can significantly reduce the amount of data in the point cloud after matching, effectively reducing the computational and storage pressure on subsequent Gaussian filtering and global map storage, and improving system operating efficiency. By retaining points near the voxel center, the spatial geometric structure of the point cloud is preserved to the maximum extent while simplifying the data, avoiding the loss of map features due to excessive data simplification. In addition, the point cloud distribution after uniform sampling is more regular, reducing the impact of noise and outliers, providing higher-quality input data for subsequent 3D Gaussian filtering, and further improving the smoothness and consistency of the final map.

[0060] To further explain, in S64, the three-dimensional Gaussian filter satisfies:

[0061] in, for The set of coordinates of the six nearest neighboring points. It is a 3x3 covariance matrix; The covariance matrix The diagonal elements satisfy:

[0062] The covariance matrix Off-diagonal elements satisfy:

[0063] in, The number of neighboring points involved in the calculation; For the g-th neighborhood point in the th... Coordinates on the axis; Corresponding to the x, y, and z axes respectively; In the Coordinates on the axis; for In the Coordinates on the axis; For the first The coordinates of the neighboring points on the k-th axis; This represents two different coordinate axes, namely x and y, x and z, and y and z.

[0064] The aforementioned 3D Gaussian filtering achieves smooth denoising of point cloud data through neighborhood weighted averaging. Its core lies in the application of the Gaussian function and the covariance matrix. The filtering formula is as follows: ,in for The set of coordinates of the six nearest neighboring points. The covariance matrix is ​​a 3x3 matrix that represents the correlation between point cloud variations along the X, Y, and Z axes. The diagonal elements (variance) satisfy Off-diagonal elements (covariance) satisfy It is calculated using the coordinates of neighboring points. This Gaussian function is then used to... The neighborhood points are weighted to obtain the smoothed result. Coordinates are used to denoise point clouds.

[0065] The aforementioned filtering method fully considers the spatial distribution characteristics of point clouds, accurately characterizing the variation patterns of point clouds in various dimensions through the covariance matrix, making the filtering process more targeted. The weighted averaging mechanism of the Gaussian function effectively suppresses the interference of noise points while preserving the original geometric features of the point cloud, avoiding map distortion caused by excessive smoothing. The filtered point cloud map has lower noise and higher smoothness. When used in conjunction with the uniform sampling method, it further improves the quality of globally dense maps, making the map more suitable for high-precision application scenarios such as 3D reconstruction and fine navigation.

[0066] A multi-robot collaborative SLAM system based on cross-validation and exponential thresholding employs the multi-robot collaborative SLAM method based on cross-validation and exponential thresholding as described above, including: The data acquisition module is used to acquire image data; The cross-validation feature matching module is used to perform cross-validation feature matching on the acquired image data to obtain matching feature point pairs; The pose tracking module is used to achieve pose tracking based on the matching feature point pairs and the pose prediction value of the previous frame, and to obtain the pose of the current frame. The exponential threshold keyframe selection module is used to filter keyframes using an exponential threshold function. The stand-alone dense mapping module is used to build a stand-alone dense map based on keyframes; The coarse-to-fine multi-map fusion module is used to perform coarse-to-fine multi-map fusion processing on dense maps of multiple robots. The post-processing and output module is used to output the final global dense map.

[0067] This system achieves clear functional division and efficient collaboration through modular design, fully leveraging the technical advantages of each core method to comprehensively enhance the overall performance of the SLAM system. The collaboration between the cross-validation feature matching module and the exponential threshold keyframe selection module effectively reduces the false matching rate and redundant data volume, improving single-machine mapping accuracy. The coarse-to-fine multi-map fusion module solves the global consistency problem in multi-robot map fusion, achieving high-precision global dense map construction. The system has a compact structure and strong scalability, adaptable to different types of visual sensors and multiple robot platforms, and can operate stably in various complex scenarios, providing reliable technical support for autonomous driving, AR / VR, mobile robotics, and other fields.

[0068] Further explanation includes: The local mapping module is used to build local maps based on keyframes and matching point pairs; The loop closure detection module is used to perform the loop closure detection process. It compares the current keyframe with the historical keyframes to identify whether the robot has returned to a previously reached position, and then outputs loop closure constraint information or no additional constraints.

[0069] The local mapping module and loop closure detection module, as supplementary functional modules of the system, further improve the performance of the SLAM system. The local mapping module takes keyframes and matching point pairs as input and constructs a point cloud map of the local area based on technologies such as Structure for Motion Restoration (SfM), providing a high-precision local foundation for the global map. The loop closure detection module executes the loop closure detection process, matching feature points and verifying geometric constraints between the current keyframe and the historical keyframes stored in the system to identify whether the robot has returned to a previously reached position. If a loop closure is detected, loop closure constraint information is output for pose optimization and map correction. If no loop closure is detected, no additional constraints are output to ensure normal system progress.

[0070] Therefore, the introduction of the local mapping module can refine the map details of local areas, improve the accuracy and completeness of local maps, and provide a solid foundation for the construction of the global map. The loop closure detection module effectively corrects the pose errors accumulated during robot movement by identifying repetitive scenes, avoiding map drift and ensuring the consistency and accuracy of the global map. The addition of these two modules makes the SLAM system more complete, further improving the stability and reliability of the system during long-term, large-scale operation, and broadening the system's application scenarios and practical value.

[0071] Furthermore, the functional units in the various embodiments of the present invention can be integrated into a processing module, or each unit can exist physically separately, or two or more units can be integrated into a module. The integrated module can be implemented in hardware or as a software functional module. If the integrated module is implemented as a software functional module and sold or used as an independent product, it can also be stored in a computer-readable storage medium.

[0072] Although embodiments of the present invention have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those skilled in the art can make changes, modifications, substitutions and variations to the above embodiments within the scope of the present invention.

Claims

1. A multi-robot collaborative SLAM method based on cross-validation and exponential thresholding, characterized in that, Includes the following steps: S1. Acquire image data; S2. Perform cross-validation feature matching on the acquired image data to obtain matching feature point pairs; S3. Based on the matching feature point pairs and the pose prediction value of the previous frame, pose tracking is achieved to obtain the pose of the current frame. S4. Filter keyframes using the exponential threshold function; S5. Construct a single-machine dense map based on keyframes; S6. Perform coarse-to-fine multi-map fusion processing on the dense maps of multiple robots on a single machine; S7. Output the final global dense map.

2. The multi-robot collaborative SLAM method based on cross-validation and exponential thresholding according to claim 1, characterized in that, In S2, the cross-validation feature matching process specifically includes the following steps: S21. Obtain two frames of images to be matched and their feature point sets, denoted as the feature point set PA of the first frame image and the feature point set PB of the second frame image, respectively; where PA={ai|i=1,2,…,Na} and PB={bi|i=1,2,…,Nb}, ai=(Uai,Vai), bi=(Ubi,Vbi). S22, By transforming the matrix Projecting the feature point set PB onto the coordinate system of the first frame image yields the projected feature point set. : S23, in the set of feature points Within the adjacent grid region, the optimal matching point is determined, and the initial matching point sets PA' and PB' are obtained by filtering; where PA'={ai'|i=1,2,…,N'} and PB'={bi'|i=1,2,…,N'}; S24. By transforming the matrix Project the initial set of matching points PA' onto the coordinate system of the second frame image to obtain the projected set of points. : S25, in the set of points Find the optimal matching point within the neighboring grid region to obtain the final matching point set PA'' and PB''; where PA"={ai"|i=1,2...,N"} and PB"={bi"|i=1,2...,N"}.

3. The multi-robot collaborative SLAM method based on cross-validation and exponential thresholding according to claim 2, characterized in that, In S24, the transformation matrix The following can be derived from formula (3): in, This is the transformation matrix from the world coordinate system to the coordinate system of the first frame image. This is the transformation matrix from the world coordinate system to the coordinate system of the second frame image. , These are the rotation matrices from the world coordinate system to the coordinate systems of the first and second frames, respectively. , These are the translation vectors from the world coordinate system to the coordinate systems of the first and second frames, respectively; when the coordinate system of the first frame is set as the origin of the world coordinate system... , It is the identity matrix, at this time .

4. The multi-robot collaborative SLAM method based on cross-validation and exponential thresholding according to claim 1, characterized in that, In S4, the keyframe filtering using the exponential threshold function specifically includes the following steps: S41. Constructing the input vector ;in, For the cost of time, For the sake of shared quality, As a result of the translation cost, For the cost of rotation, , , , These are weighting coefficients, and + + + =1; S42. Constructing the exponential threshold function ,in, This is the weight matrix; S43, Preset threshold Q, if the current frame's If the value exceeds Q, the frame is determined to be a key frame, sent to the loop closure detection process, and updated as a reference key frame for subsequent processing; wherein, the loop closure detection process refers to comparing the current key frame with historical key frames to identify whether the robot has returned to a previously reached position, and then outputting loop closure constraint information or a processing process without additional constraints.

5. The multi-robot collaborative SLAM method based on cross-validation and exponential thresholding according to claim 1, characterized in that, In S6, the coarse-to-fine multi-image fusion processing specifically includes the following steps: S61. Obtain the dense point cloud map generated by each robot in its own coordinate system, and record the transformation matrix of each robot from the world coordinate system to its own coordinate system. S62. Project the point cloud map of each robot onto the world coordinate system using the transformation matrix. The point cloud in the world coordinate system satisfies: And set the matching radius to filter and obtain the matching point sets PAw' and PBw'; in, Point cloud collected by the first robot The point cloud collected for the second robot is PAw'={awi|i=1,2...,N'} and PBw'={bwi|i=1,2...,N'}; S63. If both PAw' and PBw' are non-empty sets, use the generalized iterative nearest-point algorithm to perform a fine matching of PAw' and PBw' and solve for the exact transformation matrix. S64. The matched point cloud is processed using a uniform sampling method, and then the point cloud data is smoothed by a three-dimensional Gaussian filter.

6. The multi-robot collaborative SLAM method based on cross-validation and exponential thresholding according to claim 5, characterized in that, In S63, the implementation of the generalized iterative nearest point algorithm specifically includes the following steps: S631. Assume that point sets PA' and PB' follow a Gaussian distribution: in, , These are the means of a Gaussian distribution, respectively. , These are the covariance matrices for each point; S632, Calculate the matching error : in, Let be the transformation matrix from the first robot coordinate system to the second robot coordinate system, and ; S633, Matching Error Follows a Gaussian distribution: S634. Constructing the cost function: The transformation matrix is ​​solved by singular value decomposition. .

7. The multi-robot collaborative SLAM method based on cross-validation and exponential thresholding according to claim 5, characterized in that, In S64, the uniform sampling method constructs a three-dimensional voxel grid, in which all points within each voxel are replaced by the point closest to the voxel center.

8. The multi-robot collaborative SLAM method based on cross-validation and exponential thresholding according to claim 5, characterized in that, In S64, the three-dimensional Gaussian filter satisfies: in, for The set of coordinates of the six nearest neighboring points. It is a 3x3 covariance matrix; The covariance matrix The diagonal elements satisfy: The covariance matrix Off-diagonal elements satisfy: in, The number of neighboring points involved in the calculation; For the g-th neighborhood point in the th... Coordinates on the axis; Corresponding to the x, y, and z axes respectively; In the Coordinates on the axis; for In the Coordinates on the axis; For the first The coordinates of the neighboring points on the k-th axis; This represents two different coordinate axes, namely x and y, x and z, and y and z.

9. A multi-robot collaborative SLAM system based on cross-validation and exponential thresholding, characterized in that, The multi-robot collaborative SLAM method based on cross-validation and exponential thresholding as described in any one of claims 1-8 includes: The data acquisition module is used to acquire image data; The cross-validation feature matching module is used to perform cross-validation feature matching on the acquired image data to obtain matching feature point pairs; The pose tracking module is used to achieve pose tracking based on the matching feature point pairs and the pose prediction value of the previous frame, and to obtain the pose of the current frame. The exponential threshold keyframe selection module is used to filter keyframes using an exponential threshold function. The stand-alone dense mapping module is used to build a stand-alone dense map based on keyframes; The coarse-to-fine multi-map fusion module is used to perform coarse-to-fine multi-map fusion processing on dense maps of multiple robots. The post-processing and output module is used to output the final global dense map.

10. The multi-robot collaborative SLAM system based on cross-validation and exponential thresholding according to claim 9, characterized in that, Also includes: The local mapping module is used to build local maps based on keyframes and matching point pairs; The loop closure detection module is used to perform the loop closure detection process. It compares the current keyframe with the historical keyframes to identify whether the robot has returned to a previously reached position, and then outputs loop closure constraint information or no additional constraints.

Images