{'fraction_non_alphanumeric': 0.028020761496860367, 'fraction_numerical': 0.00965482196653479, 'mean_word_length': 4.801642451042325, 'pattern_counts': {'":': 0, '<': 1, '<?xml version=': 0, '>': 0, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 1, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
Distributed Ranging SLAM for Multiple Robots with Ultra-WideBand and Odometry Measurements

To accomplish task efficiently in a multiple robots system, a problem that has to be addressed is Simultaneous Localization and Mapping (SLAM). LiDAR (Light Detection and Ranging) has been used for many SLAM solutions due to its superb accuracy, but its performance degrades in featureless environments, like tunnels or long corridors. Centralized SLAM solves the problem with a cloud server, which requires a huge amount of computational resources and lacks robustness against central node failure. To address these issues, we present a distributed SLAM solution to estimate the trajectory of a group of robots using Ultra-WideBand (UWB) ranging and odometry measurements. The proposed approach distributes the processing among the robot team and significantly mitigates the computation concern emerged from the centralized SLAM. Our solution determines the relative pose (also known as loop closure) between two robots by minimizing the UWB ranging measurements taken at different positions when the robots are in close proximity. UWB provides a good distance measure in line-of-sight conditions, but retrieving a precise pose estimation remains a challenge, due to ranging noise and unpredictable path traveled by the robot. To deal with the suspicious loop closures, we use Pairwise Consistency Maximization (PCM) to examine the quality of loop closures and perform outlier rejections. The filtered loop closures are then fused with odometry in a distributed pose graph optimization (DPGO) module to recover the full trajectory of the robot team. Extensive experiments are conducted to validate the effectiveness of the proposed approach.


I. INTRODUCTION
Multiple robots system collectively accomplishes a task by the collaboration of a group of robots, which is shown to be efficient and robust when compared to single robot solution [1]. Localization is crucial for the robot team to coordinate with each other. The literature shows a number of mature techniques and implementations for localization in known infrastructures. However, a prior knowledge of the environment is not always available in some scenarios. For example, emergency response often requires a group of robots to explore unknown site within a minimal time.
Therefore, it is crucial for a group of robots to realize Simultaneous Localization and Mapping (SLAM) in unknown environments. Most multiple robots SLAM systems are based on centralized or distributed architecture [2] [3]. Overview of the proposed distributed ranging SLAM with two robots. Each robot carries a UWB node for ranging of the nearby robots. We perform distributed pose estimation with short-term UWB ranging and odometry measurements (see the two cyan rectangles) when the two robots are in close proximity. The estimated poses are passed for outlier rejection and then optimized through a distributed pose graph optimization module.
Centralized solution processes all collected measurements in a server [2]. The computational cost increases exponentially with the increase of the number of robots. Distributed system outperforms centralized solution in scalability and computational efficiency, as the computation is allocated among a group of robots [3]. We are interested in distributed SLAM solution for multiple robots during the exploration of unknown environments.
SLAM has been extensively researched by the robotics community with LiDAR and visual-based sensors [4], which exhibit good localization accuracy in general, but it is challenging to deal with featureless scenes (for example tunnels) and the algorithms often require a lot of computational resources. Due to its high accuracy and detection without line-of-sight, Ultra-WideBand (UWB) is widely used for localization [5] [6]. Although a precise distance is provided by UWB, the lack of bearing information makes it difficult to be applied for the navigation of autonomous robots. The conventional Angle-of-Arrival (AoA)-based solution requires a cumbersome setup for phase shift estimation [7]. We focus on the use of purely UWB ranging and odometry for pose estimation among a robot team. The proposed solution is supposed to work in non-line-of-sight conditions, where LiDAR and visual sensors often fail to perform localization due to the block of the environment. This paper presents a distributed SLAM for trajectory estimation of a group of robots based on UWB ranging and odometry measurements. Each robot is equipped with a UWB node for ranging, as shown in Figure 1.
We estimate the pose (position and orientation) between the robots using short-term UWB ranging and odometry observations. In particular, we achieve the distributed pose estimation (i.e., loop closure) through the minimization of ranging measurements taken at different positions when two robots are close. The UWB is susceptible to non-line-of-sight (NLOS) propagation [8], which results in erroneous loop closures, in particular when the path traveled by the robot is unpredictable [9]. To remove erroneous loop closures, we utilize the distributed Pairwise Consistency Maximization (PCM) algorithm to verify the quality of loop closure pairs and perform oulier rejections. The inlier loop closures are finally incorporated with the odometry through Distributed Pose Graph Optimization (DPGO) to estimate the trajectory of the robots. We evaluated the performance of the proposed approach with three robots in an indoor environment. The contributions of this paper are summarized as follows: 1) We propose an approach for distributed pose estimation between two robots using short-term UWB ranging and odometry measurements. In particular, we combine coarse search and optimization strategy to avoid the convergence to a local minimum. 2) We utilize distributed outlier rejection to remove the erroneous estimations and apply distributed pose graph optimization for trajectory estimation. To the best of our knowledge, this is the first work that uses UWB ranging for distributed SLAM with multiple robots. 3) We perform extensive experiments to evaluate the performance of the proposed approach. The results show that we achieved a positioning accuracy of 0.45m in translation and 4.01°in rotation by the fusing of UWB and odometry measurements in an indoor environment with a size of 10m×12m. We organize the remaining of this paper as follows: Section II introduces the related work. Section III describes the proposed distributed ranging SLAM. Section IV shows the experimental setups and the experimental results. Finally, Section V concludes this paper and discusses the future work.

