Multi-robot localization and mapping method based on relative coordinate system uncertainty estimation

Abstract

The present invention provides a multi-robot positioning and mapping method based on relative coordinate system uncertainty estimation. First, the least squares problem is constructed by applying Lie algebra, and the Jacobian matrix of the cost function is derived; then, the error propagation formula is used to evaluate the uncertainty, including the posture of a single robot and the covariance of the closed loop between robots; by adopting the composite posture technology, the continuous posture estimation is fused into a unified estimation to simplify the uncertainty estimation; it can be seen that the present invention integrates the closed loop between robots and the posture estimation information of a single robot, so as to accurately estimate the relative coordinate system transformation between robots, which not only improves the accuracy of the estimation, but also enhances the flexibility and practicality of the uncertainty model in multi-machine positioning and mapping.

Description

Technical Field

[0001] The present invention belongs to the technical field of real-time positioning and map construction, and in particular relates to a multi-robot positioning and mapping method based on relative coordinate system uncertainty estimation. Background Art

[0002] In the field of robotics, SLAM (Simultaneous Localization and Mapping) technology is the key to autonomous navigation and environmental understanding. It provides a theoretical and technical basis for robots to locate and map in unknown environments. The development of multi-robot collaborative SLAM technology marks a major advancement in this field. It improves the efficiency and accuracy of solving complex positioning and mapping problems by integrating the intelligence of multiple robots. This collaborative mode is particularly suitable for the exploration of vast and complex environments, such as large-area map construction, search and rescue missions at disaster sites, underground space surveys, etc., which significantly improves the efficiency of task execution and the robustness of the system, and enhances the ability to handle large-scale and complex tasks.

[0003] However, the implementation of multi-robot SLAM systems faces many challenges, especially the problem of estimating the relative coordinate transformation between robots. Since each robot operates independently in its local coordinate system, accurate alignment between different coordinate systems becomes crucial. Inconsistencies between coordinate systems may lead to errors in the shared map, which in turn causes misalignment in positioning and navigation, ultimately affecting the success of the entire collaborative task.

[0004] Usually, the global coordinate system and initial relative pose between robots are unknown, and the relative transformation between the local coordinate systems of the robots needs to be estimated through data association.

[0005] Although some progress has been made in estimating relative coordinate transformations, these estimates are often susceptible to errors caused by factors such as sensor noise and erroneous data association. Especially in multi-robot SLAM systems, these uncertainties may be further amplified when processing data from various sources. Therefore, developing a method that can effectively estimate these uncertainties is crucial to achieve accurate relative coordinate transformations. Summary of the invention

[0006] In order to solve the problem of uncertainty estimation of relative coordinate system transformation in a multi-robot SLAM system, the present invention provides a multi-robot positioning and mapping method based on relative coordinate system uncertainty estimation, aiming to improve the accuracy and efficiency of the multi-robot SLAM system when performing trajectory merging tasks, especially in estimating the relative coordinate system transformation between robots and its uncertainty, showing significant technical advantages.

[0007] A multi-robot positioning and mapping method based on relative coordinate system uncertainty estimation comprises the following steps:

[0008] S1: Each robot runs independently in its own local coordinate system and records its own motion trajectory, and sends the local map formed by its own motion trajectory to the central robot;

[0009] S2: The central robot uses the posture represented by Lie algebra to construct the posture error function of the posture transformation between itself and each non-central robot;

[0010] S3: Use the trust region algorithm to solve the posture error function to obtain the relative coordinate system transformation variables between the central robot α and each non-central robot β

[0011] S4: Take the local coordinate system where the central robot is located as the global coordinate system, and transform the variables according to each relative coordinate system The local maps of each non-central robot are converted to the global coordinate system respectively to obtain a global map that integrates all local maps.

[0012] Furthermore, in step S2, the central robot uses the posture represented by Lie algebra to construct the posture error function of the posture transformation between itself and each non-central robot as follows:

[0013]

