Multi-robot relative positioning method for field environment

Abstract

The invention relates to a multi-robot relative positioning method for a field environment, and the method comprises the steps: constructing a local point cloud map, removing ground point cloud, obtaining a non-ground local point cloud map, projecting the non-ground local point cloud map to a bird's-eye view, and obtaining a multi-frame bird's-eye view space height distribution diagram as a historical frame; screening a candidate loopback frame; screening out an effective loopback frame from the plurality of candidate loopback frames, and obtaining a relative pose of the robot between the current frame and the effective loopback frame as a loopback constraint; removing abnormal values in the loopback constraint set to obtain a reliable loopback constraint set; loopback factors are generated based on the reliable loopback constraint set, and a joint factor graph is constructed; and carrying out adaptive weight adjustment on the loopback factors in the joint factor graph, removing the loopback factors of which the weights are lower than a threshold value, updating the joint factor graph, and updating pose estimation results and global consistent trajectory estimation results of all robots at the same time.

Description

Technical Field

[0001] This invention relates to the field of robotics, and in particular to a method for relative positioning of multiple robots in outdoor environments. Background Technology

[0002] With the rapid development of robotics technology and the continuous expansion of its application scenarios, multi-robot collaborative operations have shown great potential in field applications such as search and rescue, environmental monitoring, agricultural plant protection, and military reconnaissance. In these applications, accurate relative positioning is the core technological foundation for achieving multi-robot collaborative navigation, formation control, and task allocation.

[0003] Traditional multi-robot localization methods mainly rely on global positioning systems such as GPS. However, in complex outdoor environments, GPS signals are easily affected by factors such as terrain obstruction, vegetation cover, and weather conditions, leading to decreased positioning accuracy or even signal loss. In addition, methods that rely solely on each robot performing SLAM (Simultaneous Localization and Mapping) suffer from problems such as large cumulative errors and lack of global consistency, making it difficult to meet the high-precision relative positioning requirements of multi-robot collaborative operations.

[0004] Existing multi-robot relative localization technologies face the following main technical challenges: First, the terrain in the field is complex, features are repetitive, and lighting changes drastically, making traditional visual feature matching methods prone to mismatches and loop closure detection failures. Second, multi-robot systems contain a large number of inter-robot measurement constraints, such as loop closure constraints, and outliers contained therein can seriously affect the overall localization accuracy. Existing outlier detection methods have limited effectiveness in handling complex constraint relationships. Third, the dynamic changes in the field environment and sensor noise make the localization system prone to getting trapped in local optima, and there is a lack of effective robust optimization mechanisms.

[0005] Therefore, a multi-robot relative positioning method that can adapt to complex field environments and has strong robustness is needed. Summary of the Invention

[0006] To address at least some of the problems mentioned above in the prior art, the present invention provides a multi-robot relative localization method for field environments, comprising the following steps: A local point cloud map is constructed based on the robot's pose in the local coordinate system and the 3D laser point cloud. The ground point cloud is removed to obtain a non-ground local point cloud map. The non-ground local point cloud map is then projected onto a bird's-eye view to obtain multiple frames of bird's-eye view spatial height distribution map as historical frames. Obtain the spatial height distribution map of the bird's-eye view at the current moment as the current frame, and filter the multiple historical frames most similar to the current frame as candidate loopback frames; Valid loop closure frames are selected from multiple candidate loop closure frames, and the robot's relative pose between the current frame and the valid loop closure frame is obtained as the loop closure constraint. Collect all historical loop constraints detected between the main robot and multiple sub-robots to obtain a loop constraint set. Group the loop constraints by robot pair, extract all historical loop constraints between robot pairs to obtain a loop constraint subset, and add the loop constraints between the current frame and the valid loop frame to the corresponding loop constraint subset, and update the loop constraint set. Remove outliers from the set of closure constraints to obtain a reliable set of closure constraints; Based on a reliable set of laparoscopic constraints, laparoscopic factors are generated, and a joint factor graph is constructed; and Adaptive weight adjustment is performed on the laparoscopic factors in the joint factor graph, laparoscopic factors with weights below the threshold are removed, the joint factor graph is updated, and the pose estimation results and globally consistent trajectory estimation results of all robots are updated at the same time.

[0007] Furthermore, based on the bird's-eye view spatial height distribution map, global descriptor vectors are extracted, and a global descriptor database storing the global descriptor vectors of historical frames is constructed, wherein: Normalize the spatial height distribution map from a bird's-eye view; The bird's-eye view spatial height distribution map is input into the self-supervised learning visual foundation model DINOv2 to extract global descriptor vectors, which are then normalized.

[0008] Furthermore, the cosine similarity between the global descriptor vector of the current frame and the global descriptor vector of the historical frames is calculated, and multiple most similar historical frames are selected as candidate loop closure frames. The original 3D laser point cloud corresponding to the current frame and multiple candidate loop closure frames is obtained, and point cloud registration is performed. Valid loop closure frames are selected from multiple candidate loop closure frames, and the relative pose of the robot between the current frame and the valid loop closure frames is obtained as the loop closure constraint.

[0009] Furthermore, the pairwise consistency maximization algorithm is used to perform geometric verification on each pair of closure constraints, removing outliers from the closure constraint set to obtain a reliable closure constraint set.

