A Transform-Based Multi-Robot Consensus Cooperative Localization Method and System

Abstract

The present invention discloses a method and system for multi-robot consistent collaborative positioning based on transformation, which relates to the field of robot positioning technology. The technical points of the present invention include: obtaining the self-motion information of multiple robots and the relative measurement information between the robots; establishing a linearized error state system based on the self-motion information and the relative measurement information, wherein the unobservable subspace of the linearized error state system remains time-invariant and independent of the linearization point; designing a transformed extended Kalman filter with observable consistency based on the linearized error state system, and utilizing it to perform multi-robot collaborative positioning. Among them, by establishing a relationship between the unobservable subspaces of the original system and the transformed linearized error state system, a transformation-based method is proposed to establish a linearized error state system, which solves the inconsistency problem caused by observability mismatch. Experiments show that the present invention is superior to existing methods in terms of accuracy and consistency.

Description

Technical Field

[0001] The present invention relates to the technical field of robot positioning, and in particular to a transformation-based multi-robot consistent collaborative positioning method and system. Background Art

[0002] For multi-robot systems, determining the position and orientation of each robot in the same reference frame is crucial for various tasks such as surveillance and reconnaissance, search and rescue, and formation control. While the Global Positioning System (GPS) provides a straightforward solution for localizing multi-robot systems, it is unreliable in GPS-limited or GPS-interfered environments, such as underwater, underground, or indoor environments. Consequently, much research has been devoted to collaborative localization (CL), in which robots jointly estimate their pose by leveraging egomotion information from proprioceptive sensors and relative inter-robot measurements from external sensors.

[0003] Numerous approaches have been proposed for collaborative localization, including the extended Kalman filter (EKF), particle filters, and optimization-based techniques. Among these approaches, the EKF has become the most popular due to its remarkable efficiency and competitive accuracy. However, studies have shown that the EKF suffers from inconsistency when applied in the context of multi-robot collaborative localization. A state estimator is said to be consistent if it satisfies the following two conditions: the estimated error should have zero mean, and its covariance should be less than or equal to the covariance calculated by the estimator. In the case of inconsistency, the covariance calculated by the estimator is often overconfident, resulting in incorrect information gain, which in turn affects the accuracy of the state estimate. Worse still, the accuracy of the state estimate is unknown, making the estimator unreliable. To address inconsistencies caused by observability mismatch, numerous methods have been proposed, which can be roughly divided into three categories: filters based on observability constraints, robot-centric filters, and filters based on matrix Lie groups. Filters based on observability constraints explicitly modify the linearized system of the estimator to ensure proper observability. Among these methods, the first estimate Jacobian method ensures correct observability by adjusting the linearization point. Although the observability constraint-based approach improves the consistency and accuracy of the estimator to some extent, modifying the Jacobian leads to a deviation from the first-order approximation, making the linearized system no longer theoretically optimal.

[0004] The latter two categories of estimators strive to employ customized state representations to ensure that the estimator's linearized system automatically maintains the correct observability properties. Robot-centric methods use robot-centric coordinates to construct the state vector, while matrix Lie group-based filters (such as the invariant EKF) utilize matrix Lie group representations to construct the state and uncertainty. These methods exhibit better performance in terms of accuracy and consistency, but the lack of a canonical representation for their design and the need for a deep understanding of their underlying principles make them complex in practice. Summary of the Invention

[0005] In view of the above problems, the present invention proposes a transformation-based multi-robot consistent collaborative positioning method and system.

[0006] According to one aspect of the present invention, a transformation-based multi-robot consistent collaborative positioning method is proposed, which includes the following steps:

[0007] Obtain the self-motion information of multiple robots and the relative measurement information between robots;

[0008] Establishing a linearized error state system based on the self-motion information and the relative measurement information, wherein the unobservable subspace of the linearized error state system remains time-invariant and independent of the linearization point;

[0009] Based on the linearized error state system, a transformation extended Kalman filter with considerable consistency is designed, and the transformation extended Kalman filter is used to perform multi-robot collaborative positioning.

[0010] Furthermore, the linearization error state system is established as follows:

[0011]

[0012] Where, represents the linearization state error; Represents the linearization measurement error; denote the Jacobian matrix-valued functions of state, noise, and measurement, respectively. represents the direction angle ψ at time k-1 i，k The rotation matrix, p i，k and ψ i，k They represent the position and direction of robot i at time k in the global reference frame, I represents the unit matrix; n k represents the stack of all robot noise column vectors; w k Represents the stack of column vectors of all robot measurement noise.

[0013] Furthermore, the Jacobian matrix value function of the measurement in the linearized error state system is for:

[0014]

[0015] In the formula, col represents the matrix column splicing; represents the Kronecker product;

[0016] I2 represents the second-order unit matrix.

