This study proposes the use of a split covariance intersection algorithm (SplitCI) for decentralized multirobot cooperative localization. In the proposed method, each robot maintains a local cubature Kalman filter to estimate its own pose in a predefined coordinate frame. When a robot receives pose information from neighbouring robots, it employs a SplitCI based approach to fuse this received measurement with its local belief. The computational and communicative complexities of the proposed algorithm increase linearly with the number of robots in the multirobot systems (MRS). The proposed method does not require fully connected synchronous communication channels between robots; in fact, it is applicable for MRS with asynchronous and partially connected communication networks. The pose estimation error of the proposed method is bounded. As the proposed method is capable of handling independent and interdependent information of the estimations separately, it does not generate overconfidence state estimations. The performance of the proposed method is compared with several multirobot localization approaches. The simulation and experiment results demonstrate that the proposed algorithm outperforms the singlerobot localization algorithms and achieves approximately the same estimation accuracy as the centralized cooperative localization approach, but with reduced computational and communicative cost.
With the advancement of information and communication technology, robots from different vendors, different domains of operation, and various perception and operational capabilities are unified into a single framework in order to perform a collaborative task. These heterogeneous multirobot systems (MRS) offer greater coverage in exploration and searching tasks, robustness against individual failures, improved productivity through parallel tasking, and implementation of more complex operations than a single robot [
Robots in a heterogeneous MRS may host different exteroceptive and proprioceptive sensory systems resulting in a significant variation in selflocalization across teammates. Interrobot observations and flow of information between teammates can establish a sensor sharing technique such that the localization accuracy of each member improves over the localization approaches that solely depend on the robot’s onboard sensors. This sensor sharing technique is known as cooperative localization and was initially developed to improve the localization accuracy of MRS that are navigating in global positioning system (GPS) denied areas or areas without preinstalled (known) landmarks [
Available cooperative localization algorithms can be classified into three groups: centralized/multicentralized approaches, distributed approaches, and decentralized approaches. Although the centralized/multicentralized approaches generate consistent pose estimate with high accuracy, these approaches entail considerably higher computational and communicative cost. Distributed cooperative localization algorithms attempt to reduce the communication cost. However, the computational cost of most of the distributed algorithms remains as high as the centralized/multicentralized approaches. Decentralized schemes have reduced both the computational and communication cost of multirobot cooperative localization tasks. However, available decentralized cooperative localization (DCL) schemes sometime generate overconfidence pose estimates for robots in an MRS.
Therefore, computing a nonoverconfidence state estimate with bounded error while reducing computation and communication cost is a key requirement for successful implementation of multirobot collaborative missions that rely on cooperative localization. In this paper, we extend our previous work presented in [
In 1994, Kurazume et al. proposed the first cooperative localization algorithm [
Centralized cooperative localization approaches augment each robot’s pose into a single state vector and perform state estimation tasks at a central processing unit [
In the distributed cooperative localization, each robot locally runs a filter to fuse egocentric measurements and propagate the state over time. As a result, there is no requirement for exchanging highfrequency egocentric measurements with teammates or with a central server. This enables the implementation of distributed cooperative localization algorithms using communication channels with limited bandwidth. However, interrobot observations are still fused at a central processor leading to the same computational complexity as the centralized/multicentralized approaches (i.e.,
Decentralized algorithms focus on reducing both computational and communicative complexity. In general, each robot in a DCL team locally runs a filter (estimator) to fuse its egocentric measurements as well as the interrobot observations received from its neighbours with its local belief. Available decentralized algorithms perform the sensor fusion either in a suboptimal manner [
Available cooperative localization algorithms use four types of interrobot observations: relative rangeonly [
To facilitate the mathematical formulation, let
It is assumed that the robots are navigating in a flat, twodimensional (2D) terrain. Each robot in the MRS hosts a communication device to exchange information between teammates; an interoceptive sensory system to measure egomotion (linear and angular velocities); and an exteroceptive sensory system to measure the relative pose of neighbouring robots. Known data correspondence is assumed for the exteroceptive sensory system. Besides these sensory systems and communication modules, some of the robots in the MRS host a sensory system, such as a differential global positioning system (DGPS), to receive absolute positioning information periodically.
Robot navigation in a 2D space is modeled by a general threedegreeoffreedom (3DOF) discretetime kinematic model
Consider the scenario where robot
The objective of the state propagation step is to predict current pose and the associated estimation uncertainties of a given robot using both the robot’s posterior state density and the odometry reading at the previous time step. In order to avoid the cyclic update (cyclic update is the process that uses the same information more than once), each robot maintains two covariance matrices: total covariance and independent covariance. Once the total and independent covariances are known, dependent covariance can be calculated as
This study employs CKF for sensor fusion. CKF is a recently developed suboptimal nonlinear filter which uses the sphericalradial cubature rule to solve the multidimensional integral associated with the Bayesian filter under the Gaussian approximation [
For a system with
The proposed state propagation approach is summarized as follows.
Augment state end odometry reading into single vector:
Compute the corresponding covariance matrix:
Factorize
Generate cubature points
where
Propagate each set of cubature points through nonlinear state propagation function given in (
Predict next state:
Estimate the predictive error covariance:
To calculate independent covariance, construct new block diagonalization covariance matrix as follows:
Factorize
Predict new state using independent covariance:
Estimate the independent predictive error covariance:
The algorithm is initialized with known prior density
At a relative pose measurement event each robot acquires the relative pose of its neighbours. These measurements are in the local coordinate system of the observing robot and are required to be transformed to the reference coordinate system prior to executing the sensor fusion at the neighbouring robot’s local processor. Assume that, at time
Augment the predictive state and relative pose into single vector:
Construct corresponding covariance matrix:
Factorize
Generate set of cubature points
where
Perform coordinate transform for each set of cubature points
Compute global pose of neighbour:
Compute total noise (error) covariance:
Construct a blockdiagonalized matrix using independent predictive covariance and measurement noise covariance:
Factorize
Compute coordinate transformed measurement using independent covariance:
Estimate independent covariance for the pose measurement:
Estimate dependent covariance for the pose measurement:
The algorithm is initialized with a known predictive density of the pose estimation of the observing robot along with the predictive independent covariances. At an interrobotrelativepose measurement event, the observing robot augments its predictive pose and relative pose measurement into a single state vector (line (
When a robot receives a pose measurement from a neighbour, this measurement is fused with the observed robot’s local estimate in order to improve its localization accuracy.
Calculate the predictive dependant covariance:
Compute the weighted predictive covariance
Compute the weighted measurement covariance
Algorithm
It is assumed that some of the robots in the MRS host DGPS sensor in order to measure global position information. This position measurement at time
This study assumes that each agent in the MRS initially knows its pose with respect to a given reference coordinate frame. The recursive state estimation framework of the proposed DCL algorithm is outlined in Algorithm
Sensor fusion architecture of the proposed decentralized multirobot cooperative localization scheme.
Initialize known
Set initial independent covariance:
The proposed DCL algorithm has four main steps.
At each time step, the robot acquires its egomotion sensor reading (odometry). This measurement is fused with the previous time step’s posterior estimate in order to compute the predicted pose and the associated total and independent error covariance matrices as detailed in Algorithm
At an interrobot relative pose measurement event, first, the robot reads its exteroceptive sensors and collects the relative poses of its neighbours. Then, each relative pose measurement is transformed into the reference coordinate frame as outlined in in Algorithm
At a given time step, a robot may receive pose measurements from one (or more) neighbour(s). First, the received pose measurement is fused with the local estimation using the SplitCI measurement update structure that is detailed in Algorithm
The final step of the proposed DCL algorithm is to update the robot’s local pose with the position measurement acquired from an absolute positioning system. When a new position measurement is available, it is evaluated through an ellipsoidal validation gate to identify whether the acquired measurement is a valid measurement or an outlier (line (
The performance of the proposed DCL algorithm was evaluated using a publicly available multirobot localization and mapping dataset [
Simulation parameters.
Symbol  Parameter description  Value 


Number of robots in the team  5 

Total time period of the dataset  1500 s 

Maximum sensing range  10 m 

Size of the navigation arena  15 m × 8 m 
MC  Number of MonteCarlo runs  20 
Sensor characteristics.
Sensor type  Measure  Update rate  Noise 

Odometry  Linear velocity  50 Hz 

Angular velocity  50 Hz 




Relative pose  Relative 
10 Hz  0.1 m 
Relative 
10 Hz  0.1 m  
Relative orientation  10 Hz  1 deg  


DGPS  Global 
10 Hz  0.1 m 
Global 
10 Hz  0.1 m 
Noise parameters for velocities were extracted from [
Figures
Mean estimation error of
Mean estimation error of
The estimation accuracy of the proposed DCL algorithm is compared with the estimation accuracy that were obtained from the following localization schemes:
We performed 20 MonteCarlo simulations per each localization algorithm. Then the RMSE of position and orientation estimation for 20 MonteCarlo simulations were computed. Finally, the time averaged RMSE values and associated standard deviations were calculated to perform the comparison between different localization schemes.
Consider robots without DGPS measuring capabilities (i.e.,
Time averaged RMSE and the associated standard deviation values of
Comparison of estimation error of different cooperative localization algorithms. This result is for robot
Figure
Estimation error comparison between the proposed SplitCI based approach and the centralized cooperative localization approach.
The proposed DCL algorithm was experimentally evaluated on a team of three robots (see Figure
Experimental setup.
Figure
System architecture of the experiment setup to validate the proposed DCL scheme. Note that the mapbased (scanmatchingbased) localization information is available only for platform
In the host computer, odometry readings were used for state propagation and the global pose measurements and the associated noise covariance from neighbours were used to correct the predicted pose. Note that the pose measurements from neighbours were first evaluated through ellipsoidal measurement validation gate in order to detect and discard outliers. Only platform
Figure
Pose estimation comparison of platform
Figure
Comparison of pose estimation uncertainty (standard deviation) for platform
As the pose estimation of the proposed algorithm is decentralized, the computational complexity of the proposed DCL algorithm is increased linearly with the increase of number of neighbouring robots. In other words, the computational complexity of the proposed DCL algorithm is
The proposed DCL algorithm does not require robot to communicate onboard high frequency proprioceptive sensory data with one another or with central processing unit. Only the interrobot measurements are required to exchange between neighbouring robots. These two properties considerably reduce the bandwidth requirement for communication network between robots. In general, communication complexity of the proposed algorithm remains
This study demonstrates the use of SplitCI algorithm and cubature Kalman filter for decentralized cooperative localization. Both the overall computational (processing) and communicative requirements of the proposed DCL algorithm remain
The proposed method can be directly applied to interrobot relative measurement systems that give either full relative pose or relative position of the neighbours. For the interrobot relative measurement system that measures range and bearing of the neighbours, the polartoCartesian conversion can be applied prior to evaluating these measurements on the proposed DCL algorithm. The key limitation of the proposed DCL algorithm is that the proposed method cannot be directly implemented using relative rangeonly or relative bearingonly measurement systems. However, this limitation can be addressed by implementing a hierarchical filtering approach. In this hierarchical filtering approach, each robot runs a tracking filter per neighbour to track neighbours’ relative pose. These tracks are periodically converted into the global coordinate frame and communicated to the corresponding neighbour. Once a robot receives pose measurement from a neighbour, the proposed DCL algorithm can be exploited for sensor fusion. Ongoing work attempts to implement this hierarchical filtering approach and evaluate its performance.
Converting a relative pose measurement to a global pose measurement can be defined as a converting of uncertain information from one Cartesian coordinate frame to another Cartesian coordinate frame.
Assume
Consider a robot team with two robots
Comparison of mean global pose.
True pose  Mean from MonteCarlo simulation with 10000 samples  Mean from cubaturepoints based transformation  


8  8.0127  8.0112 

6  5.9410  5.9375 

0.3491  0.3240  0.3250 
Comparison of estimated covariance matrixes.
3D
The authors declare that they have no competing interests.
The authors would like to thank the Natural Science and Engineering Research Council of Canada (NSERC) and Memorial University of Newfoundland for funding this research project.