Multi-robot cooperative positioning method and system based on UWB relative distance measurement

Abstract

The invention discloses a multi-robot cooperative positioning method and system based on UWB relative distance measurement, relates to the technical field of multi-robot cooperative positioning, and aims to solve the problem of cluster cooperative autonomous navigation of a multi-robot system in an environment that external navigation equipment is unavailable or weak communication exists. According to the technical key points, a state model of a multi-robot cooperative positioning problem and a UWB relative measurement model are established, and a linearization error state system is deduced; the reason of inconsistency of standard extended Kalman filtering in cooperative positioning is analyzed, and the essence of the standard extended Kalman filtering is revealed to be from considerable asynchronization between an original nonlinear system and a linearization error state system of an estimator. The invention designs a method based on linear time-varying transformation, constructs a KD-EKF algorithm with a time-invariant and unobservable subspace, and performs Monte Carlo contrast simulation to verify that the KD-EKF has higher positioning precision and better consistency compared with a standard EKF method and an FEJ-EKF method. A multi-robot system experimental platform based on the Tucker ground robot is developed, and experimental verification is carried out on the designed KD-CL algorithm. Experimental results verify the superiority and effectiveness of the proposed algorithm.

Description

Technical Field

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

[0002] Multi-robot system in collaborative surveillance and reconnaissance [1] , search and rescue [2] and formation control [3] When performing tasks such as [4] Traditional solutions rely mainly on external positioning devices such as the Global Navigation Satellite System (GNSS) or motion capture systems. [5,6] However, for global navigation information denial scenarios such as indoors, underground, and in complex electromagnetic environments, positioning methods that rely on global navigation information have significant limitations. [7,8] , it is necessary to achieve the joint positioning of the multi-robot system relative to a common reference frame through cooperative localization (CL) of multiple robots, which combines the self-motion information provided by internal sensing sensors (such as wheel encoders, inertial measurement units (IMUs), etc.) with the relative measurement information between robots obtained by external sensing sensors (such as ultra-wideband (UWB), cameras, etc.).

[0003] In the field of multi-source sensor information fusion, researchers have developed a variety of information fusion methods for the state estimation problem of nonlinear systems. Among them, the Extended Kalman Filter (EKF), the Iterated Extended Kalman Filter (IEKF) and the Maximum a Posteriori Estimation (MAP) are three representative nonlinear filtering methods. [9-11] . The extended Kalman filter uses a linearization method to locally linearize the nonlinear system, and then applies the Kalman filter method to process it. Compared with the extended Kalman filter, the iterative extended Kalman filter gradually approaches the true posterior distribution by repeating the linearization process multiple times at the current estimation point, thereby significantly improving the estimation performance in strong nonlinear scenarios. The maximum a posteriori estimation starts from the Bayesian probability framework and realizes state estimation by maximizing the posterior probability density function. It has better robustness in non-Gaussian noise environments.

[0004] For nonlinear estimators, consistency is an important indicator for evaluating estimation performance. If the state estimator satisfies the estimation error with zero mean and the covariance should be less than or equal to the covariance calculated by the estimator

[12] , then the estimator is said to be consistent. Although the above nonlinear estimator has achieved quite good results in the field of collaborative positioning, it faces the problem of inconsistent estimation in the global navigation information denial environment.

[13] Huang et al. proved through analysis that the dimension of the unobservable subspace of the linearized system used by the extended Kalman filter is smaller than the dimension of the nonlinear system, which causes the estimator to incorrectly obtain information along the unobservable direction, resulting in inconsistency of the estimator.

[14] To this end, the first estimate Jacobian (FEJ) and observability-constrained (OC) methods are proposed. Among them, FEJ-EKF linearizes by selecting the first linearization point to ensure the correct observability.

[15] , OC-EKF ensures correct observability through optimization methods

[16] .

[0005] Hao et al. proposed the KD-EKF estimation method, which adopted the system observability decomposition strategy and effectively eliminated the inconsistency factors in the Kalman observable standard form.

[17] Tian et al. use a linear time-varying coordinate transformation in a transformed coordinate system to make the transfer Jacobian matrix independent of the state, thereby ensuring the consistency of the estimator

[18] .

[0006] In recent years, ultra-wideband (UWB) technology has shown unique advantages in multi-robot collaborative positioning with its centimeter-level ranging accuracy, omnidirectional perception capability and environmental robustness. [19,20] Compared with vision and lidar, UWB does not rely on target appearance features or lighting conditions, and can directly achieve unambiguous identity recognition through a unique ID. Its hundreds of meters of range and anti-multipath interference characteristics ensure stable ranging in complex indoor and outdoor scenes, providing an efficient and reliable perception solution for multi-machine collaborative positioning. Andrew Fishberg et al.

[21] A relative bearing estimation system based on UWB was designed. By modeling and compensating for the UWB ranging deviation and antenna shielding effect, the average position error was significantly reduced. Thien Hoang Nguyen et al.

[22] A tightly coupled fusion system of vision, UWB and inertial navigation was developed, which effectively suppressed the cumulative drift of visual inertial navigation.

[0007] Patent document CN113252036B discloses a variational adaptive collaborative navigation method based on belief propagation. This method addresses the problem of unknown time-varying noise in the communication links of collaborative navigation systems in complex environments, which can cause filter accuracy degradation due to external interference. The method constructs a collaborative navigation filter with a sliding window that can handle time-varying noise. Key technical points include: establishing a collaborative navigation model by networking multiple navigation platforms with a fixed base station. Each platform obtains position information from adjacent platforms and base stations through UWB relative ranging, completing communication tasks between collaborative navigation platforms. A belief propagation algorithm is used to implement forward filtering to obtain status information for all navigation platforms. During navigation platform interaction, the forward filtering values ​​are used to construct a sliding window and auxiliary matrix. The covariance matrix of the time-varying process noise and measurement noise present in the filtering process is smoothly updated. The updated noise covariance is fed back to the next information exchange process to output navigation parameters. This prior art technology is suitable for collaborative navigation systems in complex environments with unknown noise caused by interference factors.

[0008] Although existing related research has made great progress, how to use UWB to enhance the collaborative positioning performance of multiple robots has not been fully studied. Summary of the Invention

[0009] The technical problems to be solved by the present invention are:

[0010] In order to solve the problem of how to use UWB to enhance the collaborative positioning performance of multi-robot systems when they are clustered and autonomously navigated in an environment where external navigation equipment is unavailable or communication is weak, a multi-robot collaborative positioning method and system based on UWB relative ranging is provided.