[0017] Furthermore, the multi-robot collaborative positioning using the transform extended Kalman filter includes:

[0018] Initialize the state vector and error covariance and estimate the state at the next moment, including: at each time step k, calculate the state prediction vector of the current time step based on the state estimation vector obtained in the previous step and the latest control input vector The error covariance is obtained by prediction:

[0019]

[0020] Where E represents expectation;

[0021] The obtained state prediction vector and the new observation vector are fused, and the system state vector is estimated using the transformation extended Kalman filter to obtain the position and yaw angle of multiple robots in the global reference frame. Specifically,

[0022] The state update equation is:

[0023]

[0024] Where Δe k represents the Kalman correction, represents the Kalman gain to be determined, y k represents the measured value, h represents the measurement equation, express The inverse, is the estimated transformation matrix:

[0025]

[0026] The error covariance update equation is:

[0027]

[0028] Where, yes The estimated value of ; Rk represents the covariance of the measurement noise.

[0029] Furthermore, the Kalman gain to be determined is The calculation formula is:

[0030]

[0031] According to another aspect of the present invention, a transformation-based multi-robot consistent collaborative positioning system is proposed, the system comprising:

[0032] an information acquisition module configured to acquire self-motion information of the plurality of robots and relative measurement information between the robots;

[0033] a system equation building module configured to build a linearized error state system based on the self-motion information and the relative measurement information, wherein the unobservable subspace of the linearized error state system remains time-invariant and is independent of the linearization point;

[0034] The collaborative positioning module is configured to design a transformation extended Kalman filter with considerable consistency based on the linearized error state system, and use the transformation extended Kalman filter to perform multi-robot collaborative positioning.

[0035] Furthermore, the linearized error state system in the system equation establishment module is established as follows:

[0036]

[0037] Where, represents the linearization state error; Represents the linearization measurement error; denote the Jacobian matrix-valued functions of state, noise, and measurement, respectively. represents the direction angle ψ at time k-1 i，k The rotation matrix, p i，k and ψ i，k They represent the position and direction of robot i at time k in the global reference frame, I represents the unit matrix; n k represents the stack of all robot noise column vectors; w k Represents the stack of column vectors of all robot measurement noise.

[0038] Furthermore, the Jacobian matrix value function of the measurement in the linearized error state system is for:

[0039]

[0040] In the formula, col represents the matrix column splicing; represents the Kronecker product; I2 represents the second-order unit matrix.

[0041] Furthermore, the collaborative positioning module using the transformed extended Kalman filter to perform multi-robot collaborative positioning includes:

[0042] Initialize the state vector and error covariance and estimate the state at the next moment, including: at each time step k, calculate the state prediction vector of the current time step based on the state estimation vector obtained in the previous step and the latest control input vector The error covariance is obtained by prediction:

[0043]

[0044] Where E represents expectation;

[0045] The obtained state prediction vector and the new observation vector are fused, and the system state vector is estimated using the transformation extended Kalman filter to obtain the position and yaw angle of multiple robots in the global reference frame. Specifically,

[0046] The state update equation is:

[0047]

[0048] Where Δe k represents the Kalman correction, represents the Kalman gain to be determined, y k represents the measured value, h represents the measurement equation, express The inverse, is the estimated transformation matrix:

[0049]

[0050] The error covariance update equation is:

[0051]

[0052] Where, yes The estimated value of R k represents the covariance of the measurement noise.

[0053] Furthermore, the Kalman gain to be determined in the collaborative positioning module The calculation formula is:

[0054]

[0055] The beneficial technical effects of the present invention are:

[0056] This paper proposes a transformation-based multi-robot consistent collaborative localization method and system. First, the ego-motion information of multiple robots and relative measurement information between them are acquired. Then, a linearized error state system is established based on the ego-motion information and relative measurement information. A transformed extended Kalman filter (T-EKF) with observable consistency is designed based on the T-EKF, and the T-EKF is used for multi-robot collaborative localization. By establishing a relationship between the unobservable subspaces of the original system and the transformed T-EKF, a transformation-based design is proposed. Specifically, a transformation-based method is proposed to establish the T-EKF. The T-EKF maintains time-invariant and is independent of the linearization point, resolving the inconsistency problem caused by observability mismatch. A novel estimator, called the T-EKF (Transformed Extended Kalman Filter), is designed based on the T-EKF. This T-EKF utilizes the transformed T-EKF for state estimation, ensuring correct observability and thus consistency. Simulations and field experiments demonstrate that the proposed method outperforms existing state-of-the-art methods in terms of accuracy and consistency. BRIEF DESCRIPTION OF THE DRAWINGS