[0014] in, Represents the relative coordinate system transformation variable between the global coordinate system of the central robot α and the local coordinate system of any non-central robot β; represents the least squares-based error function between the central robot α and the non-central robot β; represents the absolute position and posture of the central robot α in the i-th frame image collected by itself; represents the absolute position of the non-central robot β in the j-th frame image collected by itself; Θ represents the set formed by the combination of key frames, where the two frames of images in each group of key frames come from the image sequence collected by the central robot α and the image sequence collected by the non-central robot β, and the similarity of the two images is greater than the set threshold; express and The relative position between ij is the uncertainty covariance matrix representing the key frame combination; In(·) represents the logarithmic mapping operation, exp(·) represents the exponential mapping operation; ∧ represents the antisymmetric operator, ∨ represents the inverse antisymmetric operator, T represents transpose, and min represents the minimum value.

[0015] Furthermore, the covariance matrix Σij The calculation method is as follows:

[0016]

[0017] Among them, h represents Abbreviation of Represents the error function For absolute pose The partial derivative of Represents the error function For absolute pose The partial derivative of Represents the error function Relative posture The partial derivative of .

[0018] Furthermore, the absolute position Relative posture Integrate to obtain a unified composite pose Among them, α' represents the absolute position of the center robot α Incorporating relative pose The composite pose after The corresponding virtual center robot;

[0019] Based on absolute pose Simplified covariance matrix Σ ij as follows:

[0020]

[0021] Among them, Φ ij For absolute position The covariance of ij For composite pose The covariance of ij Indicates absolute position and composite pose The cross-covariance of Represents the error function For absolute pose The partial derivative of Represents the error function For composite pose The partial derivative of .

[0022] Furthermore, the composite pose The covariance Ψ ij The calculation method is as follows:

[0023]

[0024] in, Relative posture The disturbance, and the disturbance Normal distribution For disturbance The variance of the normal distribution is consistent with E[·] represents the expected operation, For absolute position The disturbance, and the disturbance Normal distribution For disturbance The variance of the normal distribution is consistent with For absolute position In the disturbance , Ad(·) represents the adjoint operation.

[0025] Furthermore, the error function For absolute pose The partial derivative of The calculation method is as follows:

[0026]

[0027] in, Indicates absolute position The left Jacobian matrix of Indicates absolute position The least squares-based error function The right Jacobian matrix of express The inverse of , Ad(·) represents the adjoint operation.

[0028] Furthermore, the error function For composite pose The partial derivative of The calculation method is as follows:

[0029]

[0030] in, Represents composite pose The left Jacobian matrix of Represents composite pose The least squares-based error function The right Jacobian matrix of express The inverse of , Ad(·) represents the adjoint operation.

[0031] Beneficial effects:

[0032] 1. The present invention provides a multi-robot positioning and mapping method based on relative coordinate system uncertainty estimation. First, the least squares problem is constructed by applying Lie algebra, and the Jacobian matrix of the cost function is derived; then, the error propagation formula is used to evaluate the uncertainty, including the posture of a single robot and the covariance of the closed loop between robots; by adopting the composite posture technology, the continuous posture estimation is fused into a unified estimation to simplify the uncertainty estimation; it can be seen that the present invention integrates the closed loop between robots and the posture estimation information of a single robot, so that the relative coordinate system transformation between robots can be accurately estimated, which not only improves the accuracy of the estimation, but also enhances the flexibility and practicality of the uncertainty model in multi-machine positioning and mapping.

[0033] 2. The present invention provides a multi-robot positioning and mapping method based on relative coordinate system uncertainty estimation. By utilizing the cost function represented by Lie algebra and the accurately derived Jacobian matrix, a more accurate relative pose transformation calculation method is provided for calculating the relative pose transformation between robots. This method not only improves the mathematical expression of the system's sensitivity to pose transformation, but also optimizes the accuracy of pose estimation, thereby improving the overall performance of the SLAM system.

[0034] 3. The present invention provides a multi-robot positioning and mapping method based on relative coordinate system uncertainty estimation. By applying the error propagation law and composite pose technology, detailed estimation and effective management of uncertainty are achieved. This method can accurately evaluate the uncertainty introduced by factors such as sensor noise and data processing errors, and reduce the impact of these uncertainties on the final pose estimation through reasonable data fusion.

[0035] 4. The present invention provides a multi-robot positioning and mapping method based on relative coordinate system uncertainty estimation, and provides a clear methodology to determine the expected value and covariance of composite poses, further enhancing the robustness and reliability of the system.