[0011] The technical solution adopted by the present invention to solve the above technical problems is:

[0012] A multi-robot collaborative localization method based on UWB relative ranging is proposed. The method defines a group of n moving robots on a two-dimensional plane. Each robot is equipped with a proprioception sensor (such as an encoder) to obtain the robot's self-motion information (linear velocity and angular velocity) and carries two UWB tags with fixed and known positions. The position and posture of all robots in the same coordinate system are estimated by fusing odometry information with UWB relative measurement information. The implementation process of the method is as follows:

[0013] Construct a multi-robot collaborative localization problem model:

[0014] Kinematic model (system state model): The equation of motion for robot i is given by the state vector containing n robot poses expressed in the global reference frame.

[0015] System measurement model: The system's relative measurement vector and measurement noise vector are defined as superposition vectors. The system's measurement equation is obtained by superimposing the system's relative measurement vector and measurement noise.

[0016] Linearized error state system model:

[0017] Combining the motion model with the system measurement model (measurement equation) yields a collaborative positioning model. Each robot forwards its current state estimate based on the latest self-motion information to obtain a predicted value. The linearized error state prediction equations of n robots are stacked to obtain the linearized error state prediction equation of the entire system. Linearizing the system measurement model yields the system's linearized measurement error equation. Combining the system's linearized error state prediction equation with the system's linearized measurement error equation yields a linearized error state system model.

[0018] Estimator consistency analysis:

[0019] The consistency of the standard extended Kalman filter is analyzed. The standard extended Kalman filter uses a linearized error state system model for state estimation. Combining the motion model and the system measurement model (measurement equation) can obtain an ideal co-localization model. By comparing the unobservable subspaces of the standard extended Kalman filter and the ideal co-localization system model, it is found that due to linearization, the standard extended Kalman filter has fewer unobservable directions than the ideal co-localization system. This will cause the estimator to obtain false information along unobservable directions, and ultimately lead to inconsistency.

[0020] The standard extended Kalman filter is improved by making the unobservable subspace time-invariant (i.e., independent of the choice of linearization point) to avoid the inconsistency caused by observability mismatch caused by linearization.

[0021] After making the above improvements to the standard extended Kalman filter, a transformation-based consistency collaborative localization algorithm is obtained. The transformation-based consistency collaborative localization algorithm is used for multi-robot collaborative localization:

[0022] Linear time-varying transformation design: Introducing a linear time-varying transformation to make the unobservable subspace of the estimator system time-invariant, that is, independent of the choice of linearization points, to ensure correct observability;

[0023] Transformation-based co-localization estimator: The transformation-based co-localization estimator performs co-localization estimation, specifically by providing initial values ​​of state and covariance, and then performing prediction, update process, and inverse transformation.

[0024] Furthermore, the specific implementation process of the method is:

[0025] Construct a multi-robot collaborative localization problem model:

[0026] 1) System state model

[0027] The state vector contains n robot poses expressed in the global reference frame. At time k, the state vector is given by:

[0028]

[0029] in, represents the position of robot i,i∈{1,2,…,n} at the kth moment, d = 2 or 3 and are the position and orientation of the robot, respectively.

[0030] At the kth moment, the motion equation of robot i is described as

[0031]

[0032] in, and They represent the linear velocity and angular velocity of robot i, which are information provided by the odometry; represents Gaussian white noise, satisfying R(ψ i,k-1 ) is the corresponding rotation matrix, that is:

[0033]

[0034] The state vector, motion input vector and process noise vector are defined as superposition vectors, and the motion equation of the system is:

[0035] x k =f(x k-1 ,u k ,ε k ) (3)

[0036] 2) System measurement model

[0037] In time k, the relative measurement information between 2n UWBs is used as relative measurement, and the relative measurement equation can be written as:

[0038]

[0039] in, UWB i ,i∈{1,2,...,n} and UWB j , relative measurement of j∈{1,2,...,n} / i; is the measurement noise assumed to be Gaussian white noise and

[0040] Assume that the positions of the two UWBs equipped on each robot are L1 and L2 in the body coordinate system. The relative distance between L1 and L2 is known, then

[0041] h (ij) (x i,k ,x j,k )=||(p i,k +L i )-(p j,k +L j )||2 (5)

[0042] Among them L i ,L j ∈{L1,L2};

[0043] h (ij) The relative measurement vector of the system can be represented by stacking them in columns. The relative measurement vector and measurement noise vector of the system are defined as superposition vectors. The measurement equation of the system is obtained by stacking the relative measurement vector and measurement noise of the system:

[0044] z k =h(x k )+η k (6)

[0045] 3) Linearized error state system

[0046] Combining formula (3) with formula (6) can get the collaborative positioning model:

[0047]

[0048] Among them, x k is the state vector, z k is the measurement vector, f(·) and h() are the state function and measurement function of the system respectively, ε k is a Gaussian white noise with zero mean and covariance Q, η k is a Gaussian white noise with zero mean and covariance R;

[0049] The standard extended Kalman filter collaborative localization (EKF-CL) transmission and update process is given: During the EKF transmission, each robot forwards the current state estimate based on the latest self-motion information Get the predicted value Put formula (3) in Linearization at can obtain the error state transfer equation:

[0050]

[0051] in F i,k-1 and Gi,k-1 is the state transfer Jacobian matrix and noise transfer matrix of robot i, specifically:

[0052]

[0053] in The linearized error state prediction equations of n robots are stacked to obtain the linearized error state prediction equation of the entire system

[0054]

[0055] Put formula (6) in Linearization at can obtain the linearized measurement error equation:

[0056]

[0057] in and They are h ij Jacobian with respect to the i-th and j-th robot poses:

[0058]

[0059]

[0060] By stacking all the linearized measurement error equations, we can get the linearized measurement error equation of the system:

[0061]

[0062] Combining Equation (9) and Equation (13), the linearized error state system can be obtained as follows:

[0063]

[0064] The standard extended Kalman filter uses a linearized error state system model (14) for state estimation;

[0065] 4) Estimator consistency analysis

[0066] A state estimator is said to be consistent if it satisfies the following two conditions: the estimated error has zero mean and its covariance is less than or equal to the covariance calculated by the estimator;

[0067] If the estimator is inconsistent, the covariance it calculates tends to be overconfident and therefore cannot characterize the estimation uncertainty. The overconfident covariance will lead to incorrect information gain, and the estimator will obtain information in the wrong direction, thus affecting the accuracy of the state estimation;

[0068] The standard extended Kalman filter has an inconsistency problem in collaborative localization, which is manifested as follows: the unobservable subspace of the original nonlinear system (7) is expanded along the direction of global position and orientation, that is,