[0057] The present invention can be better understood by referring to the description given below in conjunction with the accompanying drawings, which together with the following detailed description are included in this specification and form a part of this specification, and are used to further illustrate the preferred embodiments of the present invention and explain the principles and advantages of the present invention.

[0058] Figure 1 The figure below is a comparison diagram of the classic extended Kalman filter and the transformed extended Kalman filter proposed in the present invention; the left figure is the classic extended Kalman filter, and the right figure is the transformed extended Kalman filter proposed in the present invention;

[0059] Figure 2 is a graph of the mean absolute localization error (position and orientation) of different estimators in Monte Carlo simulation.

[0060] Figure 3 is a plot of the average relative pose error (position and orientation) of different estimators in Monte Carlo simulation.

[0061] Figure 4 is the average normalized squared estimation error (position and orientation) of the four robots in the Monte Carlo simulation.

[0062] Figure 5These are the statistical error plots of all estimators in nine sub-datasets; (a) corresponds to location error; (b) corresponds to orientation error.

[0063] Figure 6 This is a schematic diagram of the actual experimental equipment.

[0064] Figure 7 are the trajectories of the four robots in the experiment; the solid lines and dotted lines represent the actual trajectories and estimated trajectories of the four robots, respectively. DETAILED DESCRIPTION

[0065] In order to enable those skilled in the art to better understand the present invention, exemplary embodiments or examples of the present invention will be described below with reference to the accompanying drawings. Obviously, the described embodiments or examples are only some of the embodiments or examples of the present invention, and not all of them. Based on the embodiments or examples of the present invention, all other embodiments or examples obtained by those skilled in the art without creative work should fall within the scope of protection of the present invention.

[0066] By examining the unobservable subspaces of the original and estimator systems, we discovered that the directions in which the error becomes observable depend on the system state, while state-independent unobservable directions remain unobservable. Inspired by this insight, we introduce a transformation to make the unobservable subspace of the transformed system state-independent, thereby ensuring correct observability properties. We then use the transformed system for state estimation to resolve the inconsistency caused by the observability mismatch.

[0067] The embodiment of the present invention provides a multi-robot consistent collaborative positioning method based on transformation, which includes the following steps:

[0068] S1. Obtaining the self-motion information of multiple robots and the relative measurement information between the robots;

[0069] S2. Establishing a linearized error state system based on the self-motion information and the relative measurement information, wherein the unobservable subspace of the linearized error state system remains time-invariant and is independent of the linearization point;

[0070] S3. Based on the linearized error state system, a transformation extended Kalman filter with considerable consistency is designed, and the transformation extended Kalman filter is used to perform multi-robot collaborative positioning.

[0071] The embodiments of the present invention are described in detail below.

[0072] 1. Problem Description

[0073] Firstly, a model of a multi-robot collaborative localization (CL) system is constructed. Then, the inconsistency problem caused by observability mismatch is analyzed.

[0074] A. Co-location System Modeling

[0075] Consider a group of m robots, labeled m = {1, 2, …, m}, co-localizing on a two-dimensional plane. Each robot is equipped with a proprioceptor (e.g., an odometry) to sense its egomotion and an extrinsic sensor (e.g., a lidar) to collect relative inter-robot measurements. The group of m robots jointly estimates their pose relative to a common reference frame by fusing their egomotion information with inter-robot measurements.

[0076] The dynamic description of each robot i, i∈m is:

[0077]

[0078] Where k = 1, 2, ... represents a discrete time index; and They represent the position and orientation of robot i at time k in the global reference frame; and represents the linear velocity and angular velocity relative to the body reference system of robot i; and Represents the input noise of the body sensor, which obeys the zero-mean Gaussian distribution δt is the sampling period, represents the direction angle ψ i，k The rotation matrix is written as:

[0079]

[0080] Assume that each robot i is able to measure the relative position between robot i and another robot j, where j∈m / {i}, as shown below:

[0081]

[0082] in represents the measurement noise, which is assumed to obey a zero-mean Gaussian distribution, i.e.

[0083] Let x i,k =col{p i，k , ψ i，k},u i,k =col{v i,k ,ω i，k}and For i∈m. Then, the dynamic model of each robot i is as described in (1)-(2), and its measurement model in (3) can be rewritten as:

[0084] Xi,k+1 =f i (x i,k ,u i，k , n i,k ) (4)

[0085] y ij,k =h ij (x k , W ij，k ) (5)

[0086] Let the state value x k =col{x 1,k , x 2,k ,…,x m,k} and the measured value y k =col{y 1，k ,y 2,k ,…,y m,k}, where y i,k =col{y ij，k}, i, j∈m, j≠i. col represents the column concatenation of the matrix. From this, we can see that the dynamics of all robots in (4) and all their noise measurements in (5) can be written in the following compact form:

[0087] x k+1 =f(x k ,u k , n k ) (6)