II. RELATED WORK
The robotics research community is showing a growing interest in multiple robots system, due to the efficiency by collaboration of a robot team. The system is extremely useful when multiple robots with different functionalities are used for exploration of unknown environments, like underground exploration in DARPA subterranean challenge 1 . SLAM is considered fundamental to achieve this task due to its capability of localization and mapping in unknown environments. Tremendous progress has been made concerning multiple robots SLAM with different sensors and implementations.
Visual cameras and LiDARs are popular for localization and SLAM in robotics, due to their high accuracy [4] [10] [11]. Kimera-multi [4] is presented for robust distributed trajectory estimation and 3D metric-semantic meshing in multiple robots applications. Ebadi et al. [ a LiDAR-based SLAM and tested it for the exploration of underground tunnels with multiple robots. CamVox [11] is a low-cost and robust SLAM solution that combines Livox LiDAR and visual features for robust sensor fusion.
However, the performance of LiDAR and visual SLAM decreases in perceptually-degraded environments, like tunnels and long corridors. Due to detection without line-ofsight and high accuracy, UWB has been used for localization in many industrial applications. A sensor fusion scheme is presented in [12] to combine visual, IMU, and UWB for accurate and low-drift localization. Liu et al. [13] combined short-range LiDAR and UWB for mapping of mobile robots in indoor environments. Cao et al. [14] proposed a solution to localize a moving robot with a single UWB anchor based on the fusion of IMU with an Extended Kalman Filter. Funabiki et al. [15] presented an approach to combine UWB beacon and LiDAR-based geometric loop closures for SLAM in large and perceptually challenging environments.
The implementation of multiple robots SLAM can be briefly classified into centralized and distributed. The centralized SLAM requires a server for data collection and sensor fusion. Cao et al. [16] used four UWB nodes with a square configuration to determine the pose between two robots. But equipping multiple UWB sensors is costly and is not suitable for small robot applications. Liu et al. [1] proposed an approach to combine IMU and UWB for localization of multiple users. Martel [17] et al. presented an approach for relative pose estimation with six ranging measurements based on linear algorithm. Nguyen et al. [18] presented an approach to fuse UWB, IMU, and visual measurements to achieve multi-robot collaborative localization. A centralized system is not scalable with large robot teams, due to the increase of computation and communication requirements.
In distributed SLAM, the team works collaboratively to achieve the goal of SLAM and the computation is distributed to peer robots. The Gauss-Seidel approach is presented in [19] for distributed trajectory estimation, which is shown to scale well to large robot teams. Tian et al. [3] presented a distributed SLAM solution based on sparse semidefinite relaxation, which is guaranteed to be globally optimal under moderate measurement noise. This approach is improved by [20] to balance the computational and communication cost. Lajoie et al. [21] presented DOOR-SLAM, which is a fully distributed SLAM system based on robust place recognition.

III. DISTRIBUTED RANGING SLAM FOR MULTIPLE ROBOTS
In this section, we formulate the problem of distributed SLAM with UWB ranging and odometry for a group of robots. Then, we show the details of our proposed distributed SLAM, which mainly consists of the following three modules (see Figure 2). Firstly, each robot performs distributed pose estimation to determine the pose of its peer robots using short-term UWB ranging and odometry measurements. Secondly, each robot runs a distributed outlier rejection module to filter out the suspicious estimations. Thirdly, a distributed SLAM framework is used to fuse the inliers and the odometry to produce the trajectory in a common reference frame.
The robot team consists of N robots and we denote the (2) represents the rotation (i.e., θ t α ) and t t α ∈ R 2 represents the 2D position (i.e., x t α and y t α ). x k,i α denotes the relative pose from time k to time i measured by odometry of robot α. We use r t α = {r t α,β } (β ∈ N t α ) to denote the UWB ranging measurements received by robot α at time t, where N t α denotes the set of neighboring robots of robot α and r t α,β represents the UWB ranging between robot α and robot β at time t. An example of the problem can be illustrated in Figure 2, where two robots are performing distributed SLAM in unknown environment. Each robot carries a UWB node for ranging to its peer robots in close proximity. The robot also offers odometry to represent its relative movement. The goal is to achieve the trajectory estimation for multiple mobile robots through the UWB ranging and odometry measurements without a prior knowledge about the infrastructure. Our approach works in a distributed fashion, where each robot will perform its pose estimation by considering the measurements (i.e., odometry and UWB) received from all neighboring robots.

A. Module 1: Distributed Pose Estimation with Short-term UWB and Odometry Measurements
The estimation of the relative pose x t α,β (i.e., pose of robot β with reference to robot α at time t) can be achieved by finding the best pose configuration x t α,β through minimizing the residual error of UWB ranging taken at different positions (denoted by the odometry) within a sliding time window τ : Residual error of UWB ranging = arg min where e(·) denotes the residual function that computes the residual error of a UWB ranging given a pose configuration x t α,β . || · || computes the Euclidean distance between two positions. τ denotes the size of the sliding window for pose estimation. The optimization of Equation 1 can be solved efficiently by state-of-the-art optimization tools, such as g2o [22], gtsam [23], and Ceres [24], if a good initial guess of x is provided. Due to the non-convexity of Equation 1, the optimization converge to a local minimum without a proper initial guess and there is no guarantee to find the best solution.
Algorithm 1: Find coarse relative pose x t α,β between robot α and robot β with short-term UWB and odom.
Data: Odometry from robot α: x i α , odometry from robot β: x i β , UWB ranging between α and β: if r < minimumResidual then We propose to use a coarse search strategy to find a decent approximation of relative pose as initial guess for optimization in Equation 1. Based on coarse guess, we then perform nonlinear optimization with Equation 1. As UWB ranging is considered to be accurate, robot β will approximately located on a circle centered at robot α with a radius r t α,β (known by UWB ranging) and an angle φ (to be estimated) in polar coordinate system. Therefore, instead of searching the whole 2D space with Equation 1, we search in a polar space with angle between −π and π given the known UWB ranging. The angular searching step size for θ and φ is denoted as δ. The number of searching steps (i.e., w φ and w θ ) for θ and φ are denoted as w φ = w θ = ⌈ π δ ⌉. We define the entire searching space as The optimization of Equation 1 is approximated to find the minimum residual given x t α,β ∈ w: An overview of the algorithm is shown in Algorithm 1. The residual errors with respect to different φ and θ for two different test cases are shown in Figure 3. The figure shows the residual follows a multimodal distribution, and a global minimum might be not found from an arbitrary initial guess (for example the Case II in Figure 3). The coarse solution x t α,β is used as initial guess to optimize Equation 1 via g2o [22]. The node in the graph is denoted as the pose to be estimated (i.e., x t α,β ) and the constraints are represented by the UWB ranging measurements. The algorithm turns out to find the best configuration of the pose to satisfy the UWB ranging constraints based on maximum likelihood estimation.

B. Module 2: Distributed Outlier Rejection
The pose estimation accuracy in Section III-A highly depends on UWB ranging accuracy and paths of robots. These factors are not controllable for a practical applications, especially during autonomous navigation of robot in unknown environment with non-line-of-sight conditions. Therefore, we use state-of-the-art outlier rejection algorithm, namely Pairwise Consistency Maximization (PCM) [25], to deal with suspicious pose estimations (i.e., loop closures) and tailor it to a distributed and online fashion. Given two loop closures x k α,β and x i α,β between robot α and β, the following equation is used to determine if two loop closures are pairwise consistent: where || · || Σ represents the Mahalanobis distance. ⊕ and ⊖ denote composition and inversion operators of two poses [26]. An illustration of consistency check is shown in Figure  4. In ideal conditions, where all pose estimations are without noise, the composition of the poses along the cycle, which is formed by the two odometries (green lines in Figure 4) and two loop closures (cyan lines in Figure 4) is identify. Equation 3 evaluates if the error accumulated along the cycle is consistent with the noise covariance Σ. χ 2 ǫ is the quantile of the Chi-squared test for a given significance level ǫ [21].
After consistency check by Equation 3, the PCM builds an adjacency matrix to record the pairwise consistency of the loop closures. The robot computes the maximum clique from the matrix, which gives the largest set of pairwise consistent loop closures (i.e., inliers). To reduce computational cost for online operations, we use an efficient version of PCM [4], which incrementally updates the consistent loop closures with a heuristic search strategy to find the maximum clique. The inliers are passed to the pose graph for optimization, which will be detailed in Section III-C.

C. Module 3: Distributed Pose Graph Optimization
Early multi-robot SLAM solutions focus on the centralized optimization. The computation increases with the increase of robots, making this solution challenging in large environment with a large number of robots. In distributed SLAM [19], each robot solves its pose graph optimization based on local information. The robots exchange their estimations to iteratively solve the global optimization problem. We follow the approach proposed in [3]. Each robot runs a DPGO module for trajectory optimization based on the odometry and the inlier loop closures. Particularly, DPGO performs local optimization by Riemann gradient descent. It solves a rank-restricted relaxation of pose graph optimization in a distributed manner. During the update, the algorithm only requires the exchange of the poses connecting the loop closures and therefore saves a large amount of communication resources by avoiding the exchange of all pose information.
Communication between robots is crucial in distributed SLAM system. The exchange of odometry is required in two modules, namely distributed pose estimation module in Section III-A and distributed outlier rejection in Section III-B. The former shares short-term odometry measurements when two robots are in close proximity. The latter uses the relative pose estimation and its associated odometry for consistency check. Our solution is particularly suitable for the applications in ad hoc networks where external communication infrastructure is not deployed. For ad hoc networks, the robots are moving freely to form dynamic network topology, which leads to a limited communication range. Our implementation exchanges the updated poses and loop closures when the robots are in close proximity, while the communication is not required when the two robots are out of the UWB ranging.

A. Experimental Setups
We present experimental setups to demonstrate the proposed approach with three robots in an indoor environment with a size of 10m×12m. In our setup, each robot carried one UWB node (Nooploop LinkTrack 1 ) with a maximum range up to 100 meters. The sampling rate of UWB is set to 50Hz. The robot outputs the odometry measurements (differential drive with encoders) with a frequency of 10Hz. Three robots were manually controlled to move along different paths.
All modules are running under ROS (Robot Operating System). During the experiment, the maximum velocity of the robot was 0.2m/s. The total distances travelled by the three robots were 63.5m, 60.6m, and 57.1m, respectively. To obtain the ground truth, Hokuyo LiDARs were installed to perform adaptive Monte Carlo localization 2 given a map created by GMapping. The number of poses collected by the three robots for evaluation were 314, 363, and 333, respectively. An overview of the experimental snapshot is shown in Figure 5. The relative translational and rotational errors are computed by comparing the estimated poses with the ground truth. The mean squared error (MSE) in transla-   tion and rotation are computed as a metric to evaluate the positioning accuracy.

B. Evaluation of Distributed Pose Estimation using Shortterm UWB and Odometry
1) Impact of sliding window size τ : In the first set of experiments, we evaluated the performance of our pose estimation using UWB ranging and odometry measurements. We fixed the angular searching step size δ to 0.1 and examined the positioning accuracy and average time consumption for one pose estimation under the impact of sliding window size τ , as shown in Table I. This table also lists a comparison of the accuracy between nonlinear least square (NLS) optimization [27], coarse search-based approach, and the combination of coarse search and NLS optimization. As can be seen from Table I, our approach, which combines the coarse search and NLS optimization, provides the best accuracy for all     settings of τ . For example, the combined approach gives a localization accuracy of 0.83m in translation and 6.74°in rotation for τ =100, which lead to improvements of 36.6% and 27.0% when compared with the pure coarse searchbased approach (accuracy of 1.31m in translation and 9.23°i n rotation). We also observed that the localization accuracy improves with the increase of the window size τ , but at a cost of more computational resources.
2) Impact under different angular searching steps δ: We evaluated the performance of relative pose estimation under various angular searching steps δ. We fixed the sliding window size τ =100. The positioning accuracy and the average computational time for one pose estimation are shown in Figure 6. As can be seen from this figure, for all parameter settings, optimization with a coarse initial guess improves the localization accuracy when compared to the pure coarse search-based approach. The localization error is reduced with the decrease of the angular searching step, while the computational time increases at the same time.
3) Impact of different UWB and odometry settings: In this series of experiments, we set the UWB ranging limit (i.e., 20%, 40%, 60%, and 80% of the maximum UWB range reported during the test) to compare the accuracy. We also added Gaussian noise to raw UWB ranging to verify the performance under various noise levels. We fixed the searching step size δ=0.1 and the sliding window size τ =100. The result is shown in Figure 7. As can be seen from this figure, the localization accuracy is reduced with short UWB range. We also observed long UWB range in general helps to improve the localization accuracy due to the large number of UWB observations to support pose estimation. However, longer UWB ranging brings more uncertainty, particularly in non-line-of-sight conditions, which leads to a decrease of the localization accuracy. For all experimental settings, adding ranging noise will obviously lead to a decrease of localization accuracy.
We compared the localization accuracy with respect to various odometry errors. We added Gaussian noises to the position and heading of odometry and show the results in Figure 8. As can be seen from this figure, the accuracy decreases with the increase of odometry noise. However, we believe a good odometry can be provided by the onboard sensors of the robot, in particularly with the assistance of the state-of-the-art visual odometry or LiDAR odometry techniques.