[0069]

[0070] The null space (15) given above describes the unobservable directions of the ideal collaborative localization system, where the first column represents the global position and the second column represents the global direction. The last column of the unobservable subspace, the global direction, depends on the state of the system. Therefore, when the extended Kalman filter is used, the global direction becomes observable by mistake. The first two columns in (15) represent the global position, which is state-independent (time-invariant) and maintains the correct observability.

[0071] Multi-robot collaborative localization based on transformation consistency collaborative localization algorithm

[0072] 1) Linear time-varying transformation design

[0073] The introduction of linear time-varying transformation makes the unobservable subspace of the estimator system time-invariant, that is, it does not depend on the choice of linearization point to ensure correct observability. The state estimation of the transformed system solves the inconsistency caused by observability mismatch.

[0074] First, derive the linearized system (7) along the nominal trajectory, and assume Represents a nominal trajectory, then the current nominal state The nominal state at the previous moment Obey the state transfer equation:

[0075]

[0076] set up Then the first-order Taylor expansion of equation (7) at the nominal linearization point is as follows:

[0077]

[0078] in

[0079]

[0080] The coordinate transformation matrix is ​​as follows:

[0081]

[0082] Using T k Convert the error state to the transformed coordinates

[0083]

[0084] Thus we get the changed linearized system:

[0085]

[0086]

[0087] in

[0088]

[0089]

[0090] The consistency proof of the transformed system is as follows:

[0091] Let N k and denote the unobservable subspace of the original system (14) and the unobservable subspace of the transformed linearized system (18)-(19), respectively. Then, the unobservable subspace N k and the reversible transformation matrix T k Application to unobservable subspaces are completely equivalent, that is,

[0092]

[0093] The observability matrix of the original nonlinear system for the time interval between time steps k0 and k0+m is defined as:

[0094]

[0095] The unobservable subspace of the system is exactly the null space of the local observable matrix of the system. According to formula (15), it is verified that the unobservable subspace N k definition;

[0096] The locally observable matrix of the transformed linearized system (18)-(19) in time between time steps k0 and k0+m is defined as

[0097]

[0098] Substituting (20) and (21) into

[0099]

[0100] The unobservable subspace of the transformed linearized system (18)-(19) can be obtained As shown below:

[0101]

[0102] The unobservable direction becomes independent of the state and thus time-invariant;

[0103] 2) Transformation-based collaborative localization estimation

[0104] The unobservable subspace of the transformed linearized system is independent of the state and has time invariance. The KD-EKF algorithm can be obtained by filtering on the transformed linearized system. The specific content of the algorithm is as follows:

[0105] (1) Initialization

[0106] To start the filtering process, provide initial values ​​of state and covariance, the initial state and covariance As the initial value, the initial covariance of the error state after transformation According to the transformation equation (18), we have

[0107]

[0108] in is the estimated value given by T0, that is Initialization is performed only at the beginning of the state estimation, and then the remaining steps are implemented recursively.

[0109] (2) Prediction process

[0110] Each robot uses the latest self-motion information to pass forward the current state estimate according to (9) Get the predicted value

[0111]

[0112] Select the latest state estimate for formula (18) and As the linearization point, the linearized error state transfer equation of the estimator can be written as:

[0113]

[0114] in

[0115]

[0116] So

[0117]

[0118] (3) Update process

[0119] Select the state estimate for formula (19) and As the linearization point, the linearized error state measurement equation of the estimator can be written as:

[0120]

[0121] in

[0122]

[0123] Get the following updates

[0124]

[0125] where K k Represents the information gain matrix along the transformed coordinates:

[0126]

[0127] (4) Inverse transformation

[0128] Error state estimation and covariance estimation is expressed in the transformed coordinates, and the inverse transformation is performed to convert the estimated result back to the original coordinates. According to the error state transformation equation given in (17), the state update equation can be written as

[0129]

[0130] Furthermore, in the prediction process, in order to start the filtering process, it is necessary to provide initial values ​​of the state and covariance and convert these initial values ​​into the transformed coordinate system; initialization is only performed at the beginning of the state estimation, after which the prediction process and the update process will be performed recursively; since the error state estimation and covariance estimation are based on the transformed coordinate representation, an inverse transformation is required to convert the estimation results back to the original coordinate system.

[0131] A multi-robot collaborative positioning method system based on UWB relative ranging, the system has program modules corresponding to the steps of the above technical solution, and executes the steps of the multi-robot collaborative positioning method based on UWB relative ranging when running.

[0132] A computer-readable storage medium stores a computer program, wherein the computer program is configured to implement the steps of a multi-robot collaborative positioning method based on UWB relative ranging when called by a processor.

[0133] The present invention has the following beneficial technical effects:

[0134] In order to make full use of UWB to enhance the collaborative positioning performance of multi-robot system in a global navigation information denial environment, this paper designs a relative measurement scheme based on multiple UWB tags.

[23] , allowing the multi-robot system to establish implicit relative posture constraints through redundant relative distance measurements between robots; in order to deeply explore the mechanism of estimation inconsistency caused by the standard extended Kalman filter in collaborative localization, a linearized error state system is derived. By constructing a linearized error state system model, the essential difference between the linear approximation system and the original nonlinear system in the observability dimension is revealed from the perspective of observability theory; in response to the estimation inconsistency problem faced by the collaborative localization system based on multiple UWB tags in the global navigation information denial environment, a collaborative localization method based on observability decomposition is proposed to ensure consistent positioning estimation of the multi-robot system; in addition, in order to fully verify the effectiveness of the algorithm, the superiority of the scheme is verified from both theoretical derivation and engineering practice through Monte Carlo simulation and the developed physical experimental platform.

[0135] The present invention provides an implementation solution for the problem of consistent collaborative positioning of multiple robots based on UWB relative measurement. It solves the problem of cluster collaborative autonomous navigation of multi-robot systems in environments where external navigation equipment is unavailable or communication is weak. First, a state model for the multi-robot collaborative positioning problem and a UWB relative measurement model are established, and the linearized error state system is derived. Secondly, the reasons for the inconsistency of the standard extended Kalman filter (EKF) in collaborative positioning are analyzed, revealing that its essence stems from the observable asynchrony between the original nonlinear system and the linearized error state system of the estimator. Then, the present invention also designs a method based on linear time-varying transformation to construct a KD-EKF algorithm with a time-invariant unobservable subspace, and performs Monte Carlo comparative simulations to verify that the KD-EKF has higher positioning accuracy and better consistency than the standard EKF and FEJ-EKF methods. Finally, the present invention develops a multi-robot system experimental platform based on the Tucker ground robot and experimentally verifies the designed KD-CL algorithm. The experimental results verify the superiority and effectiveness of the proposed algorithm. Keywords in this invention: multi-robot system; nonlinear state estimation; collaborative positioning; consistency; extended Kalman filter; observability analysis; error state transformation. BRIEF DESCRIPTION OF THE DRAWINGS

