Multi-robot consistency cooperative positioning method and system based on transformation

Abstract

The invention discloses a multi-robot consistency cooperative positioning method and system based on transformation, and relates to the technical field of robot positioning. According to the technical key points, the method comprises the following steps: acquiring motion information of a plurality of robots and relative measurement information among the robots; a linearization error state system is established based on self motion information and relative measurement information, and an unobservable subspace of the linearization error state system keeps time unchanged and is independent of a linearization point; a transformation extended Kalman filter with considerable consistency is designed based on a linearization error state system, and multi-robot cooperative positioning is carried out by using the transformation extended Kalman filter. According to the method, the relation between the original system and the unobservable subspace of the transformed linearization error state system is established, the method based on transformation is provided to establish the linearization error state system, and the problem of inconsistency caused by mismatching of observability is solved. Experiments show that the method is superior to an existing method in the aspects 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. Although the Global Positioning System (GPS) provides a simple and straightforward solution for the localization of multi-robot systems, it is unreliable in GPS-limited or GPS-jammed environments such as underwater, underground, or indoor environments. Therefore, much research has been devoted to collaborative localization (CL), in which the robots jointly estimate their poses by leveraging self-motion information from proprioceptive sensors and relative inter-robot measurements from external sensors.

[0003] Many methods have been proposed for collaborative localization, including extended Kalman filter (EKF), particle filter, and optimization-based techniques. Among these methods, EKF has become the most popular method due to its remarkable efficiency and competitive accuracy. However, studies have shown that EKF has inconsistencies when applied in the context of multi-robot collaborative localization. A state estimator is called 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 affects the accuracy of the state estimation. Even worse, the accuracy of the state estimation is unknown, making the estimator unreliable. In order to solve the inconsistency caused by observability mismatch, many methods have been proposed by scholars at home and abroad, 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 estimated Jacobian method guarantees 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 optimal in theory.

[0004] The latter two classes of estimators strive to employ tailored state representations to ensure that the linearized system of the estimator 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 exploit matrix Lie group representations to construct states and uncertainties. These methods show 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 multi-robot consistent collaborative positioning method based on transformation is proposed, and the method comprises 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 is independent of the linearization point;

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

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

[0011]

[0012] In the formula, 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 stacking of all robot noise column vectors; w k Represents the stack of column vectors of all robot measurement noise.

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

[0014]

[0015] In the formula, col means that the matrix is ​​concatenated by columns; represents the Kronecker product;

[0016] I2 represents the second-order identity 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] In the formula, 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 system, including:

[0022] The state update equation is:

[0023]

[0024] In the formula, Δe k represents the Kalman correction, represents the Kalman gain to be calculated, 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] In the formula, 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 building module is established as follows:

[0036]

[0037] In the formula, 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 stacking of all robot noise column vectors; w k Represents the stack of column vectors of all robot measurement noise.

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

[0039]

[0040] In the formula, col means that the matrix is ​​concatenated by columns; represents the Kronecker product; I2 represents the second-order identity matrix.

[0041] Furthermore, the collaborative positioning module using the transform 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] In the formula, 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 system, including:

[0046] The state update equation is:

[0047]

[0048] In the formula, Δe k represents the Kalman correction, represents the Kalman gain to be calculated, 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] In the formula, 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] The present invention proposes a method and system for multi-robot consistent collaborative positioning based on transformation. First, the self-motion information of multiple robots and the relative measurement information between the robots are obtained; then, a linearized error state system is established based on the self-motion information and the relative measurement information; then, based on the linearized error state system, a transformed extended Kalman filter with observable consistency is designed, and the transformed extended Kalman filter is used to perform multi-robot collaborative positioning. Among them, by establishing the relationship between the unobservable subspace of the original system and the transformed linearized error state system, a transformation design based on the unobservable space is proposed, that is, a transformation-based method is proposed to establish the linearized error state system, the unobservable subspace of the linearized error state system remains time-invariant and independent of the linearization point, and the inconsistency problem caused by observability mismatch is solved; based on the transformed linearized error state system, a novel estimator is designed, called the transformed extended Kalman filter (Transformed EKF, referred to as T-EKF), which uses the transformed system for state estimation to ensure the correct observability characteristics, thereby ensuring consistency. Simulation and physical experiments show that the method proposed by the present invention is superior to the most advanced existing 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 1 is a comparison diagram of the classic extended Kalman filter and the transformed extended Kalman filter proposed in the present invention; wherein the left diagram is the classic extended Kalman filter, and the right diagram 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 diagrams of all estimators in nine sub-datasets; (a) corresponds to location error; (b) corresponds to orientation error.