[0010] Furthermore, a pairwise consistency maximization algorithm is used to perform geometric verification on each pair of loop closure constraints, removing outliers from the set of loop closure constraints, including: A quadrilateral geometric verification model is constructed for each pair of closure constraints using the pairwise consistency maximization algorithm, and pose nodes are defined. ,in For the odometry pose node of robot A, For the odometry pose node of robot B, , and , Each has a closure constraint, and the pose nodes are calculated separately. mutual pose transformation , , , ; Calculate the consistency error ,in To perform pose composition, multiple relative pose transformations are chained together. To calculate the relative transformation or residual between two poses using pose difference calculation, a consistency metric is calculated based on Mahalanobis distance, and the geometric residual is converted into a chi-squared value. The statistics of the distribution; Build The consistency matrix, where N is the total number of loop constraints, and a matrix element of 1 indicates geometric consistency and 0 indicates geometric conflict; The consistency matrix is ​​converted into an undirected graph format. The fast maximum clique heuristic algorithm is called to solve for the maximum clique and extract the largest mutually consistent subset of cyclic constraints. Based on the maximum clique solution results, closure constraints within the maximum clique are retained, while closure constraints outside the maximum clique are marked as outliers and removed. The number of removed outliers and the number of retained closure constraints are counted, the closure constraint database is updated, outlier records are removed, and a reliable set of closure constraints is output.

[0011] Furthermore, closure factors are generated based on a reliable closure constraint set, and a joint factor graph containing collaborative mapping data from multiple robots is constructed. The joint factor graph includes pose nodes, odometry factors, closure factors, and prior factors.

[0012] Furthermore, an asymptotic nonconvexity algorithm is adopted, and the Levenberg-Marquardt algorithm is used as the basic solver to iteratively solve the weighted least squares problem transformed by the asymptotic nonconvexity algorithm, and adaptive weight adjustment is performed on the cyclic factors in the joint factor graph. Remove loop factors with weights below the threshold, update the joint factor graph, and simultaneously update the pose estimation results and globally consistent trajectory estimation results for all robots.

[0013] Furthermore, odometry noise, loop closure noise, and prior noise are set in the joint factor graph; Set the parameters of the asymptotic nonconvexity algorithm and use the asymptotic nonconvexity algorithm to adaptively adjust the weights of the closure factor; Iterate through the final weight values ​​of all loop closure factors, identify loop closure factors with weights below the threshold, mark them as outlier candidates, remove them from the joint factor graph, and output the pose estimation results and globally consistent trajectory estimation results for all robots.

[0014] Furthermore, the parameters for setting the asymptotic nonconvexity algorithm include: Initial robust parameters growth factors Maximum number of iterations K=5, set outlier detection threshold: interior point confidence threshold Weight threshold ; Define a known set of reliable constraints, label the prior factors and odometer factors as reliable constraints, apply an asymptotic nonconvexity algorithm with adaptive weight adjustment only to the closure factors, and use the robust Giman-McLuhr error function as... Robust loss function, where r is the residual value of the factor; Set the initial robustness parameters for the asymptotic nonconvexity algorithm. , By iterating through all factors , It is the set of all cyclic factors in the joint factor graph, and the cyclic factors are calculated in the initial state. The squared error With the inherent saturation threshold of the cyclic factor The maximum value obtained from the ratio; When using an asymptotically nonconvex algorithm to adaptively adjust the weights of laparoscopic factors, the Levenberg-Marquardt algorithm is used to solve for weighted least squares and update the weights. Gradually reduce robust parameters until robust parameters Reach the endpoint value If convergence is determined, the current weight values ​​are fixed, where It is a growth factor.

[0015] Furthermore, the robot's starting position is defined as the local coordinate origin, and the robot's pose in the local coordinate system is obtained by fusing the IMU data collected by the robot with the 3D laser point cloud through laser-inertial odometry. Construct a local point cloud map based on the robot's pose in a local coordinate system and the 3D laser point cloud; Remove the ground point cloud to obtain a non-ground local point cloud map; The 3D laser point cloud of the non-ground local point cloud map is rasterized and projected onto the xy plane. The maximum height in each grid cell, i.e. the maximum value of the z-axis, is recorded to obtain a multi-frame bird's-eye view spatial height distribution map as a historical frame.

[0016] The present invention has at least the following beneficial effects: This invention uses point cloud data collected by lidar, projects it onto a bird's-eye view, and uses the self-supervised learning visual foundation model DINOv2 to extract global descriptor vectors. It leverages the powerful performance of the visual foundation model DINOv2 to enhance the ability to distinguish unstructured and sparse repetitive features in the field. Compared with descriptors based on appearance or geometric features, global descriptor vectors are better able to distinguish between large areas of similar grassland, forest, hill or desert in the field, which are essentially different places, and avoid producing false loops. A two-stage loop closure detection method is adopted. First, candidate loop closure frames are selected based on global descriptor vectors to complete coarse loop closure matching. Then, effective loop closure frames are selected from the candidate loop closure frames based on the GICP algorithm, and the robot's relative pose between the current frame and the effective loop closure frame is obtained as the loop closure constraint to complete fine loop closure matching, thus achieving robust loop closure detection in complex field environments. The pairwise consistency maximization (PCM) algorithm is used for outlier loop removal, efficiently filtering outliers in loop constraints to obtain a reliable set of loop constraints. Based on this set, loop factors are generated, and a joint factor graph containing collaborative mapping data from multiple robots is constructed. Then, the asymptotic nonconvexity GNC algorithm is used to adaptively adjust the weights of the loop factors in the joint factor graph to reduce the impact of outliers. By introducing both PCM and GNC algorithms to verify loop constraints, PCM identifies outliers within groups at the geometric level, while GNC identifies outliers that interfere with the solution at the optimization level. This ensures that only the highest quality loop constraints participate in the final pose optimization, resulting in more accurate pose estimation results.