C. Distributed Pose Graph Optimization with PCM Outlier Rejection
In this series of experiments, we examined the localization accuracy with PCM check as well as the performance of the distributed pose graph optimization. We set the angular searching step size δ=0.1. The results are listed in Table II.  As can be seen from this table, PCM filters out the suspicious  loop closures and improves the overall localization accuracy. For a sliding window size τ =10, we obtain a localization accuracy of 1.37m in translation and 9.59°in rotation with significance level ǫ = 0.05, which gives improvements of 69.8% and 80.6% when compared to the case without PCM check (4.54m in translation and 49.34°in rotation). In addition, we observe that the number of inliers decreases with the increase of the significance level ǫ in PCM. A too large ǫ will harm the online operations, particularly during the early stage of trajectory estimation: potential good loop closures are pruned and the pose graph optimization has to be postponed until a reasonable number of loop closures are reached. Meanwhile, a small ǫ is not able to distinguish the erroneous loop closures and results in an increase of the localization error. The distributed pose graph optimization further improves the localization accuracy by the additional incorporation of odometry information. For τ =50 and ǫ=0.5, we obtained a localization accuracy of 0.45m in translation and 4.01°in rotation (the best accuracy achieved), with improvements of 11.8% and 11.3% when compared to the results without pose graph optimization (i.e., 0.51m in translation and 4.52°in rotation). The pose estimated with UWB and odometry, inliers after PCM check, and estimated trajectory using distributed pose graph optimization are visualized in Figure 9.
Table III compared our approach with the conventional UWB SLAM [18], that uses UWB ranging for SLAM in a centralized manner. As can be seen from this table, our approach outperforms the conventional UWB SLAM in localization accuracy. It is important to point out that the conventional UWB SLAM requires initial positions of the robots to be known publicly, while our approach discards this assumption by performing the relative pose estimation using local UWB ranging and odometry measurements.