[0063] Figure 6 It is a schematic diagram of the physical 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 scheme of the present invention, exemplary implementations or embodiments of the present invention will be described below in conjunction with the accompanying drawings. Obviously, the described implementations or embodiments are only implementations or embodiments of a part of the present invention, not all of them. Based on the implementations or embodiments of the present invention, all other implementations or embodiments obtained by ordinary technicians in the field without creative work should fall within the scope of protection of the present invention.

[0066] By examining the unobservable subspaces of the original system and the estimator system, it is found that the direction in which the error becomes observable depends on the system state, while the state-independent unobservable direction remains unobservable. Inspired by this insight, the present invention introduces a transformation to make the unobservable subspace of the transformed system state-independent, thereby ensuring the correct observability properties. Then, the inconsistency caused by the observability mismatch is resolved by using the transformed system for state estimation.

[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 unchanged in time 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 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., odometer) to sense its self-motion information, and an external sensor (e.g., LiDAR) to collect relative inter-robot measurement information. The group of m robots jointly estimates their poses relative to a common reference frame by fusing the self-motion information and the relative measurements between the robots.

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

[0077]

[0078] Wherein, k = 1, 2, ... represents a discrete time index; and They represent the position and direction 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 main 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, that is,

[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. It can be seen 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 classical 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 erroneously becomes observable. In contrast, the first two columns in (8) represent global positions, 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, thereby 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] In this section, we first introduce a linearization process with error state transformation to obtain a transformed linearized system; then we establish the relationship between the unobservable subspace of the transformed linearized system and the original system. We use this relationship to design a transformation to ensure that the transformed system has an unobservable subspace that is independent of the state (independent of the linearization point) and maintains the time-invariant unobservable direction. This ensures the correct observability properties and thus maintains consistency.

[0101] A. Linearization of Time-Varying Transformations

[0102] First, the linearized system (6)-(7) is derived along the nominal trajectory. represents a noise-off nominal trajectory of the dynamics (6). The current nominal state and its previous nominal state Subject to 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; Express 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 to 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 purpose 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. There is the following lemma.

[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) respectively. Then, the unobservable subspace and the reversible transformation matrix T k Applied 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 that in (8) the unobservable subspace N k The basis of this is the m reversible block matrix 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] Among them, 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 transformed linearized error state system 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 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 that 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] In the formula, E represents expectation.

[0168] C. System update process after transformation

[0169] Assume the state update equation is:

[0170]

[0171] Among them, Δe k Kalman correction

[0172]

[0173] The corresponding covariance update is:

[0174]

[0175] In the formula, 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 Taking the derivative and setting the derivative function equal to 0, we can determine the Kalman gain The value of .

[0179]

[0180] From the above formula, the Kalman gain can be solved as:

[0181]

[0182] 4. Simulation Experiment