[0088] y k =h(x k , W k ) (7)

[0089] Where f and h represent the equation of motion and measurement equation, respectively. and are the column vector stacks of all robot inputs and noises, i.e.,

[0090] u k =col{u 1,k ,u 2,k ,…,u m，k},

[0091] n k =col{n 1,k , n 2,k ,…,n m，k},

[0092] And w k represents the column-wise stacking of all robot measurement noises, given as follows: k =col{w 1,k , w 2,k,…,w m,k},w i，k =col{w ij,k}.

[0093] B. Inconsistency Analysis

[0094] For the co-localization problem under consideration, the classic Extended Kalman Filter (EKF) has a consistency problem. The consistency problem is mainly caused by the asynchrony of the observability between the original nonlinear system and the linearized error state system of the estimator. Specifically, the linearized system of the EKF is linearized around the current best state estimate, which has one less unobservable direction than the original nonlinear system, such as Figure 1 Shown on the left.

[0095] The unobservable subspace of the original nonlinear system (6)-(7) is expanded along the direction of global position and orientation, namely:

[0096]

[0097] in span_col represents the subspace generated by the linear combination of the column vectors of the matrix; 12 represents the second-order unit matrix.

[0098] Looking at the unobservable subspace (8), we can see that the last column represents the global direction, which depends on the state of the system. Therefore, when the estimator system is linearized using the latest optimal state estimate, the global direction mistakenly becomes observable. In contrast, the first two columns in (8) represent global positions, which are state-independent (time-invariant) and maintain correct observability. By introducing the error state transformation, the unobservable subspace becomes time-invariant and independent of the choice of linearization point, thus automatically circumventing the observability mismatch problem. This insight motivates the introduction of a linear time-varying transformation that makes the unobservable subspace of the estimator system time-invariant (i.e., it does not depend on the choice of linearization point) to ensure correct observability, as Figure 1 Therefore, using the transformed system for state estimation solves the inconsistency caused by observability mismatch.

[0099] 2. Transformation-based methods

[0100] This section first introduces a linearization process with error state transformation to obtain a transformed linearized system. We then establish a relationship between the unobservable subspaces of the transformed linearized system and the original system. We exploit this relationship to design a transformation that ensures the transformed system has an unobservable subspace that is state-independent (independent of the linearization point) and maintains a time-invariant unobservable direction. This ensures the correct observability properties, thereby maintaining consistency.

[0101] A. Linearization of Time-Varying Transformations

[0102] First, linearize the system (6)-(7) along the nominal trajectory. represents a noise-off nominal trajectory of the dynamics (6). The current nominal state and its previous nominal state Obey the prediction equation:

[0103]

[0104] set up Then (6) and (7) are related to the nominal linearization point at each time k. and The first-order Taylor expansion of is as follows:

[0105] e k =F k-1 e k-1 +G k-1 v k (10)

[0106]

[0107] Among them, e k represents the linearization state error; Expresses the linearization measurement error:

[0108]

[0109] where F(·), G(·), and H(·) represent the Jacobian matrix-valued functions of the nonlinear systems (6)-(7), respectively, as shown below.

[0110]

[0111] Now we introduce a time-varying transformation matrix Tk to transform the error state e k Transform into the new coordinate system. The design of this matrix will be detailed in the subsequent sections. Applying this transformation matrix to the error state yields:

[0112]

[0113] Substituting (15) into (10) and (11), we obtain the following transformed linearized system:

[0114]

[0115] in:

[0116]

[0117]

[0118] B. Transformation Design

[0119] The goal of designing a transformation is to obtain a linearized system whose unobservable subspace remains time-invariant and thus independent of the linearization point. To this end, we first establish the relationship between the unobservable subspace of the transformed linearized system and the unobservable subspace of the original system. The following lemma holds.

[0120] Lemma 1: Let N k and denote the unobservable subspace of the system (10)-(11) and the unobservable subspace of the transformed linearized system (16)-(17). Then, the unobservable subspace and the reversible transformation matrix T k Apply to the unobservable subspace N k are completely equivalent, that is,

[0121]

[0122] Proof: For the system (10)-(11), the local observable matrix is defined on the local time interval [k, k+l] as follows:

[0123]

[0124] The unobservable subspace of the system is exactly the null space of the local observable matrix of the system. Therefore, it is easy to verify that its unobservable subspace N k Defined in (8).

[0125] The local observable matrix of the transformed linearized system (16)-(17) on the time interval [k, k+l] is defined as follows:

[0126]

[0127] Substituting (18) and (20) into (23), we obtain:

[0128]

[0129] According to formula (24), formula (21) can be proved.

[0130] Observe the unobservable subspace N in (8) k The basis of m reversible block matrices B i The columns are stacked, that is:

[0131]

[0132] in:

[0133]

[0134] In order to make the unobservable subspace of the transformation system independent of the state, a block diagonal transformation matrix Tk is designed, whose diagonal is the matrix B i The inverse of is as follows:

[0135]

[0136] According to Lemma 1, applying the transformation matrix (25) we can obtain the unobservable subspace of the transformed linearized system (16)-(17) As shown below:

[0137]

[0138] As can be seen, the unobservable direction becomes state-independent and thus time-invariant.

[0139] It should be noted that the design of the transformation is not unique. Based on Lemma 1, other forms of transformation can also be designed to solve the inconsistency problem caused by the mismatch of the dimensions of the unobservable subspace.

[0140] C. Transformed linearized error state system

[0141] Next, the transformed linearized error state system is derived using the transformation matrix (25). By substituting (25) into (18) and (19), the state and noise propagation Jacobian matrices of the transformed linearized system can be obtained as follows:

[0142]

[0143] Where I represents the unit matrix; Given by

[0144]

[0145] Substituting (25) into (20), the measurement Jacobian matrix of the transformed linearized system is:

[0146]

[0147] have i∈{1, 2, …, m}, where represents the Kronecker product, and

[0148]

[0149] in:

[0150]

[0151] The linearized error state system after transformation in (16)-(17), and (27)-(29) will be used for state estimation in the next section.

[0152] 3. Transform Extended Kalman Filter

[0153] In this section, a novel estimator, called the Transformed Extended Kalman Filter (T-EKF), is proposed to address the inconsistency problem caused by observability mismatch. The specific steps of the estimator are as follows.

[0154] A. Initialization

[0155] To start the filtering process, initial guesses for the state and covariance must be provided, respectively. and P 0|0 :

[0156]

[0157] About the initial covariance of the transformed error state According to the transformation equation (15), we have

[0158]

[0159] in is the estimated value given by T0

[0160]

[0161] The initialization step is performed only at the beginning of the state estimation. Subsequently, the remaining steps are implemented in a loop.

[0162] B. Prediction Process

[0163] At each time step k, assuming we have the estimate obtained in the previous step and the latest control input u k State prediction It can be obtained through the nonlinear state model (6) as follows:

[0164]

[0165] Accordingly, its covariance is predicted as follows:

[0166]

[0167] Where E represents expectation.

[0168] C. System update process after transformation

[0169] Assume the state update equation is:

[0170]

[0171] Where Δe k is the Kalman correction

[0172]

[0173] The corresponding covariance update is:

[0174]

[0175] Where, yes The estimated value of ; Rk represents the covariance of the measurement noise; is the undetermined Kalman gain, which is obtained by the following steps.

[0176] The trace of the posterior estimated covariance matrix at time k is:

[0177]

[0178] The system hopes that the mean square error is the smallest, so the mean square error is the smallest for the unknown quantity Taking the derivative and setting the derivative function equal to 0, we can determine the Kalman gain value.

[0179]

[0180] The Kalman gain can be solved from the above formula as:

[0181]

[0182] 4. Simulation Experiment

[0183] In this section, Monte Carlo simulations are performed to verify the performance of the proposed T-EKF method. In particular, the performance of T-EKF is compared with the unscented Kalman filter (UKF), extended Kalman filter (EKF), first estimate Jacobian EKF (FEJ), and invariant EKF (I-EKF).

[0184] Consider a scenario where four robots move randomly on a two-dimensional plane. The robots move with a constant linear velocity of 0.3 m / s, while their angular velocities are drawn from a uniform distribution in the range [-0.5, 0.5] rad / s. The self-motion measurements (linear and angular velocities) are perturbed by zero-mean Gaussian white noise with standard deviations of 0.15 m / s and 0.06 rad / s. Each robot can obtain a relative position measurement randomly with a detection probability of 24%. The relative position measurements are also perturbed by a zero-mean Gaussian distribution with standard deviation of 0.1 m. The initial positions of the four robots are randomly placed. The simulation step size is set to δt = 2.0 s, and 100 trials are performed in the Monte Carlo simulation.

[0185] For fair comparison, all estimators are implemented with the same parameters. In addition, the absolute pose error (APE) and relative pose error (RPE) are used to quantify the accuracy of the estimator, while the normalized estimation error square (NEES) is used to evaluate the estimation consistency. The evaluation results are summarized in Table 1. The absolute pose error and relative pose error of different estimators are shown in Figure 2 and 3 In this simulation scenario, for consistent estimators, the mean position NEES should be close to 2, while the mean orientation NEES should be close to 1. Among these estimators, the best results are highlighted in bold.

[0186] Table 1 Average APE, RPE and NEES in Monte Carlo simulation

[0187]