[0036] 5. The present invention provides a multi-robot positioning and mapping method based on relative coordinate system uncertainty estimation. By applying the error propagation law, the error caused by sensor noise and uncertainty in the data processing process is estimated, thereby establishing a quantitative model of uncertainty; then the proposed uncertainty estimation model is used to perform weighted optimization on the least squares problem; this process includes applying the joint covariance matrix as a weighting factor to ensure that each measurement value is appropriately adjusted according to the impact of its uncertainty on the final estimation result. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] Figure 1 A flowchart of a multi-robot positioning and mapping method based on relative coordinate system uncertainty estimation provided by the present invention;

[0038] Figure 2 The two robots provided by the present invention independently run the track and map of the SLAM system;

[0039] Figure 3 A conversion relationship diagram between the local coordinate system and the global coordinate system provided by the present invention;

[0040] Figure 4 This is the combined positioning and mapping results of the two robots provided by the present invention. DETAILED DESCRIPTION

[0041] In order to enable those skilled in the art to better understand the solution of the present application, the technical solution in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application.

[0042] The main goal of the present invention is to solve the uncertainty estimation problem of relative coordinate system transformation in a multi-robot SLAM system to improve the accuracy and reliability of the system. In response to this challenge, the present invention proposes a multi-robot positioning and mapping method based on relative coordinate system uncertainty estimation. The method proposes a new uncertainty estimation model that can accurately evaluate the uncertainty of relative coordinate system transformation in a multi-robot system.

[0043] To achieve the above objectives, the present invention uses Lie algebra to represent the least squares problem and derives the specific form of the Jacobian matrix. Then, the error propagation law is used to comprehensively consider the uncertainty factors from each robot posture and the closed loop between robots. Furthermore, in order to optimize the uncertainty estimation process, the present invention proposes a method of fusing two continuous postures into a unified combined posture, which not only simplifies the uncertainty calculation process, but also improves the accuracy and efficiency of the estimation.

[0044] Specifically, Figure 1 As shown, a multi-robot positioning and mapping method based on relative coordinate system uncertainty estimation includes the following steps:

[0045] S1: Each robot runs independently in its own local coordinate system and records its own motion trajectory, and sends the local map formed by its own motion trajectory to the central robot;

[0046] It should be noted that when the multi-robot system is initialized, the position and posture of each robot are initialized first, including determining the position and posture of each robot relative to the initial position and posture, providing initial data for the subsequent relative coordinate system transformation estimation. Among them, the relative position and posture include the relative three-dimensional direction rotation and relative three-dimensional translation vector between robots. The local map includes the information of three-dimensional map points.

[0047] S2: The central robot uses the posture represented by Lie algebra to construct the posture error function of the posture transformation between itself and each non-central robot;

[0048] like Figure 2 As shown in FIG. 1 , it shows the trajectory and map point examples of two robots performing SLAM tasks in different coordinate systems. The present invention first uses Lie algebra to represent the pose transformation, and then defines and constructs an error function based on the least squares problem. The problem aims to find the optimal relative coordinate system transformation estimate so that the difference between the prediction obtained based on the robot's observations and motion model and the actual observation data is minimized;

[0049] Based on this, the central robot uses the posture represented by Lie algebra to construct the posture error function of the posture transformation between itself and each non-central robot as follows:

[0050]

[0051] in, Represents the relative coordinate system transformation variable between the global coordinate system of the central robot α and the local coordinate system of any non-central robot β; represents the least squares-based error function between the central robot α and the non-central robot β; represents the absolute position and posture of the central robot α in the i-th frame image collected by itself; represents the absolute position of the non-central robot β in the j-th frame image collected by itself; Θ represents the set formed by the combination of key frames, where the two frames of images in each group of key frames come from the image sequence collected by the central robot α and the image sequence collected by the non-central robot β, and the similarity of the two images is greater than the set threshold; express and The relative position between ij is the uncertainty covariance matrix representing the key frame combination; In(·) represents the logarithmic mapping operation, exp(·) represents the exponential mapping operation; ∧ represents the antisymmetric operator, ∨ represents the inverse antisymmetric operator, T represents transpose, and min represents the minimum value.

[0052] Assumptions A small change The present invention derives right The partial derivative of