[0017] The asymptotically nonconvex GNC algorithm is used to adaptively adjust the weights of the closure factors in the joint factor graph to reduce the impact of outliers. This breaks through the rigidity of traditional robust kernel functions that reduce the impact of outliers by limiting the upper limit of residual penalty. It adapts to the dynamic changes in field scenarios, eliminates the need for repeated manual parameter adjustments, and avoids the problems of traditional kernel functions that are either too conservative in thresholding and rejecting too many true closures or too aggressive in thresholding and accepting too many false closures. Attached Figure Description

[0018] To further illustrate the above and other advantages and features of the various embodiments of the present invention, a more specific description of the embodiments of the invention will be presented with reference to the accompanying drawings. It is to be understood that these drawings depict only typical embodiments of the invention and are therefore not intended to limit its scope. In the drawings, identical or corresponding parts will be indicated by identical or similar reference numerals for clarity.

[0019] Figure 1 A flowchart of a multi-robot relative localization method for field environments according to an embodiment of the present invention is shown.

[0020] Figure 2A logical architecture diagram of a multi-robot relative localization method for field environments according to an embodiment of the present invention is shown.

[0021] Figure 3 A flowchart illustrating the extraction of global descriptor vectors from a DINO-BEV model according to an embodiment of the present invention is shown.

[0022] Figure 4 A schematic diagram of a joint factor graph according to an embodiment of the present invention is shown. Detailed Implementation

[0023] It should be noted that the components in the accompanying drawings may be shown exaggerated for illustrative purposes and may not be to scale.

[0024] In this invention, the various embodiments are merely intended to illustrate the solutions of the invention and should not be construed as limiting.

[0025] In this invention, unless otherwise specified, the quantifiers “a” and “one” do not exclude scenarios involving multiple elements.

[0026] It should also be noted that, in the embodiments of the present invention, only a portion of the parts or components may be shown for clarity and simplicity. However, those skilled in the art will understand that, under the teachings of the present invention, the required parts or components can be added as needed for specific scenarios.

[0027] It should also be noted that within the scope of this invention, the terms "same", "equal", and "equal to" do not mean that the two values ​​are absolutely equal, but allow for a certain reasonable error. In other words, the terms also cover "substantially the same", "substantially equal", and "substantially equal to".

[0028] It should also be noted that in the description of this invention, the terms "center," "longitudinal," "lateral," "upper," "lower," "front," "rear," "left," "right," "vertical," "horizontal," "top," "bottom," "inner," and "outer," etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. They are used only for the convenience of describing the invention and for simplifying the description, and do not explicitly or implicitly suggest that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, they should not be construed as limitations on the invention. Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance.

[0029] Furthermore, the embodiments of the present invention describe the process steps in a specific order. However, this is only for the convenience of distinguishing each step, and is not a limitation on the order of each step. In different embodiments of the present invention, the order of each step can be adjusted according to the process.

[0030] Existing multi-robot relative localization algorithms for field environments suffer from problems such as easy misjudgment of loop closure detection and poor robustness to outliers.

[0031] Loop closure detection is prone to misjudgment: In the wild, there are often large areas of similar grasslands, forests, hills, or deserts. Descriptors based on appearance or geometric features struggle to distinguish these fundamentally different locations, leading to incorrect judgments of returning to the origin, i.e., false loop closures. Once incorrect relative poses are added to the pose graph, the entire map and trajectory will be severely distorted.

[0032] Poor robustness to outliers: Traditional robust kernel functions reduce the impact of outliers by limiting the upper limit of residual penalty, and the threshold usually needs to be set manually. However, in unstructured field environments, the optimal threshold will constantly change. Too conservative parameters will lead to rejecting too many true loops, while too aggressive parameters will lead to accepting too many spurious loops.

[0033] Therefore, providing a multi-robot relative positioning method that can adapt to complex field environments and has strong robustness is of great theoretical significance and practical value for improving the collaborative operation capability of multi-robot systems in field environments.

[0034] Figure 1 A flowchart of a multi-robot relative localization method for field environments according to an embodiment of the present invention is shown. Figure 2 A logical architecture diagram of a multi-robot relative localization method for field environments according to an embodiment of the present invention is shown. Figure 3 A flowchart illustrating the extraction of global descriptor vectors from a DINO-BEV model according to an embodiment of the present invention is shown. Figure 4 A schematic diagram of a joint factor graph according to an embodiment of the present invention is shown.

[0035] A multi-robot relative localization method for outdoor environments includes the following steps: Step 1: Construct a local point cloud map based on the robot's pose in the local coordinate system and the 3D laser point cloud. Remove the ground point cloud to obtain a non-ground local point cloud map. Project the non-ground local point cloud map onto the bird's-eye view to obtain multiple frames of bird's-eye view spatial height distribution map as historical frames.

[0036] The mobile robot node acquires 3D laser point cloud and IMU data, defining the robot's starting position as the local coordinate origin. Using a laser-inertial odometry system (FAST-LIO), the IMU data and 3D laser point cloud are fused to obtain the robot's pose in the local coordinate system. A local point cloud map is then constructed based on the robot's pose in the local coordinate system and the 3D laser point cloud.

[0037] The local point cloud map and pose in the local coordinate system collected by multiple robots are sent to the central server node. First, the ground point cloud in the local point cloud map is removed, and then it is projected onto the bird's-eye view to obtain a multi-frame bird's-eye view spatial height distribution map. Historical frames are obtained from the bird's-eye view spatial height distribution map.