[0188] Obviously, among the four estimators, T-EKF performs better in terms of APE, RPE, and NEES. However, because the linearization system used by FEJ does not conform to the first-order Taylor expansion, severe divergence occurs at the end of the simulation, resulting in large linearization errors and reduced accuracy. In contrast, the linearization systems used by I-EKF and T-EKF not only maintain the correct observability characteristics, but also achieve first-order optimality. Therefore, the performance of these two estimators is more accurate and reliable. The nonlinear estimator UKF propagates the probability density function through sigma point transformation, but still faces consistency issues caused by observability mismatch. Therefore, its performance is significantly lower than that of T-EKF and I-EKF.

[0189] To further assess the consistency of these estimators, Figure 4 The average NEES of the four robots in the Monte Carlo simulation are plotted in . Figure 4 As shown in Figure 3, FEJ, I-EKF, and T-EKF are almost identical in terms of consistency and outperform EKF and UKF due to the preservation of the correct unobservable direction. It is worth noting that FEJ performs slightly worse than I-EKF and T-EKF at the end of the simulation because its linearized system violates the first-order optimality.

[0190] Table 1 also shows the average running time of each estimator in each iteration cycle. The results were performed on a laptop with a Ryzen 5 3500 CPU and 16GB of RAM. Due to the analytical Jacobian matrix, the computational cost of the T-EKF is almost comparable to that of the EKF. In contrast, the nonlinear estimator UKF requires more computational resources due to the large number of sigma points that must be maintained and propagated.

[0191] 5. Physical Experiment

[0192] In this section, the proposed T-EKF method is validated on the publicly available UTIAS multi-robot collaborative localization and mapping dataset and verified through real hardware experiments.

[0193] A. Dataset Experimental Results

[0194] The UTIAS dataset consists of nine sub-datasets collected in nine different runs using five two-wheeled differential drive robots with different motion characteristics. Each sub-dataset contains a coherent set of self-motion information (linear and angular velocity), relative range-azimuth measurements, and accurate ground truth data. The ninth sub-dataset is more challenging because some obstacles are deliberately placed in the environment to block the robot's line of sight. This limits the robot's perception range and reduces the number of relative measurements. In addition, the erroneous data association between relative measurements increases significantly, which can cause the estimator to diverge.

[0195] The T-EKF was compared with the EKF, UKF, localization using only odometry (DR), FEJ, and I-EKF. To evaluate the performance of these estimators in the case where absolute measurements are rejected, all landmark measurements were excluded, and only relative distance and orientation measurements between robots were relied upon to localize the entire robot team. Furthermore, to handle multiple simultaneous measurements, a sequential update technique was applied. That is, when multiple simultaneous relative measurements were received at a certain moment, the estimator would process them sequentially, using the previously updated estimate as the prior estimate for the next measurement.

[0196] In the experiments, all estimators were tested on the entire set of nine sub-datasets at a frequency of 20 Hz. All available relative measurements between the robots were used to estimate the updates. The average position and orientation RMSE of these estimators on the nine sub-datasets are summarized in Table 2. Figure 5 Statistical error plots for these estimators are shown.

[0197] like Figure 5As shown, T-EKF achieves the best mean performance overall among these estimators. Localization using only DR performs the worst in almost all sub-datasets because relative measurements are not exploited. FEJ and I-EKF suffer severe degradation from outliers caused by miscorrelation in sub-dataset 9. In contrast, T-EKF shows better robustness to measurement perturbations. Due to the inconsistency problem caused by observation mismatch, the performance of EKF and UKF is inferior to the methods that ensure appropriate observation properties. It is worth noting that although T-EKF resolves the inconsistency problem and achieves consistent estimation results, it does not show the highest accuracy on all sub-datasets, such as sub-datasets 3 and 4. This is because the optimality of the estimator is statistically guaranteed. Due to the inherent randomness, the accuracy cannot be guaranteed to be optimal in every test.

[0198] Table 2 Average RMSE of the UTIAS dataset experiments

[0199]

[0200] Root mean square error of the bearing estimate (rad)

[0201]

[0202]

[0203] B. Actual Experiment Results

[0204] Next, the proposed method is further validated using an experimental group consisting of four two-wheel differential drive robots, e.g. Figure 6 As shown. Specifically, TARKBot-R20 is used as the robot platform. Each robot is equipped with a RaspberryPi 5 as its embedded computer. All robots are equipped with wheel odometers to perceive self-motion information, including linear velocity and angular velocity, and are equipped with Nooploop UWB modules to obtain relative distances. In addition, the robots are equipped with two forward-looking wide-angle cameras and uniquely identifiable ArUco markers arranged in cubes. The visual detection of ArUco markers between robots provides relative orientation measurements. In order to evaluate the positioning performance, the Nokov motion capture system is used to track the position and orientation of these robots as real data.