[0053]

[0054] Among them, the left Jacobian matrix represents the first-order approximation of the perturbation; Ad(·) represents the adjoint operation; the right Jacobian matrix About The first-order approximation of is expressed as

[0055]

[0056] Where ξ=[φ,ρ], φ is the first three terms of ξ, representing the rotation part of Lie algebra; ρ is the first three terms of ξ, representing the translation part of Lie algebra; I represents the unit matrix. Further, the uncertainty of the present invention refers to the degree of unknown or inaccurate measurement or estimation. The present invention uses the covariance matrix Σ ij To quantify uncertainty. The present invention defines the information matrix is the inverse of the covariance matrix. The larger the value of each element in the matrix, the smaller the uncertainty of the corresponding measurement value. Therefore, the present invention uses the information matrix as a weighting factor. By converting the covariance matrix into an information matrix, the weighted least squares problem is processed.

[0057] Specifically, in order to accurately integrate the overall uncertainty, the present invention applies the error propagation formula to give the covariance matrix Σ ij The calculation method is as follows:

[0058]

[0059] Among them, h represents Abbreviation of Represents the error function For absolute pose The partial derivative of Represents the error function For absolute pose The partial derivative of Represents the error function Relative posture The partial derivative of .

[0060] It should be noted that if Figure 3 As shown, due to and is a variable in the same local coordinate system, so the absolute position can be Relative posture Integrate to form a unified absolute pose That is to say, in order to reduce the uncertainty of the overall system, the present invention analyzes the continuous posture relationship and converts the absolute posture Relative posture Integrate to obtain a unified composite pose Among them, α' represents the absolute position of the center robot α Incorporating relative pose The composite pose after The corresponding virtual center robot;

[0061] Based on this, the present invention defines small disturbance as zero-mean Gaussian distribution, and assumes and They are and The covariance of and The perturbation distributions are and Based on absolute pose The covariance matrix Σ ij Further simplification is as follows:

[0062]

[0063] Among them, Φ ij For absolute position The covariance of ij For composite pose The covariance of ij Indicates absolute position and composite pose The cross-covariance of Represents the error function For absolute pose The partial derivative of Represents the error function For composite pose The partial derivative of .

[0064] It can be seen that a merging strategy is adopted to merge continuous poses into a unified pose, and the expected value and covariance of the merged pose are given to simplify and improve the efficiency of the estimation process; this method not only simplifies the covariance matrix Σ ij , which simplifies the derivation of the Jacobian matrix and the calculation process of the joint covariance matrix, reduces the amount of calculation for estimating the marginal covariance and improves the accuracy of the estimation.

[0065] It should be noted that the present invention uses the posture represented by Lie algebra to construct the expression of the posture error function, and finds the minimum value of the error function based on the least squares theory. At the same time, in order to better understand and analyze the impact of posture transformation on the system, the present invention derives the Jacobian matrix, which provides a mathematical representation of the sensitivity of small changes in posture transformation. This step also provides the necessary mathematical basis for subsequent error analysis and optimization. At the same time, in the covariance matrix Σ ij In the calculation formula, Φ ij and ij is known, ij and It is unknown, which will be described in detail below. ij and That is to say, according to the above formula, it is necessary to determine the composite pose The expected value and covariance of

[0066] Assumptions and The perturbation distributions are and are uncorrelated with each other. Under a given disturbance

[0067]

[0068] This means

[0069]

[0070] Where E[·] represents the expected operation, and They are and The expected value under perturbation.

[0071] therefore, Approximately equivalent to

[0072] The next task is to calculate the covariance Ψ ij . Known theorem:

[0073]

[0074] Where Cov(·) represents the cross-covariance operation; ξ1 and ξ2 represent the autocovariance of the two variables.

[0075] By applying the theorem, the present invention can obtain the composite posture The covariance Ψ ij The calculation method is as follows:

[0076]

[0077] In a given and In the case of, the present invention replaces each item one by one, and obtains

[0078]

[0079] in, Relative posture The disturbance, and the disturbance Normal distribution For disturbance The variance of the normal distribution is consistent with E[·] represents the expected operation, For absolute position The disturbance, and the disturbance Normal distribution For disturbance The variance of the normal distribution is consistent with For absolute position In the disturbance , Ad(·) represents the adjoint operation.