[0038] Global descriptor vectors are extracted from the bird's-eye view spatial height distribution map. Historical frames are then selected from the bird's-eye view spatial height distribution map to construct a global descriptor database that stores the global descriptor vectors of historical frames.

[0039] Step 11: The mobile robot node uses a laser-inertial odometry system (FAST-LIO) to fuse IMU data with a 3D laser point cloud to obtain the robot's pose in a local coordinate system. Specifically, the IMU data consists of acceleration and angular velocity data, which are first processed through forward integration to obtain the robot's state prediction value. Simultaneously, the residual between the current frame and the previous frame's point-to-plane coordinates is calculated as the measurement error input to a Kalman filter algorithm for pose update, outputting a low-drift, highly smooth, and real-time reliable robot pose estimation result. Then, a local point cloud map is constructed using the point cloud and the robot pose estimation result through a downsampling method.

[0040] Step 12: Use the cloth simulation filtering algorithm CSF to remove the ground point cloud in the local point cloud map, and retain the point cloud above the ground to obtain the non-ground local point cloud map.

[0041] To adapt to the field environment, the cloth resolution parameter of the CSF algorithm is set to 0.3m, which means that details smaller than 0.3m will be ignored by the cloth, which helps to filter out low vegetation and ground noise. The cloth stiffness is set to 2, which is suitable for areas with rapid slope changes, deep ditches, steep slopes or very irregular natural ground.

[0042] Step 13: Project the non-ground local point cloud map after removing ground points onto the bird's-eye view to obtain a multi-frame bird's-eye view spatial height distribution map.

[0043] The 3D laser point cloud of the non-terrestrial local point cloud map is rasterized and projected onto the xy plane. The maximum height in each grid cell, i.e., the maximum value on the z-axis, is recorded to obtain a multi-frame bird's-eye view spatial height distribution map. The bird's-eye view spatial height distribution map is a fixed-size 2D raster image. Key parameters for BEV projection are set as follows: raster resolution R = 0.1m / pixel and projection range L×W = 30m×30m, to ensure that the bird's-eye view spatial height distribution map covers an appropriate local perception range and accuracy. Each frame of the multi-frame 3D laser point cloud from the non-terrestrial local point cloud map is projected onto the bird's-eye view to generate a single frame of the bird's-eye view spatial height distribution map, thus obtaining a multi-frame bird's-eye view spatial height distribution map.

[0044] The spatial height distribution map from a bird's-eye view is normalized and converted into the input format of the DINO-BEV model.

[0045] Step 14: Input the bird's-eye view spatial height distribution map into the self-supervised learning visual base model, extract global descriptor vectors, and establish a global descriptor database that stores the global descriptor vectors of historical frames.

[0046] Here, the self-supervised learning visual foundation model is the DINO-BEV model. DINO (Distillation with No Labels) refers to a model that takes a bird's-eye view as input, specifically using the latest self-supervised learning visual foundation model, DINOv2. The DINOv2 model has the following advantages: The DINOv2 model boasts powerful self-supervised learning capabilities, employing an innovative self-supervised learning paradigm that eliminates the need for manually labeled data. DINOv2's large parameter scale allows it to extract richer, finer-grained visual features from massive amounts of images, enhancing its representational power. DINOv2 effectively addresses the degradation problem of dense feature maps during training, stabilizing the feature structure while allowing for free evolution of feature representations. DINOv2 exhibits exceptional generalization ability, being the first self-supervised learning model to outperform supervised models on intensive tasks such as semantic segmentation, object detection, and depth estimation without fine-tuning. Its extracted features possess strong versatility and robustness, achieving state-of-the-art performance across various computer vision tasks.

[0047] The DINO-BEV model comprises three parts: a linear projection layer, a positional encoding layer, and a Transformer encoder. First, the bird's-eye view spatial height distribution map is input into the linear projection layer, converting pixel values ​​containing height information into feature embeddings that conform to the processing dimensions of the Transformer model. Then, a positional encoding layer is added to imbue the unordered feature sequence with spatial information. After projection and positional encoding, the input is constructed into two token sequences: a classification token and an image patch token. The former is inserted at the beginning of the sequence to aggregate global information and the image, while the latter is a sequence of feature embeddings from individual image patches segmented from the bird's-eye view spatial height distribution map. The complete token sequence, consisting of the classification token and the image patch token, is then input into the Transformer encoder. The Transformer encoder uses a multi-layer self-attention mechanism to perform deep interaction and contextual reasoning on global and local information. Finally, it outputs a global descriptor vector and local feature maps. The global descriptor vector is used for subsequent location recognition and cosine similarity retrieval.

[0048] Step 2: Obtain the bird's-eye view spatial height distribution map at the current moment as the current frame, calculate the cosine similarity between the global descriptor vector of the current frame and the global descriptor vector of the historical frames, select the most similar historical frames as candidate loop closure frames, and complete the loop closure coarse matching.

[0049] Calculate the cosine similarity between the global descriptor of the current frame and the global descriptors of historical frames, and set a similarity threshold. The most similar candidate loop closure frames (K=5) are selected to complete the coarse loop closure matching. Time-constrained filtering is applied to exclude candidate frames with a time interval of less than 30 frames, reducing false matches.

[0050] The main purpose of performing coarse loop closure matching is to filter out candidate loop closure frames, and it also outputs the robot's initial pose value for the current frame. The robot's initial pose value for the current frame is the relative pose between the current frame and the candidate loop closure frames.