[0136] Figure 1 An example scene graph for a collaborative positioning system. Figure 2 is a consistency diagram, Figure 3 is the root mean square error curve, Figure 4 is the normalized squared error curve, Figure 5 is the error and ±3σ bound diagram, Figure 6 Configure the image for the robot, Figure 7 This is the framework diagram of the consistent collaborative positioning system. Figure 8 is the robot's trajectory diagram, Figure 9 This is a scene diagram of the multi-robot collaborative positioning experiment. Figure 10This is the root mean square error curve of the robot physical experiment. DETAILED DESCRIPTION

[0137] Combined with attachment Figure 1-10 The implementation of a multi-robot collaborative positioning method and system based on UWB relative ranging according to the present invention is described in detail as follows:

[0138] Part 1 describes the problem. Part 2 establishes a state model for the multi-robot collaborative localization problem and a UWB relative measurement model, and derives a linearized error state system. By analyzing the unobservable matrix, the reasons for the inconsistency of EKF in CL are explained. Part 3 designs a linear time-varying transformation, constructs a KD-EKF estimator with a time-invariant unobservable subspace, and conducts simulation experiments. Part 4 conducts Monte Carlo comparative simulations to verify that the KD-EKF estimator has higher positioning accuracy and consistency. Part 5 builds a multi-robot collaborative localization experimental platform based on the Tucker ground robot to verify the superiority and effectiveness of the proposed algorithm.

[0139] 1 Problem description of multi-robot collaborative localization problem

[0140] Consider a group of n robots moving on a two-dimensional plane. Each robot is equipped with a proprioceptive sensor (such as an encoder) to obtain the robot's self-motion information (linear velocity and angular velocity), and carries two UWB tags with fixed and known positions. The system estimates the position and posture of all robots in the same coordinate system by fusing the odometry information with the relative measurement information of the UWB, such as Figure 1 shown.

[0141] 2 State model of multi-robot collaborative localization problem

[0142] 2.1 System State Model

[0143] During filter design, we modeled the localization problem as 2.5D, choosing a horizontal body coordinate system with its z-axis aligned with gravity. In collaborative localization tasks, a reference frame that only considers non-roll and non-pitch angles is often used to transform the full 3D problem into 2.5D, as roll and pitch angles can be recovered using an onboard inertial measurement unit (IMU). Therefore, only the robot's position and orientation need to be determined.

[0144] For the reasons mentioned above, the state vector contains n robot poses expressed in the global reference frame. Therefore, at time k, the state vector is given by:

[0145]

[0146] in, represents the position of robot i,i∈{1,2,…,n} at the kth moment, d = 2 or 3 and are the position and orientation of the robot, respectively.

[0147] At the kth moment, the motion equation of robot i is described as

[0148]

[0149] in, and They represent the linear velocity and angular velocity of robot i, respectively, which are the information provided by the odometer. represents Gaussian white noise, satisfying R(ψ i,k-1 ) is the corresponding rotation matrix, that is:

[0150]

[0151] For the entire system, the state vector, motion input vector, and process noise vector are defined as superposition vectors. The system's motion equation is:

[0152] x k =f(x k-1 ,u k ,ε k ) (3)

[0153] 2.2 System Measurement Model

[0154] In time k, the relative measurement information between 2n UWBs is used as relative measurement, and the relative measurement equation can be written as:

[0155]

[0156] in, UWB i ,i∈{1,2,...,n} and the relative measurement of UWB j,j∈{1,2,...,n} / i. is the measurement noise assumed to be Gaussian white noise and

[0157] Assume that the positions of the two UWBs equipped on each robot are L1 and L2 in the body coordinate system. The relative distance between L1 and L2 is known, then

[0158] h (ij) (x i,k ,x j,k )=||(p i,k +L i )-(pj,k +L j )||2 (5)

[0159] Among them L i ,L j ∈{L1,L2}.

[0160] h (ij) The relative measurement vector of the system can be represented by stacking them in columns. The relative measurement vector and measurement noise vector of the system are defined as superposition vectors. The relative measurement vector and measurement noise of the system are stacked to obtain the measurement equation of the system:

[0161] z k =h(x k )+η k (6)

[0162] 2.3 Linearized Error State System

[0163] Combining formula (3) with formula (6) can get the collaborative positioning model:

[0164]

[0165] Among them, x k is the state vector, z k is the measurement vector, f() and h() are the state function and measurement function of the system respectively, ε k is a Gaussian white noise with zero mean and covariance Q, η k is Gaussian white noise with zero mean and covariance R.

[0166] Next, we present the transfer and update process of the standard extended Kalman filter collaborative localization (EKF-CL). During the EKF transfer, each robot forwards its current state estimate based on the latest self-motion information. Get the predicted value Put formula (3) in Linearization at can obtain the error state transfer equation:

[0167]

[0168] in F i,k-1 and G i,k-1 is the state transfer Jacobian matrix and noise transfer matrix of robot i, specifically:

[0169]

[0170] in The linearized error state prediction equations of n robots are stacked to obtain the linearized error state prediction equation of the entire system

[0171]

[0172] Put formula (6) in Linearization at can obtain the linearized measurement error equation:

[0173]

[0174] in and They are h ij Jacobian with respect to the i-th and j-th robot poses:

[0175]

[0176] By stacking all the linearized measurement error equations, we can get the linearized measurement error equation of the system:

[0177]

[0178] Combining Equation (9) and Equation (13), the linearized error state system can be obtained as follows:

[0179]

[0180] The standard extended Kalman filter uses a linearized error state system model (14) for state estimation.

[0181] 2.4 Estimator Consistency Analysis

[0182] State estimation is a very important issue in the control community. How to accurately evaluate the accuracy of state estimation is one of the most important issues. In other words, whether the estimator is consistent. If the state estimator meets the following two conditions: the estimated error has zero mean, and its covariance should be less than or equal to the covariance calculated by the estimator, then it is said to be consistent, such as Figure 2 shown.