[0080] So far, the present invention has obtained the expectation and covariance of the composite pose.

[0081] Furthermore, the present invention uses Lie algebra to derive the error function For absolute pose The partial derivative of The calculation method is as follows:

[0082]

[0083] in, Indicates absolute position The left Jacobian matrix of Indicates absolute position The least squares-based error function The right Jacobian matrix of express The inverse of , Ad(·) represents the adjoint operation.

[0084] Furthermore, the present invention uses Lie algebra to derive the error function For composite pose The partial derivative of The calculation method is as follows:

[0085]

[0086] in, Represents composite pose The left Jacobian matrix of Represents composite pose The least squares-based error function The right Jacobian matrix of express The inverse of , Ad(·) represents the adjoint operation.

[0087] It should be noted that when solving the weighted least squares problem based on Lie algebra, the covariance matrix Σ ijis used as a weighting factor for the measured values, and the present invention quantifies and integrates the uncertainty of all inputs in a systematic way. Based on the above formula, the present invention can calculate the covariance matrix Σ ij .

[0088] S3: Use the trust region algorithm to solve the posture error function to obtain the relative coordinate system transformation variables between the central robot α and each non-central robot β

[0089] It should be noted that the present invention adopts a gradient-based optimization algorithm, the Trust Region Algorithm (TR), to solve the least squares problem. The key to the Trust Region Algorithm is to accurately estimate the cost function and its uncertainty, which is a prerequisite for ensuring the effectiveness and stability of the algorithm. By carrying out detailed analysis and quantification of uncertainty factors, the algorithm can effectively balance the weights of each contribution, avoid over-reliance on certain data that may have high uncertainty, and enable it to more accurately adjust the optimization direction and step size in each iteration, thereby improving the accuracy and robustness of the entire multi-machine SLAM system.

[0090] S4: Take the local coordinate system where the central robot is located as the global coordinate system, and transform the variables according to each relative coordinate system The local maps of each non-central robot are converted to the global coordinate system respectively to obtain a global map that integrates all local maps.

[0091] like Figure 4 As shown in the figure, the relative coordinate system is used to transform Figure 2 The result of merging the two robot trajectories and map points. Through this system demonstration, it can be observed that the algorithm has very high alignment accuracy for different robot trajectories, even in the presence of measurement errors and environmental interference. This proves the superior performance of the algorithm in multi-robot SLAM applications and ensures high accuracy and reliability of the system.

[0092] In summary, in order to estimate the relative coordinate system transformation in a multi-robot SLAM system, the present invention proposes a multi-robot positioning and mapping method based on relative coordinate system uncertainty estimation. It innovatively proposes an uncertainty estimation model, which is specially designed for a multi-robot simultaneous localization and mapping (SLAM) system. By adopting the least squares problem represented by Lie algebra and combining the error propagation formula, the model effectively integrates the pose estimation of a single robot and the covariance of the closed-loop measurement between robots, thereby providing a more accurate method to quantify uncertainty. Specifically speaking, the present invention has the following advantages:

[0093] 1. The mathematical framework of Lie algebra is used to accurately express and process the posture error, and the derivation of the Jacobian matrix is ​​combined to enable the gradient-based optimization algorithm to efficiently solve the problem. In other words, the present invention utilizes the powerful mathematical properties of Lie algebra, and the present invention provides a more detailed framework for the representation of posture transformation and its uncertainty. This not only promotes the accurate quantification and distribution of uncertainty, but also improves the efficiency and accuracy of the multi-robot SLAM system. In this way, the present invention can better understand the uncertainty in the multi-machine SLAM system, thereby improving the adaptability and robustness of the system to complex environments and dynamic changes.

[0094] 2. The error propagation formula can accurately calculate how the uncertainty caused by observation noise and data processing errors propagates in the system.

[0095] 3. By introducing the joint covariance matrix as a weighting factor into the least squares problem, the model can adjust according to the uncertainty weight of each measurement value during the optimization process. This weighting mechanism enables the model to more accurately reflect the credibility and importance of each data point, thereby effectively considering data uncertainty during the optimization process.