[0051] The initial pose value of this robot is an approximate estimate based on similarity inference, which has low accuracy, but can serve as the starting point for subsequent fine registration. Its core function is to provide convergent initial values ​​for loop closure fine matching and avoid loop closure fine matching from getting trapped in local optima.

[0052] Step 3: Obtain the original 3D laser point cloud corresponding to the current frame and multiple candidate loop closure frames, perform point cloud registration, select the valid loop closure frames from the multiple candidate loop closure frames, and obtain the robot's relative pose between the current frame and the valid loop closure frames as loop closure constraints.

[0053] This step involves fine-tuning loop closures. Specifically, the 3D laser point clouds of the current frame and candidate loop closure frames are registered using the Generalized Iterative Closest Point (GICP) algorithm. The identity matrix is ​​used as the initial transformation, and the candidate loop closure frame with the highest fitness score is selected as the valid loop closure frame. For example, a fitness score less than 0.5 can be considered a valid loop closure frame.

[0054] The GICP algorithm precisely aligns the original point clouds of the current frame with those of the candidate loopback frames. If the two frames come from the same location, the registration residual will be small (high fitness score); if it is a mismatch (similar features but different locations), the registration residual will be significantly increased (low fitness score), thus eliminating false candidate frames.

[0055] The GICP algorithm also outputs the robot's relative pose between the current frame and valid loopback frames. The relative pose obtained through GICP can serve as a loopback constraint. This constraint contains two parts: which frames are related to which frames, and the robot's relative pose between frames. Point cloud distortion correction was performed before point cloud registration.

[0056] Step 4: Collect all historical loop constraints detected between the main robot and multiple sub-robots to obtain a loop constraint set. Group the loop constraints by robot pair, extract all historical loop constraints between robot pairs (i, j) to obtain a loop constraint subset, and add the loop constraints between the current frame and the valid loop frame to the corresponding loop constraint subset, and update the loop constraint set.

[0057] The master robot here is a robot selected from multiple robots, the others are called sub-robots, and the master robot can be changed. A robot pair can consist of a master robot and a sub-robot.

[0058] Step 5: For the closure constraints of different robots in the closure constraint set, use the Pairwise Consistency Maximization (PCM) algorithm to perform geometric verification on each pair of closure constraints, remove outliers in the closure constraint set, and obtain a reliable closure constraint set.

[0059] PCM is used to perform consistency checks on each pair of closure constraints. By analyzing the geometric closed-loop consistency between closures, a reliable set of closure constraints is selected.

[0060] Step 51: Use the pairwise consistency maximization algorithm to construct a quadrilateral geometric verification model for each pair of lapsing constraints, where... For the odometry pose node of robot A, For the odometry pose node of robot B , and , Each has a closure constraint, and the pose nodes are calculated separately. mutual pose transformation , , .

[0061] Step 52, calculate the consistency error r ,in To perform pose composition, multiple relative pose transformations are chained together. To calculate the relative transformation or residual between two poses using pose difference calculation, based on Mahalanobis distance. To calculate the consistency metric, the geometric residuals are transformed into a statistic that follows a chi-square distribution, thus taking into account the uncertainty of the constraints. Use a 95% confidence level to set the threshold, i.e., the PCM threshold. .

[0062] Step 53, Build The consistency matrix is ​​defined as follows: N is the total number of lapsing constraints, where 1 represents geometric consistency and 0 represents geometric conflict. The consistency matrix is ​​converted into an undirected graph format, and the fast maximum clique heuristic is used to solve for the maximum clique, i.e., to extract the largest mutually consistent subset of lapsing constraints.

[0063] Step 54: Based on the maximum clique solution results, closure constraints within the maximum clique are retained, while closure constraints outside the maximum clique are marked as outliers and removed. The number of removed outliers and the number of retained closure constraints are counted, the closure constraint database is updated, outlier records are removed, and a set of reliable closure constraints that have passed PCM verification is output for subsequent optimization.

[0064] Step 6: Generate closure factors based on the reliable closure constraint set, and construct a joint factor graph containing collaborative mapping data from multiple robots. The joint factor graph includes pose nodes, odometry factors, closure factors, and prior factors.

[0065] Pose nodes represent the pose state variables of each robot at different times. Each pose node contains six degrees of freedom: three-dimensional position and three-dimensional rotation. Pose nodes can be obtained through the robot's own laser-inertial odometry.

[0066] Odometry factor: Represents the relative pose between adjacent keyframes of the same robot, obtained based on laser odometry or visual odometry measurements.

[0067] The closure factor is an optimization term that expresses the closure constraint in mathematical form. The closure factor represents the closure constraint detected within the same robot or between different robots. The closure factor includes the intra-robot closure factor and the inter-robot closure factor.

[0068] Prior factors: Represent prior constraints on specific pose nodes. Global anchor point prior factors are set for the master robot, and high-noise prior factors are set for other sub-robots.

[0069] Joint factor diagram as follows Figure 4 As shown, Figure 4 The triangles in the diagram represent pose nodes, which are the poses of robot i and j at time t. Figure 4 The solid arrow represents the odometry factor, which is the robot's own odometry constraint. Figure 4 The dashed arrows represent the closure factors between robots, which are the closure constraints between multiple robots.

[0070] Step 7: Adaptively adjust the weights of the loop factors in the joint factor graph, remove loop factors with weights below the threshold, update the joint factor graph, and simultaneously update the pose estimation results and global consistent trajectory estimation results of all robots.

[0071] A Graduated Non-Convexity (GNC) algorithm is employed, along with the Levenberg-Marquardt (LM) algorithm as the underlying solver, to iteratively solve the weighted least squares problem of the GNC transformation. Adaptive weight adjustments are made to the closure factors in the joint factor graph. Closure factors with weights below a threshold are removed, the joint factor graph is updated, and the pose estimation results and globally consistent trajectory estimation results for all robots are also updated.