[0183] If the estimator is inconsistent, the covariance it calculates tends to be overconfident and therefore cannot characterize the estimation uncertainty. Overconfident covariance will lead to incorrect information gain, and the estimator will obtain information in the wrong direction, thus affecting the accuracy of state estimation. However, the standard extended Kalman filter has an inconsistency problem in collaborative positioning, which is manifested as follows: the unobservable subspace of the original nonlinear system (7) expands along the direction of global position and orientation.

[24] ,Right now

[0184]

[0185] The null space (15) given above describes the unobservable directions of the ideal co-localization system, where the first column represents the global position and the second column represents the global direction. The last column of the unobservable subspace, the global direction, depends on the state of the system. Therefore, when the extended Kalman filter is used, the global direction becomes observable incorrectly. The first two columns in (15) represent the global position, which is state-independent (time-invariant) and maintains the correct observability. Based on the above working analysis, it can be seen that the standard extended Kalman filter has fewer unobservable directions than the ideal co-localization system due to linearization. This will cause the estimator to obtain false information along the unobservable directions and ultimately lead to inconsistency.

[0186] 3 Transformation-based consistent collaborative positioning algorithm

[0187] 3.1 Linear time-varying transformation design

[0188] Based on the above analysis, the present invention introduces a linear time-varying transformation that renders the unobservable subspace of the estimator system time-invariant. This transformation, independent of the choice of linearization points, ensures correct observability. Consequently, using the transformed system for state estimation resolves the inconsistency caused by observability mismatch.

[0189] First we derive the linearized system (7) along the nominal trajectory. Represents a nominal trajectory, then the current nominal state The nominal state at the previous moment Obey the state transfer equation:

[0190]

[0191] set up Then the first-order Taylor expansion of (7) at the nominal linearization point is as follows:

[0192]

[0193] in

[0194]

[0195] The coordinate transformation matrix is ​​as follows:

[0196]

[0197] Using T k Convert the error state to the transformed coordinates

[0198]

[0199] Thus we get the changed linearized system:

[0200]

[0201]

[0202] in

[0203]

[0204] The consistency proof of the transformed system is as follows:

[0205] Theorem 1. Let N k and denote the unobservable subspace of the original system (14) and the unobservable subspace of the transformed linearized system (18)-(19) respectively. Then, the unobservable subspace N k and the reversible transformation matrix T k Application to unobservable subspaces are completely equivalent

[25] ,Right now,

[0206]

[0207] Proof: The observability matrix of the original nonlinear system at the time interval between time steps k0 and k0+m is defined as:

[0208]

[0209] 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 Definition as in (15).

[0210] The locally observable matrix of the transformed linearized system (18)-(19) at time steps between k0 and k0+m is defined as follows:

[0211]

[0212] Substituting (20) and (21) into

[0213]

[0214] Therefore, (23) can be proved and the proof is complete.

[0215] The unobservable subspace of the transformed linearized system (18)-(19) can be obtained As shown below:

[0216]

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

[0218] 3.2 Transformation-based Co-localization Estimator

[0219] The unobservable subspace of the transformed linearized system is independent of the state and has time invariance. We can obtain the KD-EKF algorithm by filtering on the transformed linearized system. The specific content of the algorithm is as follows:

[0220] 3.2.1 Initialization

[0221] In order to start the filtering process, initial values ​​of the state and covariance must be provided. Typically, the initial state and covariance As the initial value, the initial covariance of the error state after transformation According to the transformation equation (18), we have

[0222]

[0223] in is the estimated value given by T0, that is Initialization is performed only at the beginning of state estimation. Subsequently, the remaining steps are implemented recursively.

[0224] 3.2.2 Prediction Process

[0225] Each robot uses the latest self-motion information to pass forward the current state estimate according to (9) Get the predicted value

[0226]

[0227] Select the latest state estimate for formula (18) and As the linearization point, the linearized error state transfer equation of the estimator can be written as:

[0228]

[0229] in

[0230]

[0231] So

[0232]

[0233] 3.2.3 Update Process

[0234] Select the state estimate for formula (19) and As the linearization point, the linearized error state measurement equation of the estimator can be written as:

[0235]

[0236] in

[0237]

[0238] Then you can get the following update

[0239]

[0240] where K k Represents the information gain matrix along the transformed coordinates:

[0241]

[0242] 3.2.4 Inverse Transformation

[0243] Due to the error in state estimation and covariance estimation is expressed in transformed coordinates, so an inverse transformation is required to convert the estimated result back to the original coordinates. According to the error state transformation equation given in (17), the state update equation can be written as

[0244]

[0245] 4 Simulation Experiment

[0246] In this section, we conduct Monte Carlo simulations to verify the performance of the KD-EKF co-localization algorithm. We compare the KD-EKF algorithm with pure odometry integration, the extended Kalman filter (Std-EKF), and the first estimate Jacobian EKF (FEJ-EKF). For a comprehensive performance evaluation, we use two metrics: root mean square error (RMSE) and normalized estimation error square (NEES). For a consistent estimator, the NEES value should be close to the dimensionality of the state vector. The greater the deviation of the NEES value from the dimensionality, the poorer the consistency.

[0247] Consider a collaborative positioning system. There are four robots in the scene. Each robot is equipped with two UWBs. Their positions in the body coordinate system are L1 = [00.1] T and L2=[0-0.1] T The standard deviations of the linear velocity and angular velocity measurements of each robot are σ v =0.03m / s and σ w =0.03 rad / s, the noise standard deviation of the relative distance measurement is 0.01 m, and a total of 50 Monte Carlo simulations are performed with a simulation step size of 1 s. To ensure a fair comparison, all estimators use the same parameters.

[0248] Figure 3 and Figure 4 The root mean square error (RMSE) and normalized estimated square error (NEES) of the system's position and orientation in Monte Carlo simulations are presented, respectively. Table 1 summarizes the root mean square error (RMSE) and normalized estimated square error (NEES) of the average position and orientation in the simulations. Since this system has four robots and the state vector has a dimension of 12, the system's NEES should be close to 12. Both the figure and the table show that, because pure integration only utilizes odometry information, its localization performance is far inferior to that of the other three algorithms. The other algorithms leverage relative measurement information to create coupling between systems, effectively improving the localization performance of other robots in the system. The consensus algorithms FEJ-EKF and KD-EKF demonstrate superior consistency and accuracy compared to Std-EKF. Furthermore, the KD-EKF algorithm outperforms FEJ-EKF by a small margin. This is because the KD-EKF algorithm precisely eliminates factors that cause the unobservable subspace to shrink.

[0249] Table 1 Root mean square error and normalized estimated square error of Monte Carlo simulation

[0250]

[0251]