[0205] The robot is programmed to follow a predefined trajectory, e.g. Figure 7Specifically, two robots were assigned to track a figure-eight pattern, while the other two robots moved along circular trajectories. The entire experiment lasted approximately 240 seconds. By using onboard ultra-wideband (UWB) tags, the robots were able to continuously detect relative distance measurements between each other at a frequency of 50 Hz. Due to the limited field of view (FoV) of the camera, the robots only occasionally obtained relative orientation views. These estimators were evaluated in two different setups: one using all relative measurements and the other using only relative orientation measurements from the camera.

[0206] The average root mean square error (RMSE) of these estimators across the 10 repeated experiments is summarized in Table 3. As can be seen in Table 3, T-EKF outperforms the other estimators. In addition, compared to the UTIAS dataset, the advantage of T-EKF over existing methods is not particularly significant. This is mainly because T-EKF shows significant advantages over other methods when faced with intermittent measurements and low-quality odometry. Under relatively ideal measurement conditions, the performance of T-EKF is similar to that of existing consistency methods.

[0207] Table 3 Average RMSE of 10 repeated experiments

[0208]

[0209] This paper proposes a transformation-based approach to address inconsistencies caused by observability mismatch in collaborative localization (CL). This approach involves implementing a time-varying transformation such that the unobservable subspace of the transformed system is state-independent and thus unaffected by linearization points. Based on this approach, a novel estimator, the Transformed Extended Kalman Filter (T-EKF), is designed. The T-EKF utilizes the transformed system for state estimation, ensuring correct observability and consistency. Simulations and experiments demonstrate the accuracy and consistency of the proposed approach.

[0210] Another embodiment of the present invention provides a transformation-based multi-robot consistent collaborative positioning system, the system comprising:

[0211] an information acquisition module configured to acquire self-motion information of the plurality of robots and relative measurement information between the robots;

[0212] a system equation building module configured to build a linearized error state system based on the self-motion information and the relative measurement information, wherein the unobservable subspace of the linearized error state system remains time-invariant and is independent of the linearization point;

[0213] The collaborative positioning module is configured to design a transformation extended Kalman filter with considerable consistency based on the linearized error state system, and use the transformation extended Kalman filter to perform multi-robot collaborative positioning.

[0214] In this embodiment, preferably, the linearized error state system in the system equation establishment module is established as follows:

[0215]

[0216] Where, represents the linearization state error; Represents the linearization measurement error; denote the Jacobian matrix-valued functions of state, noise, and measurement, respectively. represents the direction angle ψ at time k-1 i，k The rotation matrix, p i，k and ψ i，k They represent the position and direction of robot i at time k in the global reference frame, I represents the unit matrix; n k represents the stack of all robot noise column vectors; w k Represents the stack of column vectors of all robot measurement noise.

[0217] In this embodiment, preferably, the Jacobian matrix value function of the measurement in the linearized error state system is for:

[0218]

[0219] In the formula, col represents the matrix column splicing; represents the Kronecker product;

[0220] I2 represents the second-order unit matrix.

[0221] In this embodiment, preferably, the collaborative positioning module using the transformed extended Kalman filter to perform multi-robot collaborative positioning includes:

[0222] Initialize the state vector and error covariance and estimate the state at the next moment, including: at each time step k, calculate the state prediction vector of the current time step based on the state estimation vector obtained in the previous step and the latest control input vector The error covariance is obtained by prediction:

[0223]

[0224] Where E represents expectation;

[0225] The obtained state prediction vector and the new observation vector are fused, and the system state vector is estimated using the transformation extended Kalman filter to obtain the position and yaw angle of multiple robots in the global reference frame. Specifically,

[0226] The state update equation is:

[0227]

[0228] Where Δe k represents the Kalman correction, represents the Kalman gain to be determined, y k represents the measured value, h represents the measurement equation, express The inverse, is the estimated transformation matrix:

[0229]

[0230] The error covariance update equation is:

[0231]

[0232] Where, yes The estimated value of R k represents the covariance of the measurement noise.

[0233] In this embodiment, preferably, the Kalman gain to be determined in the collaborative positioning module is The calculation formula is:

[0234]

[0235] The functions of the transformation-based multi-robot consistent collaborative positioning system described in an embodiment of the present invention can be explained by the aforementioned transformation-based multi-robot consistent collaborative positioning method. Therefore, for the parts not described in detail in the system embodiment, please refer to the above method embodiment and will not be repeated here.

[0236] Although the present invention has been described with respect to a limited number of embodiments, those skilled in the art, having benefit of the foregoing description, will appreciate that other embodiments are contemplated within the scope of the invention thus described. This disclosure is intended to be illustrative rather than restrictive of the scope of the invention, which is defined by the appended claims.

Claims