[0072] Step 71: Set the odometer noise, loop closure noise, and prior noise in the joint factor plot. Odometer noise. Loop noise Prior noise Set the relinearization threshold to 0.05.

[0073] Step 72: Set the parameters of the asymptotic nonconvexity algorithm, and use the asymptotic nonconvexity algorithm to adaptively adjust the weights of the closure factors.

[0074] Initial robust parameters growth factors Maximum number of iterations K=5. Set outlier detection threshold: interior point confidence threshold. Weight threshold .

[0075] Define a known set of reliable constraints, label the prior factors and odometer factors as reliable constraints, and apply the GNC adaptive weight adjustment mechanism only to the closure factor. Use the robust Geman-McClure error function as... The robust loss function is defined as follows: r represents the residual value of the factor. For reliable constraints with small residuals, the loss function value approximates the standard squared loss, maintaining optimization accuracy. For unreliable constraints with large residuals, the growth rate of the loss function value slows significantly or even saturates, thus limiting the contribution weight of outliers to the objective function.

[0076] Set robust initial parameters for the asymptotic nonconvex algorithm. Specifically, By traversing all cyclic factors , It is the set of all cyclic factors in the joint factor graph, and the cyclic factors are calculated in the initial state. The squared error With the inherent saturation threshold of this cyclic factor The maximum value obtained from the ratio is used to adaptively adjust the weights of the laparoscing factors using an asymptotic nonconvexity algorithm. The Levenberg-Marquardt (LM) algorithm is then used to solve the weighted least squares problem to update the weights. Gradually reduce robust parameters This achieves the transition from standard squared loss to a strongly robust function, where... It is a growth factor. Optimization will continue until... Reach the endpoint value If convergence is determined, the current weight value is fixed.

[0077] Step 73: Iterate through the final weight values ​​of all loop closure factors, identify loop closure factors with weights below the threshold, mark them as outlier candidates, remove them from the joint factor graph, update the globally consistent trajectory estimate for each robot, and store them in groups by robot ID.

[0078] This invention employs an asymptotic nonconvexity algorithm to adaptively adjust the weights of closure factors, removing outliers. It overcomes the rigidity of traditional robust kernel functions that reduce the impact of outliers by limiting the upper limit of residual penalties. It adapts to the dynamic changes in outdoor scenes and can automatically adapt to the fluctuations in outlier distribution caused by changes in lighting, occlusion, and sparse features in outdoor scenes. It eliminates the need for repeated manual parameter adjustments and perfectly avoids the problems of traditional kernel functions, which either conservatively reject too many true closures or aggressively accept too many false closures.

[0079] Furthermore, employing the Geman-McClure function, specifically designed for strong outlier optimization, as a robust loss function enhances the stability of outlier suppression. This function is characterized by: for small error normal terms, the loss grows smoothly with the error, ensuring its effective constraint on the model; for large error outliers, the loss growth converges rapidly, preventing an explosion in loss due to outliers, thus significantly reducing the interference of outliers on the optimization of the closure factor.

[0080] The following example, using the collaborative localization of three special field robots in a complex mountainous environment, illustrates the specific implementation process of the method of this invention.

[0081] 1. System Configuration The system comprises three specialized field robots and one server. Each robot node is equipped with a DJI MID360 3D LiDAR, with a scanning frequency of 10Hz, a horizontal field of view of 360°, and a vertical field of view of 59°. The LiDAR includes an IMU, enabling simultaneous acquisition of point cloud data and acceleration / angular velocity data. The central server node is a high-performance mobile server equipped with GPU computing power, allowing for online inference of deep neural networks. The robots and the ground station communicate via a wireless network, sharing loop closure detection results and optimized pose information.

[0082] 2. Training of the DINO-BEV model.

[0083] DINOv2 (ViT-S / 14 distilled) was used as the pre-trained model for the DINO-BEV model, and fine-tuning was performed on a self-collected field environmental point cloud dataset. The model requires point cloud BEV images as input, which are then compressed into a 384-dimensional vector. The training objective of the model can be... The expression represents the query point cloud, where q represents the query point cloud, and i and j represent any two frames of point clouds in the point cloud dataset. The closer the query point cloud is to the point cloud in the dataset, the higher the corresponding vector. The closer the distance in the vector space.

[0084] To achieve the training objective, a fine-tuning loss function is designed: Where c is a constant, For the negative sample set, To query samples, For the corresponding positive samples in the database, Let be the i-th negative sample in the point cloud dataset. The Adam optimizer is used during training, with a learning rate of . .

[0085] 3. Mobile robot node.

[0086] Using the robot's initial position as the local coordinate origin, the robot's pose in the local coordinate system is obtained by fusing IMU data and 3D laser point cloud through the laser-inertial odometry FAST-LIO.

[0087] IMU data includes acceleration and angular velocity data. The IMU data is first processed through forward integration to obtain the robot's state prediction. Simultaneously, the residual between the point and the plane in the current frame and the previous frame is calculated as the measurement error input to the Kalman filter algorithm for pose update, outputting a low-drift, highly smooth, and real-time reliable robot pose estimation result. Then, a local point cloud map is constructed using the point cloud and the robot pose estimation result through downsampling.