[0183] Monte Carlo simulations are performed in this section 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 of [-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. Relative position measurements can be obtained randomly for each robot with a detection probability of 24%. The relative position measurements are also perturbed by a zero-mean Gaussian distribution with a 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 APE, RPE and NEES. However, since the linearization system used by FEJ does not conform to the first-order Taylor expansion, serious 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 problems 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 estimates, Figure 4 The average NEES of the four robots in the Monte Carlo simulation is plotted in Figure 4 As shown in Figure 2, FEJ, I-EKF, and T-EKF are almost consistent and outperform EKF and UKF in terms of consistency due to the maintenance 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 simulation test is carried out on the The results are performed on a laptop with a Ryzen 5 3500 CPU and 16GB RAM. The computational cost of T-EKF is almost comparable to that of EKF due to the analytical Jacobian matrix. 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 wrong data association between relative measurements increases significantly, which may cause the estimator to diverge.

[0195] T-EKF is compared with 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 are excluded and only relative distance and orientation measurements between robots are relied upon to localize the entire robot team. In addition, to handle multiple simultaneous measurements, a sequential update technique is applied, i.e., when multiple simultaneous relative measurements are received at a certain moment, the estimator will process them sequentially, using the previously updated estimate as the prior estimate for the next measurement.

[0196] In the experiments, all estimators were tested at 20 Hz on the entirety of the nine sub-datasets. All available relative measurements between 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 mis-association 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 both EKF and UKF is inferior to the methods that ensure appropriate observability properties. It is worth noting that although T-EKF solves 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 UTIAS dataset experiments

[0199]

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

[0201]

[0202]

[0203] B. Physical Experiment Results

[0204] Next, the proposed method is further validated using an experimental set 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 and angular velocities, and Nooploop UWB modules to obtain relative distances. In addition, these robots are configured with two forward-looking wide-angle cameras and uniquely identifiable ArUco markers arranged in cubes. 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 7As shown. Specifically, two robots were assigned to track a figure-eight pattern, while the other two robots moved along circular trajectories. The entire experiment lasted about 240s. By using onboard ultra-wideband (UWB) tags, the robots were able to continuously detect relative distance measurements between each other at a frequency of 50Hz. 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 settings: 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 in these 10 repeated experiments is summarized in Table 3. As observed in Table 3, T-EKF outperforms other estimators. In addition, compared with the UTIAS dataset, the advantage of T-EKF over existing methods is not particularly obvious. This is mainly because T-EKF shows significant advantages over other methods when facing 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] A transformation-based approach is proposed in this paper to address the inconsistency caused by observability mismatch in co-localization (CL), which involves implementing a time-varying transformation so that the unobservable subspace of the transformed system is state-independent and thus unaffected by the linearization points. Based on this approach, a novel estimator is designed, called the Transformed Extended Kalman Filter (T-EKF). The T-EKF uses the transformed system for state estimation, thereby 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 multi-robot consistent collaborative positioning system based on transformation, 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] In the formula, 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 stacking 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 means that the matrix is ​​concatenated by columns; represents the Kronecker product;

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

[0221] In this embodiment, preferably, the collaborative positioning module using the transform 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] In the formula, 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 system, including:

[0226] The state update equation is:

[0227]

[0228] In the formula, Δe k represents the Kalman correction, represents the Kalman gain to be calculated, 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] In the formula, 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 The calculation formula is:

[0234]

[0235] The function of a 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 according to a limited number of embodiments, it will be apparent to those skilled in the art, with the benefit of the above description, that other embodiments are contemplated within the scope of the invention thus described. The disclosure of the present invention is intended to be illustrative rather than restrictive of the scope of the invention, which is defined by the appended claims.

Claims

1. A transformation-based multi-robot consistent collaborative positioning method, 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 is independent of the linearization point; Based on the linearized error state system, a transform extended Kalman filter with considerable consistency is designed, and the transform extended Kalman filter is used to perform multi-robot collaborative positioning.

2. The method for coordinating multi-robots based on transformation according to claim 1, characterized in that: The linearization error state system is established as follows: In the formula, 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 stacking 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 means that the matrix is ​​concatenated by columns; represents the Kronecker product; I2 represents the second-order identity matrix.

4. The method for coordinating multi-robots based on transformation according to claim 3 is 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: In the formula, 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 system, including: The state update equation is: In the formula, Δe k represents the Kalman correction, represents the Kalman gain to be calculated, 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: In the formula, yes The estimated value of R k represents the covariance of the measurement noise.

5. The method for coordinating multi-robots 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: In the formula, 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 stacking 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 means that the matrix is ​​concatenated by columns; represents the Kronecker product; I2 represents the second-order identity matrix.

9. The transformation-based multi-robot consistent collaborative positioning system according to claim 8, characterized in that: The collaborative positioning module uses the transform 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: In the formula, 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 system, including: The state update equation is: In the formula, Δe k represents the Kalman correction, represents the Kalman gain to be calculated, 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: In the formula, 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