1. A multi-robot consistent collaborative positioning method based on transformation, characterized in that: The following steps are involved: Obtain the self-motion information of multiple robots and the relative measurement information between robots; Establishing a linearized error state system based on the self-motion information and the relative measurement information, wherein the unobservable subspace of the linearized error state system remains time-invariant and independent of the linearization point; Based on the linearized error state system, a transformation extended Kalman filter with considerable consistency is designed, and the transformation extended Kalman filter is used to perform multi-robot collaborative positioning.

2. The transformation-based multi-robot consistent collaborative positioning method according to claim 1, characterized in that: The linearization error state system is established as follows: Where, represents the linearization state error; Represents the linearization measurement error; denote the Jacobian matrix-valued functions of state, noise, and measurement, respectively. represents the direction angle ψ at time k-1 i,k The rotation matrix, p i,k and ψ i,k They represent the position and direction of robot i at time k in the global reference frame, I represents the unit matrix; n k represents the stack of all robot noise column vectors; w k Represents the stack of column vectors of all robot measurement noise.

3. The transformation-based multi-robot consistent collaborative positioning method according to claim 2, characterized in that: The Jacobian matrix value function of the measurement in the linearized error state system for: In the formula, col represents the matrix column splicing; represents the Kronecker product; I2 represents the second-order unit matrix.

4. The transformation-based multi-robot consistent collaborative positioning method according to claim 3, characterized in that: The multi-robot collaborative positioning using the transform extended Kalman filter includes: Initialize the state vector and error covariance and estimate the state at the next moment, including: at each time step k, calculate the state prediction vector of the current time step based on the state estimation vector obtained in the previous step and the latest control input vector The error covariance is obtained by prediction: Where E represents expectation; The obtained state prediction vector and the new observation vector are fused, and the system state vector is estimated using the transformation extended Kalman filter to obtain the position and yaw angle of multiple robots in the global reference frame. Specifically, The state update equation is: Where Δe k represents the Kalman correction, represents the Kalman gain to be determined, y k represents the measured value, h represents the measurement equation, express The inverse, is the estimated transformation matrix: The error covariance update equation is: Where, yes The estimated value of R k represents the covariance of the measurement noise.

5. The method for multi-robot consistent collaborative positioning based on transformation according to claim 4, characterized in that: The Kalman gain to be determined The calculation formula is:

6. A multi-robot consistent collaborative positioning system based on transformation, characterized in that: include: an information acquisition module configured to acquire self-motion information of the plurality of robots and relative measurement information between the robots; a system equation building module configured to build a linearized error state system based on the self-motion information and the relative measurement information, wherein the unobservable subspace of the linearized error state system remains time-invariant and is independent of the linearization point; The collaborative positioning module is configured to design a transformation extended Kalman filter with considerable consistency based on the linearized error state system, and use the transformation extended Kalman filter to perform multi-robot collaborative positioning.

7. The transformation-based multi-robot consistent collaborative positioning system according to claim 6, characterized in that: The linearized error state system in the system equation building module is established as follows: Where, represents the linearization state error; Represents the linearization measurement error; denote the Jacobian matrix-valued functions of state, noise, and measurement, respectively. represents the direction angle ψ at time k-1 i,k The rotation matrix, p i,k and ψ i,k They represent the position and direction of robot i at time k in the global reference frame, I represents the unit matrix; n k represents the stack of all robot noise column vectors; w k Represents the stack of column vectors of all robot measurement noise.

8. The transformation-based multi-robot consistent collaborative positioning system according to claim 7, characterized in that: The Jacobian matrix value function of the measurement in the linearized error state system for: In the formula, col represents the matrix column splicing; represents the Kronecker product; I2 represents the second-order unit matrix.

9. The transformation-based multi-robot consistent collaborative positioning system according to claim 8, characterized in that: The collaborative positioning module uses the transformed extended Kalman filter to perform multi-robot collaborative positioning, which includes: Initialize the state vector and error covariance and estimate the state at the next moment, including: at each time step k, calculate the state prediction vector of the current time step based on the state estimation vector obtained in the previous step and the latest control input vector The error covariance is obtained by prediction: Where E represents expectation; The obtained state prediction vector and the new observation vector are fused, and the system state vector is estimated using the transformation extended Kalman filter to obtain the position and yaw angle of multiple robots in the global reference frame. Specifically, The state update equation is: Where Δe k represents the Kalman correction, represents the Kalman gain to be determined, y k represents the measured value, h represents the measurement equation, express The inverse, is the estimated transformation matrix: The error covariance update equation is: Where, yes The estimated value of R k represents the covariance of the measurement noise.

10. The transformation-based multi-robot consistent collaborative positioning system according to claim 9, characterized in that: The Kalman gain to be determined in the collaborative positioning module The calculation formula is:

Images