[0096] Of course, the present invention may have many other embodiments. Without departing from the spirit and essence of the present invention, those skilled in the art may certainly make various corresponding changes and modifications based on the present invention, but these corresponding changes and modifications should all fall within the scope of protection of the claims attached to the present invention.

Claims

1. A multi-robot positioning and mapping method based on relative coordinate system uncertainty estimation, characterized in that: The following steps are involved: S1: Each robot runs independently in its own local coordinate system and records its own motion trajectory, and sends the local map formed by its own motion trajectory to the central robot; S2: The central robot uses the posture represented by Lie algebra to construct the posture error function of the posture transformation between itself and each non-central robot; S3: Use the trust region algorithm to solve the posture error function to obtain the relative coordinate system transformation variables between the central robot α and each non-central robot β S4: Take the local coordinate system where the central robot is located as the global coordinate system, and transform the variables according to each relative coordinate system The local maps of each non-central robot are converted to the global coordinate system respectively to obtain a global map that integrates all local maps.

2. A multi-robot positioning and mapping method based on relative coordinate system uncertainty estimation as claimed in claim 1, characterized in that: In step S2, the central robot uses the posture represented by Lie algebra to construct the posture error function of the posture transformation between itself and each non-central robot as follows: in, Represents the relative coordinate system transformation variable between the global coordinate system of the central robot α and the local coordinate system of any non-central robot β; represents the least squares-based error function between the central robot α and the non-central robot β; represents the absolute position and posture of the central robot α in the i-th frame image collected by itself; represents the absolute position of the non-central robot β in the j-th frame image collected by itself; Θ represents the set formed by the combination of key frames, where the two frames of images in each group of key frames come from the image sequence collected by the central robot α and the image sequence collected by the non-central robot β, and the similarity of the two images is greater than the set threshold; express and The relative position between ij is the uncertainty covariance matrix representing the key frame combination; In(·) represents the logarithmic mapping operation, exp(·) represents the exponential mapping operation; ^ represents the antisymmetric operator, ∨ represents the inverse antisymmetric operator, T represents transpose, and min represents the minimum value.

3. A multi-robot positioning and mapping method based on relative coordinate system uncertainty estimation as claimed in claim 2, characterized in that: Covariance matrix Σ ij The calculation method is as follows: Among them, h represents Abbreviation of Represents the error function For absolute pose The partial derivative of Represents the error function For absolute pose The partial derivative of Represents the error function Relative posture The partial derivative of .

4. A multi-robot positioning and mapping method based on relative coordinate system uncertainty estimation as claimed in claim 3, characterized in that: The absolute position Relative posture Integrate to obtain a unified composite pose Among them, α' represents the absolute position of the center robot α Incorporating relative pose The composite pose after The corresponding virtual center robot; Based on absolute pose Simplified covariance matrix Σ ij as follows: Among them, Φ ij For absolute position The covariance of ij For composite pose The covariance of ij Indicates absolute position and composite pose The cross-covariance of Represents the error function For absolute pose The partial derivative of Represents the error function For composite pose The partial derivative of .

5. A multi-robot positioning and mapping method based on relative coordinate system uncertainty estimation as claimed in claim 4, characterized in that: Composite pose The covariance Ψ ij The calculation method is as follows: in, Relative posture The disturbance, and the disturbance Normal distribution For disturbance The variance of the normal distribution is consistent with E[·] represents the expected operation, For absolute position The disturbance, and the disturbance Normal distribution For disturbance The variance of the normal distribution is consistent with For absolute position In the disturbance , Ad(·) represents the adjoint operation.

6. A multi-robot positioning and mapping method based on relative coordinate system uncertainty estimation as claimed in claim 4, characterized in that: Error function For absolute pose The partial derivative of The calculation method is as follows: in, Indicates absolute position The left Jacobian matrix of Indicates absolute position The least squares-based error function The right Jacobian matrix of express The inverse of , Ad(·) represents the adjoint operation.

7. A multi-robot positioning and mapping method based on relative coordinate system uncertainty estimation as claimed in claim 4, characterized in that: Error function For composite pose The partial derivative of The calculation method is as follows: in, Represents composite pose The left Jacobian matrix of Represents composite pose The least squares-based error function The right Jacobian matrix of express The inverse of , Ad(·) represents the adjoint operation.

Images