[0252] Figure 5 A plot of the error versus the ±3σ bound is shown. Over time, the ±3σ bounds of the FEJ-EKF and KD-EKF increase, while the ±3σ bound of the Std-EKF remains bounded. The orientation errors calculated by the FEJ-EKF and KD-EKF algorithms remain within the ±3σ bounds, while the Std-EKF error can sometimes exceed the ±3σ bounds. This indicates that the covariance calculated by the Std-EKF algorithm tends to be overconfident, failing to represent the estimation uncertainty and resulting in incorrect information gain, which in turn affects the accuracy of the state estimate. The improved algorithm significantly improves this situation.

[0253] 5. Physical Experiment

[0254] To verify the feasibility of the proposed system, we built a collaborative positioning system consisting of three two-wheel differential drive robots. The detailed configuration of the platform is as follows: Figure 6 Specifically, we used the TARKBot-R20 as the robot platform, and each robot was equipped with a Raspberry Pi 5 as the embedded computer. Each robot was equipped with an STM32, which was responsible for obtaining wheel speed from the encoder, and two Nooploop UWB modules for absolute distance measurement. Its position in the body coordinate system was L1 = [00.15] T and L2 = [0-0.15]T To evaluate the localization performance, we use the Nokov motion capture system to track the robot position and orientation as the ground truth.

[0255] The overall operating framework of the system is as follows Figure 7 As shown in the figure. After the system obtains the wheel speed through the encoder, the differential drive model can be used to obtain the system's self-motion information (including linear velocity and angular velocity). Using this self-motion information, the robot can perform state transfer. The system can use UWB to obtain relative position information between machines, and use relative measurement information to update the state. However, it should be noted that the error state estimate and covariance estimate obtained during the transfer-update process are expressed in transformed coordinates. Therefore, an inverse transformation is required to convert the estimation results back to the original coordinates to obtain the robot's state estimate and covariance estimate in the original coordinate system.

[0256] The robot is programmed to follow a predefined trajectory, e.g. Figure 8 Specifically, one robot was assigned to track a figure eight pattern, while the other two robots moved along circular trajectories. Figure 9 The experiment was conducted in an indoor scenario shown in Figure 2, and the entire experiment lasted approximately 180 seconds. Using onboard ultra-wideband (UWB) tags, the robots were able to continuously detect the relative distance measurements between each other at a frequency of 50 Hz.

[0257] Figure 10 The average root mean square error (RMSE) of the estimators across five repeated experiments is shown. It can be seen that KD-EKF achieves higher overall positioning accuracy. While KD-EKF significantly outperforms the other estimators in heading angle estimation, this advantage is not evident in position estimation. This may be because the system noise is not Gaussian, resulting in no significant improvement in positioning accuracy using the consensus algorithm.

[0258] 6 Conclusion

[0259] This paper addresses the problem of multi-robot collaborative localization based on UWB relative distance measurement and obtains the following research results: First, by establishing a state model, a relative measurement model, and a linearized error state system. Secondly, the root cause of the consistency problem of the classical extended Kalman filter (EKF) in collaborative localization is analyzed, namely the observability mismatch between the original nonlinear system and the linearized error state system. Then, to solve this problem, an improved algorithm based on linear time-varying transformation is proposed. By converting the unobservable subspace of the estimator system into a time-invariant form, the correctness of the system observability is ensured. Theoretical analysis shows that this method can effectively eliminate the influence of linearization points on the unobservable subspace of the system. Finally, the superior performance of the proposed algorithm in terms of positioning accuracy and consistency is verified through Monte Carlo simulation and physical experiments, confirming the effectiveness and practicality of the method.

[0260] The references cited in this paper are as follows:

[0261] [1] D,Alitappeh RJ,Pimenta LCA,et al.Dynamic PerimeterSurveillance with aTeam of Robots[C].2016IEEE International Conference onRobotics and Automation(ICRA),Stockholm,Sweden,May 16-21,2016.

[0262] [2] Queralta JP, Taipalmaa J, Can Pullinen B, et al. Collaborative Multi-Robot Search andRescue: Planning, Coordination, Perception, and Active Vision [J]. IEEE Access, 2020, 8: 191617-191643.

[0263] [3]Cossette C C,Shalaby M A,SaussiéD,et al.Optimal Multi-robotFormations for RelativePose Estimation Using Range Measurements[C].2022IEEE / RSJ International Conferenceon Intelligent Robots and Systems(IROS),Kyoto,Japan,October 23-27,2022.

[0264] [4]Xun Z,Huang J,Li Z,et al.CREPES:Cooperative RElative PoseEstimation System[C].2023IEEE / RSJ International Conference on IntelligentRobots and Systems(IROS),Detroit,USA,October 1-5,2023.

[0265] [5]Zhao B,Liang Y,Dong X,et al.An Improved Motion Capture System forMultipleWheeled Mobile Robots Based on KCF and GMM[C].2019Chinese ControlConference(CCC),Guangzhou,China,July 27-30,2019.

[0266] [6]Vásárhelyi G,Virágh Cs,Somorjai G,et al.Outdoor Flocking andFormation Flight withAutonomous Aerial Robots[C].2014IEEE / RSJ InternationalConference on IntelligentRobots and Systems(IROS),Chicago,USA,September 14-18,2014.

[0267] [7] Zhang T, Yuan M, Wei L, et al. MR-ULINS: A Tightly-Coupled UWB-LiDAR-InertialEstimator With Multi-Epoch Outlier Rejection[J]. IEEE Robotics and Automation Letters, 2024, 9(12): 11786-11793.

[0268] [8]Horyna J, V,Pritzl V,et al.Fast Swarming of UAVs in GNSS-Denied Feature-PoorEnvironments Without Explicit Communication[J].IEEERobotics and Automation Letters,2024,9(6):5284-5291.

[0269] [9] Jin Guizhong, Chen Guocan, Zhao Lanlan, et al. Application of Kalman filter correction technology based on multi-source forecast residuals [J]. Journal of Hohai University (Natural Science Edition), 2023: 1-12.

[0270]

[10] Sun Jiaqing, Chen Xuan, Zhai Lijuan, et al. Identification and analysis of inertia parameters of three-axis turntable frame based on extended Kalman filter algorithm [J]. Integrated Circuit Applications, 2023, 40(8): 22-24.

[0271]

[11] Xu Yao, Sun Yongzhi, Ma Lianwei. A self-localization algorithm for wheeled mobile robots based on IEKF[J]. Journal of Zhejiang University of Science and Technology, 2021, 33(1): 39-45.

[0272]