[0088] To improve point cloud density, the point cloud data from time t-10 to time t is unified to the coordinate system at time t using pose estimation, thus constructing a local point cloud map. Before constructing the local point cloud map, distortion correction is performed on these 10 point cloud frames. Finally, the robot ID, the local map at time t, and the pose estimation results are transmitted to the ground station via wireless network.

[0089] 4. Central server node.

[0090] 4.1 Local map feature extraction: The cloth simulation filtering algorithm is used to remove ground points from the local map point cloud. The point cloud after removing ground points is rasterized and projected onto the xy plane. The maximum height in each grid, i.e. the maximum value of the z-axis, is recorded to obtain a multi-frame bird's-eye view spatial height distribution map. Finally, the bird's-eye view spatial height distribution map is input into the DINO-BEV model to obtain the global descriptor vector of the current frame and is normalized.

[0091] 4.1 Two-stage loop closure detection: Phase 1: Calculate the cosine similarity between the global descriptor vector of the current frame and the global descriptor vectors of historical frames. Set a similarity threshold. The five most similar candidate loop closure frames (K=5) are selected as candidate loop closure frames to complete the coarse loop closure matching. A time-constrained filter is applied to exclude candidate frames with a time interval of less than 30 frames, reducing false matches.

[0092] The references to historical frames, current frames, and global descriptor vectors are as described above and will not be repeated here.

[0093] The second stage involves acquiring the original 3D laser point clouds corresponding to the current frame and the candidate loop closure frames, registering the 3D laser point clouds of the current frame and the candidate loop closure frames using the GICP algorithm, using the identity matrix as the initial transformation, selecting the candidate loop closure frame with the highest fitness score as the effective loop closure frame, obtaining the robot's relative pose between frames as the loop closure constraint, and sending the relative pose to the ground station.

[0094] 4.2. Abnormal loop removal based on PCM algorithm: Collect all historical lap-loop constraints between robot pairs (i, j) and calculate the consistency error r. Based on Mahalanobis distance The consistency metric is calculated, and the geometric residuals are transformed into a statistic that follows a chi-square distribution. To account for the uncertainty of the constraints, a threshold, known as the PCM threshold, is set using a 95% confidence level. Constructing a consistency matrix Let N be the total number of closure constraints. Solve for the maximum clique. Based on the maximum clique solution, closure constraints within the maximum clique are retained, while closure constraints outside the maximum clique are marked and removed as outliers. Count the number of removed outliers and the number of retained closure constraints, update the closure constraint set, and remove records of outlier constraints. Output a reliable set of closure constraints that has passed PCM validation for subsequent optimization.

[0095] 4.3 Robust Optimization Based on GNC Algorithm: Construct a joint factor graph for multiple robots, including pose nodes, odometry factors, closure factors, and prior factors. Set anchor point constraints to fix the global coordinate system for the master robot. Forcibly fix a key pose node of the selected master robot (usually the first robot to start / collect data) at the origin of the global coordinate system (or a preset reference point). By adding strong constraints to lock the reference anchor of the coordinate system, ensure that the global coordinate system does not drift and all robot poses have a unified reference benchmark during the optimization of the joint factor graph for multiple robots.

[0096] Odometry noise, loop closure noise, and prior noise are defined in the joint factor map. Robust optimization is performed using the GNC algorithm: initial robustness parameters, decay factors, and number of iterations are set; odometry and prior factors are marked as known reliable constraints; weight adjustments are applied only to loop closure factors. The Geman-McClure robust loss function is used to define the optimization objective. The weighted least squares problem is solved iteratively using the Levenberg-Marquardt algorithm, asymptotically reducing the robustness parameters to transition from convex optimization to strongly robust optimization until the weights converge, resulting in a globally consistent multi-robot trajectory.

[0097] By utilizing the precise odometry provided by the FAST-LIO laser-inertial odometry system and the joint factor graph optimization steps, multi-robot systems can eliminate cumulative odometry drift and achieve globally consistent trajectories.

[0098] While some embodiments of the present invention have been described in this application, those skilled in the art will understand that these embodiments are merely illustrative. Numerous variations, alternatives, and improvements will arise in those skilled in the art under the teachings of this invention without departing from its scope. The appended claims are intended to define the scope of the invention and thereby cover methods and structures within the scope of the claims themselves and their equivalents.

Claims

1. A multi-robot relative positioning method for outdoor environments, characterized in that, Includes the following steps: A local point cloud map is constructed based on the robot's pose in the local coordinate system and the 3D laser point cloud. The ground point cloud is removed to obtain a non-ground local point cloud map. The non-ground local point cloud map is then projected onto a bird's-eye view to obtain multiple frames of bird's-eye view spatial height distribution map as historical frames. Obtain the spatial height distribution map of the bird's-eye view at the current moment as the current frame, and filter the multiple historical frames most similar to the current frame as candidate loopback frames; Valid loop closure frames are selected from multiple candidate loop closure frames, and the robot's relative pose between the current frame and the valid loop closure frames is obtained as the loop closure constraint. Collect all historical loop constraints detected between the main robot and multiple sub-robots to obtain a loop constraint set. Group the loop constraints by robot pair, extract all historical loop constraints between robot pairs to obtain a loop constraint subset, and add the loop constraints between the current frame and the valid loop frame to the corresponding loop constraint subset, and update the loop constraint set. Remove outliers from the set of closure constraints to obtain a reliable set of closure constraints; Based on a reliable set of laparoscopic constraints, laparoscopic factors are generated, and a joint factor graph is constructed. as well as Adaptive weight adjustment is performed on the laparoscopic factors in the joint factor graph, laparoscopic factors with weights below the threshold are removed, the joint factor graph is updated, and the pose estimation results and globally consistent trajectory estimation results of all robots are updated at the same time.