D. Computation and Communication Efficiency
Finally, we evaluated the computational cost and the communication resources involved in our approach, as listed in Table IV. We choose the window size τ =50 and the angular searching step size δ=0.1. The significance level in PCM is set to be ǫ=0.1. Our approach was tested on an AMD Ryzen 5950X processor with a frequency of 3.4GHz. The computational time was cumulated over all three robots based on one pose update. The pose is expressed in double precision (8 Bytes) in our implementation. We show the total data communicated among all three robots at different updating frequencies (i.e., 10Hz and 1Hz) of DPGO. As can be seen from this table, the total computational time required for the three modules is 81ms, which we believe is fast enough for the online localization and control of mobile robots. The cost of communication is small, which can be easily deployed in most robotics applications. As can be also seen from this table, performing DPGO at a higher frequency costs more communication resources, due to the frequent exchange of the poses among the robots.

V. CONCLUSIONS
We proposed an approach for distributed SLAM using UWB ranging and odometry measurements for a group of robots in unknown environments. The proposed method determines the relative pose between robots based on shortterm UWB ranging and odometry measurements. In addition, we presented a distributed pose graph optimization with pairwise consistency check for trajectory estimation. The proposed approach was verified with three robots in an indoor environment with a size of 10m×12m. The results showed that the proposed distributed ranging SLAM achieved a localization accuracy of 0.45m in translation and 4.01 • in rotation. Our approach provides a solution to the robotics community for the localization of a team of robots without any knowledge about the infrastructure. In future, we would like to generalize our approach to 3D, which requires the coarse search of the initial guess in 3D polar coordinate system. The comparison to the quadratically constrained quadratic programming-based solution [9] will be also included in our future work.