[12] Bar-Shalom Y,Kirubarajan T,Li X R.Estimation with Applications toTracking andNavigation[M].New York:John Wiley&Sons,2001.

[0273]

[13] Huang G P,Trawny N,Mourikis A I,et al.Observability-basedconsistent EKF estimatorsfor multi-robot cooperative localization[J].Autonomous Robots,2011,30(1):99-122.

[0274]

[14] Huang G.Improving the consistency of nonlinear estimators:analysis,algorithms,andapplications[D].Minneapolis:University of Minnesota,2012.

[0275]

[15] Huang G P,Mourikis A I,Roumeliotis S I.A First-Estimates JacobianEKF for ImprovingSLAM Consistency[C].2008 11th International Symposium onExperimental Robotics(ISER),Athens,Greece,July 2008.

[0276]

[16] Hesch J A,Kottas D G,Bowman S L,et al.Camera-IMU-basedLocalization:Observability Analysis and Consistency Improvement[J].TheInternational Journal ofRobotics Research,2014,33(1):182-201.

[0277]

[17] Hao N, He F, Tian C, et al. KD-EKF: A Consistent CooperativeLocalization EstimatorBased on Kalman Decomposition[C]. 2023 IEEE / RSJInternational Conference on Intelligent Robots and Systems (IROS), Detroit, USA, October 1-5, 2023.

[0278]

[18] Tian CG, Hao N, He FH, Yao H D.Consistent Distributed CooperativeLocalization:ACoordinate Transformation Approach[C]. 2024IEEE / RSJInternational Conference on Intelligent Robots and Systems(IROS),Abu Dhabi,United Arab Emirates,October 14-18,2024:10297-10303.

[0279]

[19] Hu Jing, Tian Junxi, Zou Shiming, et al. Joint RSS / TOA / INS UAV positioning algorithm based on adaptive Kalman filter [J]. Unmanned Systems Technology, 2022, 5(02): 62-70.

[0280]

[20] Sun Dayang, Bai Enhao, Li Hongyang, et al. Implementation of cluster collaboration platform based on UWB local measurement[J]. Unmanned Systems Technology, 2021, 4(05): 61-70.8.

[0281]

[21] Fishberg A,How J P.Multi-Agent Relative Pose Estimation with UWBand ConstrainedCommunications[C].2022IEEE / RSJ International Conference on Intelligent Robots andSystems(IROS),Kyoto,Japan,October 23-27,2022:778-785.

[0282]

[22] Nguyen T H,Nguyen T M,Xie L.Range-Focused Fusion of Camera-IMU-UWB forAccurate and Drift-Reduced Localization[J].IEEE Robotics andAutomation Letters,2021,6(2):1678-1685.

[0283]

[23] Yuan S,Wang H,Xie L.Survey on Localization Systems and Algorithmsfor UnmannedSystems[J].Unmanned Systems,2021,9(2):129-163.

[0284]

[24] Hao N,He F,Xie C,et al.Nonlinear observability analysis of multi-robot cooperativelocalization[J].Systems&Control Letters,2022,168:105340.

[0285]

[25] Hao N,He F,Tian C,Hou Y.On the Consistency of Multi-RobotCooperative Localization:A Transformation-Based Approach[J].IEEE Robotics andAutomation Letters,2025,10(1):280-287.

Claims

1. A multi-robot collaborative positioning method based on UWB relative ranging, characterized in that: The method defines a group of n moving robots on a two-dimensional plane. Each robot is equipped with a proprioception sensor to obtain the robot's self-motion information and carries two UWB tags with fixed and known positions. The position and posture of all robots in the same coordinate system are estimated by fusing odometry information with relative measurement information from UWB. The implementation process of the method is as follows: Construct a multi-robot collaborative localization problem model: Kinematic model: The equations of motion for robot i are given based on the state vectors containing n robot poses expressed in the global reference frame. System measurement model: The system's relative measurement vector and measurement noise vector are defined as superposition vectors. The system's measurement equation is obtained by superimposing the system's relative measurement vector and measurement noise. Linearized error state system model: Combining the motion model with the system measurement model yields a collaborative positioning model. Each robot forwards its current state estimate based on the latest self-motion information to obtain a predicted value. The linearized error state prediction equations of n robots are stacked to obtain the linearized error state prediction equation of the entire system. The system measurement model is linearized to obtain the system's linearized measurement error equation, and the linearized error state system model can be obtained by combining the system's linearized error state prediction equation and the system's linearized measurement error equation. Estimator consistency analysis: The consistency of the standard extended Kalman filter is analyzed. The standard extended Kalman filter uses a linearized error state system model for state estimation. Combining the motion model and the system measurement model can obtain an ideal co-localization model. By comparing the unobservable subspaces of the standard extended Kalman filter and the ideal co-localization system model, it is found that due to linearization, the standard extended Kalman filter has fewer unobservable directions than the ideal co-localization system. This will cause the estimator to obtain false information along unobservable directions, and ultimately lead to inconsistency. The standard extended Kalman filter is improved by transforming the unobservable subspace into a time-invariant one, thus avoiding the inconsistency caused by the observability mismatch caused by linearization. After making the above improvements to the standard extended Kalman filter, a transformation-based consistency collaborative localization algorithm is obtained. The transformation-based consistency collaborative localization algorithm is used for multi-robot collaborative localization: Linear time-varying transformation design: Introducing a linear time-varying transformation to make the unobservable subspace of the estimator system time-invariant, that is, independent of the choice of linearization points, to ensure correct observability; Transformation-based co-localization estimator: The transformation-based co-localization estimator performs co-localization estimation, specifically by providing initial values ​​of state and covariance, and then performing prediction, update process, and inverse transformation.