2. The method according to claim 1, characterized in that, Global descriptor vectors are extracted based on the spatial height distribution map from a bird's-eye view, and a global descriptor database is constructed to store the global descriptor vectors of historical frames, wherein: Normalize the spatial height distribution map from a bird's-eye view; The bird's-eye view spatial height distribution map is input into the self-supervised learning visual foundation model DINOv2 to extract global descriptor vectors, which are then normalized.

3. The method according to claim 2, characterized in that, Calculate the cosine similarity between the global descriptor vector of the current frame and the global descriptor vector of the historical frames, and select the most similar historical frames as candidate loop closure frames. The original 3D laser point cloud corresponding to the current frame and multiple candidate loop closure frames is obtained, and point cloud registration is performed. Valid loop closure frames are selected from multiple candidate loop closure frames, and the relative pose of the robot between the current frame and the valid loop closure frames is obtained as the loop closure constraint.

4. The method according to claim 1, characterized in that, The pairwise consistency maximization algorithm is used to perform geometric verification on each pair of closure constraints, remove outliers from the closure constraint set, and obtain a reliable closure constraint set.

5. The method according to claim 4, characterized in that, The pairwise consistency maximization algorithm is used to perform geometric verification on each pair of loop closure constraints, removing outliers from the set of loop closure constraints, including: The pairwise consistency maximization algorithm (PCM) is used to construct a quadrilateral geometric verification model for each pair of closure constraints, and pose nodes are set. ,in For the odometry pose node of robot A, For the odometry pose node of robot B, , and , Each has a closure constraint, and the pose nodes are calculated separately. mutual pose transformation , , , ; Calculate the consistency error ,in To perform pose composition, multiple relative pose transformations are chained together. To calculate the relative transformation or residual between two poses using pose difference calculation, a consistency metric is calculated based on Mahalanobis distance, and the geometric residual is converted into a chi-squared value. The statistics of the distribution; Build The consistency matrix, where N is the total number of loop constraints, and a matrix element of 1 indicates geometric consistency and 0 indicates geometric conflict; The consistency matrix is ​​converted into an undirected graph format. The fast maximum clique heuristic algorithm is called to solve for the maximum clique and extract the largest mutually consistent subset of cyclic constraints. Based on the maximum clique solution results, closure constraints within the maximum clique are retained, while closure constraints outside the maximum clique are marked as outliers and removed. The number of removed outliers and the number of retained closure constraints are counted, the closure constraint database is updated, outlier records are removed, and a reliable set of closure constraints is output.

6. The method according to claim 1, characterized in that, Based on a reliable set of closure constraints, closure factors are generated, and a joint factor graph containing collaborative mapping data from multiple robots is constructed. The joint factor graph includes pose nodes, odometry factors, closure factors, and prior factors.

7. The method according to claim 1, characterized in that, An asymptotic nonconvexity algorithm is adopted, and the Levenberg-Marquardt algorithm is used as the basic solver to iteratively solve the weighted least squares problem transformed by the asymptotic nonconvexity algorithm, and adaptive weight adjustment is performed on the cyclic factors in the joint factor graph. Remove loop factors with weights below the threshold, update the joint factor graph, and simultaneously update the pose estimation results and globally consistent trajectory estimation results for all robots.

8. The method according to claim 7, characterized in that, Set the odometer noise, loop closure noise, and prior noise in the joint factor plot; Set the parameters of the asymptotic nonconvexity algorithm and use the asymptotic nonconvexity algorithm to adaptively adjust the weights of the closure factor; Iterate through the final weight values ​​of all loop closure factors, identify loop closure factors with weights below the threshold, mark them as outlier candidates, remove them from the joint factor graph, and output the pose estimation results and globally consistent trajectory estimation results for all robots.

9. The method according to claim 8, characterized in that, The parameters for setting the asymptotic nonconvexity algorithm include: Initial robust parameters growth factors Maximum number of iterations K=5, set outlier detection threshold: interior point confidence threshold Weight threshold ; Define a known set of reliable constraints, label the prior factors and odometer factors as reliable constraints, apply an asymptotic nonconvexity algorithm with adaptive weight adjustment only to the closure factors, and use the robust Giman-McLuhr error function as... Robust loss function, where r is the residual value of the factor; Set the initial robustness parameters for the asymptotic nonconvexity algorithm. , By iterating through all factors , It is the set of all cyclic factors in the joint factor graph, and the cyclic factors are calculated in the initial state. The squared error With the inherent saturation threshold of the cyclic factor The maximum value obtained from the ratio; When using an asymptotically nonconvex algorithm to adaptively adjust the weights of laparoscopic factors, the Levenberg-Marquardt algorithm is used to solve for weighted least squares and update the weights. Gradually reduce robust parameters Until robust parameters Reach the endpoint value If convergence is determined, the current weight values ​​are fixed, where It is a growth factor.

10. The method according to claim 1, characterized in that, The robot's starting position is defined as the local coordinate origin. The robot's pose in the local coordinate system is obtained by fusing the IMU data collected by the robot with the 3D laser point cloud through laser-inertial odometry. Construct a local point cloud map based on the robot's pose in a local coordinate system and the 3D laser point cloud; Remove the ground point cloud to obtain a non-ground local point cloud map; The 3D laser point cloud of the non-ground local point cloud map is rasterized and projected onto the xy plane. The maximum height in each grid cell, i.e. the maximum value of the z-axis, is recorded to obtain a multi-frame bird's-eye view spatial height distribution map as a historical frame.

Images