2. The multi-robot collaborative positioning method based on UWB relative ranging according to claim 1, characterized in that: The specific implementation process of the method is as follows: Construct a multi-robot collaborative localization problem model: 1) System state model The state vector contains n robot poses expressed in the global reference frame. At time k, the state vector is given by: in, represents the position of robot i,i∈{1,2,…,n} at the kth moment, d = 2 or 3 and are the position and orientation of the robot, respectively. At the kth moment, the motion equation of robot i is described as in, and They represent the linear velocity and angular velocity of robot i, which are information provided by the odometry; represents Gaussian white noise, satisfying R(ψ i,k-1 ) is the corresponding rotation matrix, that is: The state vector, motion input vector and process noise vector are defined as superposition vectors, and the motion equation of the system is: x k =f(x k-1 ,u k ,ε k ) (3) 2) System measurement model In time k, the relative measurement information between 2n UWBs is used as relative measurement, and the relative measurement equation can be written as: in, UWB i ,i∈{1,2,...,n} and the relative measurement of UWB j,j∈{1,2,...,n} / i; is the measurement noise assumed to be Gaussian white noise and Assume that the positions of the two UWBs equipped on each robot are L1 and L2 in the body coordinate system. The relative distance between L1 and L2 is known, then h (ij) (x i,k ,x j,k )=||(p i,k +L i )-(p j,k +L j )||2 (5) Among them L i ,L j ∈{L1,L2}; h (ij) The relative measurement vector of the system can be represented by stacking them in columns. The relative measurement vector and measurement noise vector of the system are defined as superposition vectors. The measurement equation of the system is obtained by stacking the relative measurement vector and measurement noise of the system: z k =h(x k )+η k (6) 3) Linearized error state system Combining formula (3) with formula (6) can get the collaborative positioning model: Among them, x k is the state vector, z k is the measurement vector, f(·) and h(·) are the state function and measurement function of the system respectively, ε k is a Gaussian white noise with zero mean and covariance Q, η k is a Gaussian white noise with zero mean and covariance R; The standard extended Kalman filter collaborative localization (EKF-CL) transmission and update process is given: During the EKF transmission, each robot forwards the current state estimate based on the latest self-motion information Get the predicted value Put formula (3) in Linearization at can obtain the error state transfer equation: in F i,k-1 and G i,k-1 is the state transfer Jacobian matrix and noise transfer matrix of robot i, specifically: in The linearized error state prediction equations of n robots are stacked to obtain the linearized error state prediction equation of the entire system Put formula (6) in Linearization at can obtain the linearized measurement error equation: in and They are h ij Jacobian with respect to the i-th and j-th robot poses: By stacking all the linearized measurement error equations, we can get the linearized measurement error equation of the system: Combining Equation (9) and Equation (13), the linearized error state system can be obtained as follows: The standard extended Kalman filter uses a linearized error state system model (14) for state estimation; 4) Estimator consistency analysis A state estimator is said to be consistent if it satisfies the following two conditions: the estimated error has zero mean and its covariance is less than or equal to the covariance calculated by the estimator; If the estimator is inconsistent, the covariance it calculates tends to be overconfident and therefore cannot characterize the estimation uncertainty. The overconfident covariance will lead to incorrect information gain, and the estimator will obtain information in the wrong direction, thus affecting the accuracy of the state estimation; The standard extended Kalman filter has an inconsistency problem in collaborative localization, which is manifested as follows: the unobservable subspace of the original nonlinear system (7) is expanded along the direction of global position and orientation, that is, The null space (15) given above describes the unobservable directions of the ideal collaborative localization system, where the first column represents the global position and the second column represents the global direction. The last column of the unobservable subspace, the global direction, depends on the state of the system. Therefore, when the extended Kalman filter is used, the global direction becomes observable by mistake. The first two columns in (15) represent the global position, which is state-independent (time-invariant) and maintains the correct observability. Multi-robot collaborative localization based on transformation consistency collaborative localization algorithm 1) Linear time-varying transformation design The introduction of linear time-varying transformation makes the unobservable subspace of the estimator system time-invariant, that is, it does not depend on the choice of linearization point to ensure correct observability. The state estimation of the transformed system solves the inconsistency caused by observability mismatch. First, derive the linearized system (7) along the nominal trajectory, and assume Represents a nominal trajectory, then the current nominal state The nominal state at the previous moment Obey the state transfer equation: set up Then the first-order Taylor expansion of equation (7) at the nominal linearization point is as follows: in The coordinate transformation matrix is ​​as follows: Using T k Convert the error state to the transformed coordinates Thus we get the changed linearized system: in The consistency proof of the transformed system is as follows: Let N k and denote the unobservable subspace of the original system (14) and the unobservable subspace of the transformed linearized system (18)-(19), respectively. Then, the unobservable subspace N k and the reversible transformation matrix T k Application to unobservable subspaces are completely equivalent, that is, The observability matrix of the original nonlinear system for the time interval between time steps k0 and k0+m is defined as: The unobservable subspace of the system is exactly the null space of the local observable matrix of the system. According to formula (15), it is verified that the unobservable subspace N k definition; The locally observable matrix of the transformed linearized system (18)-(19) at time steps between k0 and k0+m is defined as follows: Substituting (20) and (21) into The unobservable subspace of the transformed linearized system (18)-(19) can be obtained As shown below: The unobservable direction becomes independent of the state and thus time-invariant; 2) Transformation-based collaborative localization estimation The unobservable subspace of the transformed linearized system is independent of the state and has time invariance. The KD-EKF algorithm can be obtained by filtering on the transformed linearized system. The specific content of the algorithm is as follows: (1) Initialization To start the filtering process, provide initial values ​​of state and covariance, the initial state and covariance As the initial value, the initial covariance of the error state after transformation According to the transformation equation (18), we have in is the estimated value given by T0, that is Initialization is performed only at the beginning of the state estimation, and then the remaining steps are implemented recursively. (2) Prediction process Each robot uses the latest self-motion information to pass forward the current state estimate according to (9) Get the predicted value Select the latest state estimate for formula (18) and As the linearization point, the linearized error state transfer equation of the estimator can be written as: in So (3) Update process Select the state estimate for formula (19) and As the linearization point, the linearized error state measurement equation of the estimator can be written as: in Get the following updates where K k Represents the information gain matrix along the transformed coordinates: (4) Inverse transformation Error state estimation and covariance estimation is expressed in the transformed coordinates, and the inverse transformation is performed to convert the estimated result back to the original coordinates. According to the error state transformation equation given in (17), the state update equation can be written as 3. The multi-robot collaborative positioning method based on UWB relative ranging according to claim 1, characterized in that: The specific implementation process of the method is as follows: in the prediction process, in order to start the filtering process, it is necessary to provide initial values ​​of the state and covariance, and convert these initial values ​​into the transformation coordinate system; Initialization is only performed at the beginning of the state estimation. After that, the prediction process and the update process will be performed recursively. Since the error state estimation and covariance estimation are based on the transformed coordinate representation, an inverse transformation is required to convert the estimation results back to the original coordinate system.

4. A multi-robot collaborative positioning method system based on UWB relative ranging, characterized by: The system has a program module corresponding to the steps of any one of claims 1 to 3 above, and executes the steps of the multi-robot collaborative positioning method based on UWB relative ranging when running.

5. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, and the computer program is configured to implement the steps of a multi-robot collaborative positioning method based on UWB relative ranging according to any one of claims 1 to 3 when called by a processor.

Images