Abstract
This study proposes the use of a split covariance intersection algorithm (Split-CI) 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 Split-CI 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 single-robot localization algorithms and achieves approximately the same estimation accuracy as the centralized cooperative localization approach, but with reduced computational and communicative cost.
1. Introduction
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 [1]. Integration of robots from different domains of operation, such as a robot team with unmanned ground vehicles (UGV) and unmanned aerial vehicles (UAV), enhances team perception while increasing accessibility in a cluttered environment [2].
Robots in a heterogeneous MRS may host different exteroceptive and proprioceptive sensory systems resulting in a significant variation in self-localization 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 [3, 4]. However, it is reported that a minimum of one agent in a cooperative localization team should possess absolute positioning capabilities in order to have a bounded estimation error and uncertainty [5].
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 [6] and propose a scalable DCL approach for heterogeneous MRS which is guarantee for nonoverconfidence pose estimate with the bounded estimation error. The proposed method applies the split covariance intersection (Split-CI) algorithm to accurately track independencies and interdependencies between teammates’ local pose estimates. The recently developed cubature Kalman filter (CKF) is exploited for sensor fusion. Each robot periodically measures the relative pose of its neighbours. These measurements and associated measurement uncertainties are first transformed into a common reference frame and then communicated to corresponding neighbour robots. Once a robot receives pose measurements from a neighbour, it uses the Split-CI algorithm to fuse received measurements with its local belief. The work presented in this paper offers the following key contributions: (i) a novel DCL approach that combines the properties of the Split-CI algorithm and CKF in order to avoid double counting of the information while generating pose estimation with bounded estimation error, (ii) an extension to the general CKF to accurately calculate and maintain both the independent and the dependent covariances, and (iii) a consistent and debiased algorithm to convert information between two Cartesian coordinate systems. The proposed algorithm has the following properties: (i) per-measurement computational and communication cost of the proposed DCL approach is constant, (ii) there is no requirement for a synchronous or fully connected communication network, (iii) the algorithm is robust against the single point of failure, and (iv) the algorithm is capable of generating a nonoverconfidence pose estimate with the bounded estimation error for each member of the MRS.
2. Background
In 1994, Kurazume et al. proposed the first cooperative localization algorithm [3]. Ever since, numerous approaches for cooperative localization have been reported. These implementations can be categorized into three main groups: () centralized/multicentralized cooperative localization, () distributed cooperative localization, and () decentralized cooperative localization.
Centralized cooperative localization approaches augment each robot’s pose into a single state vector and perform state estimation tasks at a central processing unit [3, 4, 8–12]. The computational cost of the centralized algorithms scales to , where is the number of robots in the MRS. Since these implementations require each robot to communicate its high frequency egocentric data and interrobot observations to a central processing unit, communication links should have wider bandwidth. Multicentralized approaches have been reported to improve the robustness of the general centralized approaches against the single point of failure by duplicating the state estimation process on a few or all robots in the team [12, 13]. This causes an increasing per measurement communicative cost linearly with respect to the number of independent processing units. A multicentralized approach that enables cooperative localization for sparsely communicating robot networks requires more communication bandwidth and onboard memory than traditional multicentralized approaches [14].
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 high-frequency 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., ). Work presented in [15] proposed distributed maximum a posteriori estimation for cooperative localization and achieved reduced computational complexity, that is, , and also improved robustness against the single point of failure.
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 ego-centric 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 [6, 16–21] or in an approximate manner [22–25]. Approximate approaches assume that a given robot local pose estimate is independent from the measurements sent by neighbours. This assumption leads to an overconfidence state estimation. Work presented in [17] uses a dependency-tree to track the recent interaction between robots. However, this approach maintains only the recent interdependencies of robot pose estimate; it tends to be overconfidence. An interlaced extended Kalman filter- (EKF-) based suboptimal filtering approach is presented in [18, 19] to avoid the possibility of generating an overconfidence state estimation. This approach requires each robot in the MRS to maintain a bank of EKFs representing the interaction between teammates. Although it produces a nonoverconfidence state estimation, this book-keeping approach is unscalable, as the number of EKF runs on a single robot increases exponentially with the number of robots in the MRS. A suboptimal filtering approach called channel filtering is presented in [16] which requires a communication network without loops. However, a communication network without loops is an unrealistic assumption for the practical implementation of cooperative localization. An extended information filter is used for implementing the DCL [20], in which each robot maintains the history of robot-to-robot relative measurement updates. CI-based approaches are also reported for the DCL [21]. However, the general CI algorithm neglects possible independencies between local estimates. This causes more conservative estimates and may produce an estimation error covariance which is larger than that of the best unfused estimate [26].
Available cooperative localization algorithms use four types of interrobot observations: relative range-only [27, 28], relative bearing-only [29, 30], both relative range and relative bearing [31–33], and full relative pose measurement [5, 6, 21, 24, 25, 34]. Full relative pose measurement-based cooperative localization schemes always generate more accurate pose estimate than range-only, bearing-only, and range-and-bearing measurement systems [35]. The lowest estimation accuracy was found with the bearing-only measurement system [35].
3. Problem Statement
To facilitate the mathematical formulation, let represent the set that contains the unique identification indices of all robots in the MRS. The cardinality of this set, that is, , corresponds to the total number of robots in the MRS. Further, it is assumed that the robots are represented by where the identification indices of the robots range from to . The set and its cardinality may or may not be known to each agent in the MRS.
It is assumed that the robots are navigating in a flat, two-dimensional (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.
3.1. Robot’s Motion Model
Robot navigation in a 2D space is modeled by a general three-degree-of-freedom (3-DOF) discrete-time kinematic modelwhere is the robot’s pose with respect to a given coordinate frame (or global coordinate frame) at discrete time . The nonlinear state propagation function and the sampling time are represented by and , respectively. is the system input and , where . The parameters and are nominal linear velocities in - and -directions, respectively. is the nominal angular velocity. represents the additive white Gaussian noise term with covariance . For nonholonomic robots, terms associated with linear velocities in -direction, that is, and , should be omitted from the system model.
3.2. Interrobot Relative Measurement Model
Consider the scenario where robot measures relative pose of robot . This relative pose measurement is modeled aswith where is the relative pose of robot as measured by ; that is, , where , , and are -position, -position, and the orientation of robot with respect to robot . This pose measurement is on the body-fixed coordinate system of robot . The nonlinear measurement function is represented by . The measurement noise covariance, , is assumed to be an additive white Gaussian noise with covariance . Parameters and represent the distance between two robots and the sensing range of robot , respectively. It is assumed that the communication range of a given robot is greater than or equal to its sensing range. represents the set that contains the unique identification indices of robots which are within the sensing range of robot at the discrete time . The matrix transpose operation is represented by . At a given time step, there may be no robots, one robot, or multiple robots operating within the sensing range of . The maximum number of robots that can operate within the sensing range of is one robot less than the total number of robots in the MRS, that is, . These imply that for . Letrepresent all relative pose measurements acquired by at time step , where . Symbol represents that the distribution is Gaussian with mean (actual measurement) and covariance . The cardinality of the set follows the same property as set ; that is, for .
4. Decentralized Cooperative Localization Algorithm
4.1. State Propagation
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 aswhere , , and are total covariance, independent covariance, and dependent covariance of ’s pose estimation at time step , respectively.
This study employs CKF for sensor fusion. CKF is a recently developed suboptimal nonlinear filter which uses the spherical-radial cubature rule to solve the multidimensional integral associated with the Bayesian filter under the Gaussian approximation [36]. CKF is a Jacobian-free approach that is always guaranteed to have a positive definite covariance matrix and has demonstrated superior performance than the celebrity EKF and the unscented Kalman filter (UKF) [37–39].
For a system with state variables, the third-order spherical-radial cubature rule selects cubature points in order to compute the standard Gaussian integral, as follows:where the square-root factor of the covariance satisfies the equality . The cubature points are given by where represents the th elementary column vector. In this paper, robot pose and odometry vectors are augmented into a single state vector leading to , where is the size of pose vector and is the size of the odometry vector. Further, the general CKF algorithm is extended to accommodate independent covariance calculation capabilities.
The proposed state propagation approach is summarized as follows.
Algorithm 1 (state propagation).
Data. Assume at time posterior density function of robot’s pose estimation , independent covariance matrix , and odometry reading are known.
Result. Calculate the predictive density function of robot’s pose estimation and associated independent covariance matrix .(1)Augment state end odometry reading into single vector:(2)Compute the corresponding covariance matrix: (3)Factorize (4)Generate cubature points : where (5)Propagate each set of cubature points through nonlinear state propagation function given in (1) :(6)Predict next state: (7)Estimate the predictive error covariance: (8)To calculate independent covariance, construct new block diagonalization covariance matrix as follows: (9)Factorize , then generate a new set of cubature points, and propagate this new cubature point set through the nonlinear state propagation function (1) (refer to lines (), (), and () for equations).(10)Predict new state using independent covariance: (11)Estimate the independent predictive error covariance: is size of robot’s pose vector, is size of robot’s odometry vector, is matrix with rows and columns and all entries are zeros, represents , and represents .
The algorithm is initialized with known prior density , independent covariance matrix , and odometry reading at the previous time step (say, time step ). The algorithm predicts the robot pose for the next time step along with associated total and independent covariances. First, the algorithm augments the estimated pose vector with the odometry vector at time (line ()). The associated covariance matrix is then computed by block-diagonalization of the estimation and process covariance matrices (line ()). In the CKF, a set of the cubature points is used to represent the current estimated pose and associated estimation uncertainties (line ()). To generate these cubature points, the square-root factor of the covariance matrix is required. Any matrix decomposition approach that preserves the equality given in (10) can be exploited to compute the square-root factor of the covariance matrix (line ()). The cubature points that represent current state and odometry measurements are evaluated on the nonlinear state propagation function (line ()), which generates the cubature point distribution for a predicted state. The predicted pose (or state) of the robot is the average of the propagated cubature points (line ()). Total predictive covariance is then computed from (14). Once the total predictive covariance is calculated, a new block-diagonalized covariance matrix, that is, , is generated using the independent covariance matrix of time , that is, , and process covariance matrix, that is, (line ()). After computing , its square-root factor is computed (similar to line ()); then a set of cubature points are generated using the new square-root factor (similar to line ()); and, finally, the newly generated cubature points are propagated through the nonlinear state propagation function (similar to line ()) (line ()). These steps are followed by the computation of prediction for independent propagated state (line ()) and the associated covariance matrix (line ()).
4.2. Computing Pose of Neighbours
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 , robot measures the relative pose of robot . This nonlinear coordinate transformation can be modeled aswhere is known as the pose composition operator and is the global pose of on the reference coordinate frame, as measured by . The superscript asterisk “” is used to indicate that the measurement is on the reference coordinate system. Symbol has the same meaning as (2). Since this Cartesian-to-Cartesian transformation is nonlinear, a cubature point-based approach, as summarized in Algorithm 2, is exploited to achieve consistent and unbiased coordinate transformation (see Appendix).
Algorithm 2 (relative to global conversion).
Data. Assume at time the predictive density function of a robot’s (say, ) pose estimation , independent covariance matrix , and relative pose measurement of a neighbour (say, ) are available.
Result. Calculate global pose measurement of , that is, , and associated independent and dependent measurement covariances, i.e., and .(1)Augment the predictive state and relative pose into single vector: (2)Construct corresponding covariance matrix: (3)Factorize (4)Generate set of cubature points : where (5)Perform coordinate transform for each set of cubature points : (6)Compute global pose of neighbour: (7)Compute total noise (error) covariance: (8)Construct a block-diagonalized matrix using independent predictive covariance and measurement noise covariance: (9)Factorize and then generate a new set of cubature points followed by the coordination transformation for each cubature point (refer to lines (), (), and () for equations).(10)Compute coordinate transformed measurement using independent covariance: (11)Estimate independent covariance for the pose measurement: (12)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 inter-robot-relative-pose measurement event, the observing robot augments its predictive pose and relative pose measurement into a single state vector (line ()). The associated covariance matrix is obtained by block-diagonalization of the predictive total covariance () and noise covariance of the relative pose measurement () (line ()). This block-diagonalized covariance matrix is then factorized and exploited for generating a set of cubature points to represent the state vector (lines () and ()). The generated cubature points are evaluated on the nonlinear Cartesian-to-Cartesian coordinate transformation function, that is, (18), in order to compute the coordinate transformed cubature points (line ()). This step is followed by the computation of the observed robot pose in the reference coordinate system (line ()) and associated total noise (error) covariance matrix (line ()). Once the total noise covariance is calculated, the algorithm constructs a new block-diagonalized covariance matrix, , using the predictive independent covariance matrix and the relative pose measurement noise covariance matrix (line ()). After computing , its square-root factor is computed similar to line (); then a set of cubature points are generated using the new square-root factor (similar to line ()); and, finally, newly generated cubature points are transformed from the local coordinate system to the global coordinate system (similar to line ()) (line ()). These steps are followed by the computing of coordinate transformed measurement and associated independent noise covariance matrix (lines () and ()). Finally, the dependent covariance of the coordinate transformed measurement is calculated as the difference between total and independent covariances (line ()).
4.3. Update Local Pose Estimation Using the Pose Sent by Neighbours
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.
Algorithm 3 (state update with the measurement sent by neighbours).
Data. Assume predictive density of robot pose estimation , the associated independent covariance matrix , and pose measurements from a neighbour along with the associated independent and dependent covariances are available.
Result. Calculate posterior density of time , that is, , and the associated independent covariance matrix .(1)Calculate the predictive dependant covariance: (2)Compute the weighted predictive covariance (3)Compute the weighted measurement covariance (4) if measurement gate validated then(5)Compute Kalman gain (6)Update robot pose (7)Update total covariance (8)Update independent covariance (9) else(10)Assign predictive state and covariances into posterior state and covariances(11) end if is identity matrix of and is weighting coefficient and belongs to the interval
Algorithm 3 summarizes the steps involved in this measurement update. The algorithm is initialized by calculating the observed robot’s predictive dependent covariance (line ()). The weighted predictive covariance and weighted measurement covariance are then calculated as given in (31) and (32), respectively (lines () and ()). Coefficient belongs to the interval and can be determined such that the trace or determinant of the updated total covariance is minimized. Detection and elimination of outliers are important for preventing the divergence of state estimates. This requirement is fulfilled by employing an ellipsoidal measurement validating gate [40] (line ()). As the pose measurements from neighbours are in the reference coordinate frame, the measurement model of this sensor fusion becomes linear. Therefore, the linear Kalman filter can be exploited for sensor fusion. In this measurement update, the measurement matrix of the traditional Kalman filter becomes an identity matrix, (), of . Using the multiplicative property of the identity matrix (i.e., , where is ) the Kalman gains, the updated robot pose, and the associated total and independent covariance matrices can be computed from (33), (34), (35), and (36), respectively (lines (), (), (), and ()). For outliers, measurements are discarded and the predictive pose and the associated total and independent covariance matrices are directly assigned to the corresponding posterior quantities (lines () and ()).
4.4. Update Local Pose Estimation Using the Measurement Acquired by the Absolute Positioning System
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 is modeled aswhere is the measurement vector and is the additive white Gaussian noise term with covariance . This measurement is linear and independent from the robot’s pose estimate. Thus, this measurement can be fused with the current state estimation using the general linear Kalman filter measurement update steps followed by This equation computes the updated independent covariance matrix at the event of DGPS measurement update.
4.5. Sensor Fusion Architecture
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 4 and is graphically illustrated in Figure 1.
Algorithm 4 (Split-CI based cooperative localization algorithm). (1)Initialize known and (2)Set initial independent covariance: (3) for do(4)Read ego-motion sensor (5)Propagate state: Algorithm 1(6)if then(7)for do(8)Read (9)Transform relative pose measurement to reference coordinate frame: Algorithm 2(10)Transmit (11)end for(12)end if(13)if pose measurement receives from neighbours then(14)for do(15)Collect from (16)Perform Split-CI-based measurement update: Algorithm 3(17)Enable recursive update (18)end for(19)Set independent covariance to zero: (20)end if(21)if DGPS measurement available then(22)Read (23)if measurement gate validated then(24)Compute , , and as detailed in Section 4.4(25)else(26)Assign predictive quantities to corresponding posterior quantities (27)end if(28)else(29)Assign predictive quantities to corresponding posterior quantities: (41)(30)end if(31) end for is the set containing unique identification indices of robots that communicate global pose measurements to at time .
The proposed DCL algorithm has four main steps.
Step 1 (propagate state (lines ()-() in Algorithm 4)). At each time step, the robot acquires its ego-motion 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 1.
Step 2 (measure neighbours’ pose (lines ()–() in Algorithm 4)). 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 2. Finally, the transformed global pose measurements and the associated independent and dependent covariance matrices are transmitted to the corresponding neighbouring robots.
Step 3 (update with pose measurements sent by neighbours (lines ()–() in Algorithm 4)). 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 Split-CI measurement update structure that is detailed in Algorithm 3. In order to enable the recursion for available pose measurements from multiple neighbours, the updated pose and associated total and independent covariances are assigned back to the corresponding predictive parameter (line ()). The recursion is then continued until all the received pose measurements are considered. Work presented in [41] provides a complete theoretical analysis and simulation-based validation for the consistency of the Split-CI-based filtering. However, the simulation study presented in [21] surfaced that the estimated states using the Split-CI based DCL algorithm sometimes diverge. This may occur because the resulting pose estimation might be correlated partially or fully to subsequent pose measurements received from neighbours. To overcome this issue, the proposed DCL algorithm directly assigns the known-independent covariance component to the correlated component (line ()). In other words, this study set the independent covariance component to zero after interrobot measurement update event, which is not included in the standard Split-CIF algorithm described in [42].
Step 4 (update with absolute position measurement (lines ()–() in Algorithm 4)). 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 ()). If it is a valid measurement then the measurement is fused with the local estimation (line ()). Otherwise, the predictive quantities are directly assigned to the corresponding posterior quantities (line ()). For the time steps where no absolute position measurements are available, the predictive quantities are directly assigned to the corresponding posterior quantities (line ()).
5. Simulation Results
5.1. Setup
The performance of the proposed DCL algorithm was evaluated using a publicly available multirobot localization and mapping dataset [43]. This 2D indoor dataset was generated from five robots (which we refer to as , , , , and ) that navigated in a indoor space. Although this dataset consists of odometry readings, ground truth measurements, and range and bearing measurements to neighbours and landmarks we only used the odometry readings and ground truth measurements of each robot in order to evaluate the proposed DCL algorithm. This simulation study assumed that all five robots would be equipped with lightweight sensory systems to uniquely identify and measure the relative pose of their neighbours. Further, it was assumed that only two members of the robot team (i.e., and ) would be capable of acquiring DGPS measurements periodically. Interrobot measurements and DGPS measurements were synthesized from the ground truth data. Simulation parameters and sensor characteristics related to this simulation setup are summarized in Tables 1 and 2, respectively.
5.2. Results
Figures 2 and 3 illustrate the mean estimation error and the associated 3- error boundaries of the proposed DCL algorithm for 20 Monte-Carlo runs. Figure 2 corresponds to a robot with absolute position measuring capabilities and Figure 3 corresponds to a robot without absolute position measuring capabilities. From these results, it can be seen that the estimation errors of the proposed DCL algorithm are always inside the corresponding 3- error boundaries. This observation verifies that the proposed DCL algorithm is capable of avoiding the cyclic update and generating nonoverconfidence state estimations. Additionally, it is clear that the robots with absolute position measuring capabilities can achieve a more accurate pose estimation than the robots without such capabilities (note that the -axis of Figures 2 and 3 is presented in two different scales). Further, the results confirm that the estimation error of the proposed DCL algorithm is bounded.
(a) position
(b) -position
(c) -orientation
(a) -position
(b) -position
(c) -orientation
5.3. Comparison
The estimation accuracy of the proposed DCL algorithm is compared with the estimation accuracy that were obtained from the following localization schemes:(1)Single-Robot Localization (SL) Method. Each robot continually integrates its odometry readings in order to estimate its pose in a given coordinate frame. This method is also known as dead-reckoning. Robots with DGPS measuring capability fuse their DGPS sensor readings with the local estimate in order to improve pose estimation accuracy.(2)DCL Using Naïve Block-Diagonal (NB) Method. In this method, the pose measurements sent by neighbours are treated as an independent information and are fused directly with the local estimate. In other words, possible correlations between local estimate and pose measurements sent by neighbours are neglected at the sensor fusion step.(3)DCL Using Ellipsoidal Intersection (EI) Algorithm. The EI algorithm always assumes there exist unknown correlations between each robot’s local pose estimations and uses a set of explicit expressions to calculate these unknown correlations, that is, mutual-mean and mutual-covariance. When a robot receives a pose measurement(s) from its neighbour(s) EI algorithm first calculates these unknown correlations. In order to obtain the updated estimation the calculated mutual-mean and mutual-covariance are fused with the robot’s local estimates and the pose measurements received from the robot’s neighbours [44].(4)DCL Using Covariance Intersection (CI) Algorithm. Each robot runs a local estimator to estimate its pose using onboard sensors and the pose measurement from neighbours. When a robot receives pose measurements from its neighbours, the covariance intersection algorithm is used to fuse these pose measurements with the robot’s local estimate [21].(5)Centralized Cooperative Localization (CCL) Approach. The pose of each robot is augmented into a single state vector. The ego-centric measurements of robots and interrobot observations are fused using EKF. This is a centralized approach which can accurately track the correlations between robots’ pose estimations. Therefore, the results of this approach will serve as the benchmark for the performance evaluation of the proposed DCL algorithm.
We performed 20 Monte-Carlo simulations per each localization algorithm. Then the RMSE of position and orientation estimation for 20 Monte-Carlo 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., , , and ). The pose estimations of these robots entirely rely on the odometry readings and the interrobot observations. Therefore, the time averaged RMSE and the associated standard deviation values of the pose estimation of these robots provide insight into the performance of each localization algorithm. The time averaged RMSE and the associated standard deviation values of the localization of using the single-robot localization scheme were found as m in -direction, in -direction, and rad in orientation estimation (the format of the listed estimation errors is mean ± standard deviation). The time averaged RMSE of the localization of using any of the cooperative localization algorithms (NB, EI, CI, Split-CI, and CCL) were less than in both - and -directions and less than rad in orientation estimation. These observations imply that the cooperative localization approaches can significantly improve the accuracy of pose estimation of agents in a MRS.
Time averaged RMSE and the associated standard deviation values of -position, -position, and -orientation estimates of using different cooperative localization schemes are compared in Figure 4. This comparison shows that the centralized cooperative localization algorithm outperforms all the other approaches. This was the expected result, as the centralized estimator maintained the joint-state and the associated dense covariance matrix in order to accurately represent the correlation between teammates pose estimates. Although the estimated pose using the proposed Split-CI based DCL algorithm is less accurate than that of the centralized cooperative localization algorithm, it demonstrates better accuracy over all other DCL approaches that we have evaluated in this paper.
Figure 5 illustrates the estimation error comparison between the proposed Split-CI based DCL algorithm and the centralized cooperative localization algorithm. It indicates that the centralized approach has better accuracy; however, the estimation accuracy obtained from the proposed DCL algorithm is comparable with the estimation accuracy obtained from the centralized approach.
(a) -position
(b) -position
(c) -orientation
6. Experimental Results
6.1. Setup
The proposed DCL algorithm was experimentally evaluated on a team of three robots (see Figure 6): one Seekur Jr. (we will refer to this as platform ) and two Pioneer robots (we will refer to these as platforms and ). Each robot is equipped with wheel encoders for odometry. Additionally, SICK laser scanners were attached to acquire range and bearing measurements for objects around the robot, periodically. Robots were navigated in indoor environment while maintaining triangular formation between them.
6.2. System Architecture
Figure 7 illustrates the system architecture of the experiment setup. Each robot acquires its odometry measurements and laser-scan readings periodically. The acquired measurements are transmitted to a host computer through a TCP/IP interface. Platform was provided with the map of the navigation space and it performed scan-matching-based localization using this map. The position estimations of this scan-matching-based localization for platform were considered as absolute position measurements for cooperative localization schemes and were transmitted to the host computer that executes the localization 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 used scan-matching-based position calculation data at the sensor fusion. At each host processing unit, the received laser-scan data were first converted to the Cartesian coordinate frame from the polar coordinate system. This gives a set of points that represents the relative positions of objects around the corresponding robot. Laser-scan-based feature extraction algorithm was then exploited to detect and measure the relative pose of neighbouring robots. The data correspondence problem was addressed through the nearest neighbour data association technique. These relative pose measurements were then converted to global (reference) coordinate frame and then were communicated to the corresponding host.
6.3. Results
Figure 8 illustrates the comparison of pose estimates for platform that were obtained from three different sensor fusion approaches: the centralized cooperative localization method, the proposed Split-CI based DCL algorithm, and the single-robot localization (dead-reckoning) method. The estimates obtained from the centralized cooperative localization approach serve as the benchmark for evaluating the proposed DCL algorithm. On the other hand, the estimates obtained from the single-robot localization represent the worst case pose estimates for each time step. These results suggest that the proposed Split-CI based DCL algorithm and the centralized cooperative localization algorithm generate approximately the same pose estimates for platform . Although the two estimates are not identical to one another, the difference between the two estimates did not exceed the double-sided 3- error boundary, that is, the gray color region of Figure 8, of the proposed DCL algorithm. Pose estimates generated from dead-reckoning diverged from the true state (or the state obtained from the centralized approach) with the increase of experimental time period.
(a) -position
(b) -position
(c) -orientation
Figure 9 illustrates the comparison of pose uncertainty for three different sensor fusion approaches: the centralized cooperative localization method, the proposed split-CI based DCL algorithm, and single-robot localization method. These results verify that the cooperative localization approaches have bounded pose estimation uncertainty while the pose estimation uncertainty of the single-robot localization approach increases unboundedly. The lowest pose uncertainty is recorded in the centralized approach (see Figure 9(c)). The pose uncertainty found in the proposed Split-CI based DCL algorithm is slightly greater than that of the centralized approach. This is the expected result as the centralized estimator maintained the joint-state and associated dense covariance matrix in order to accurately represent the correlation between teammates’ pose estimates.
(a) -position estimation
(b) -position estimation
(c) -orientation estimation
7. Complexity
7.1. Computational Complexity
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 , where is the number of neighbours, per robot per time step. This remains true for all the DCL algorithms while the computational complexity scales for the centralized cooperative localization where is the number of robots in the MRS.
7.2. Communicative Complexity
The proposed DCL algorithm does not require robot to communicate on-board 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 per robot, per interrobot observation event.
8. Conclusion
This study demonstrates the use of Split-CI algorithm and cubature Kalman filter for decentralized cooperative localization. Both the overall computational (processing) and communicative requirements of the proposed DCL algorithm remain per robot per time step, where is the number of neighbouring robots. This is a considerable reduction compared to state-of-the-art centralized cooperative localization approach in which computational cost scales and requires wider communication bandwidth to exchange high frequency ego-centric measurements between robots and/or central processing unit. Besides the reduced computational and communicative complexity, both the simulation and experiment results demonstrate that the estimation accuracy of the proposed method is comparable with the centralized cooperative localization. Therefore, the proposed DCL algorithm is more suitable for implementing cooperative localization for a MRS with large number of robots. Additionally, the simulation and experimental results verified that the estimation errors of the proposed DCL scheme are bounded and are not overconfidence. This can be attributed to the modification we added at line () in Algorithm 4. The results verified that the cooperative localization approaches outperform the single-robot localization method. Therefore, interrobot observation and flow of information between robots will be the most appropriate approach when implementing localization approach for heterogeneous MRS.
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 polar-to-Cartesian 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 range-only or relative bearing-only 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.
Appendix
Consistent and Debiased Method for Cartesian-to-Cartesian Conversion
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 is a random variable with mean and covariance . Additionally, assume there is another random variable which relates to as follows: where represents nonlinear function. If the objective is to calculate the mean and covariance of , given , , and , the transformed statistics are said to be consistent if the inequalityholds [45]. Work presented in this study applies a cubature-point-based approach to perform Cartesian-to-Cartesian coordinate transformation (see Section 4.2 for more details). Here, we present a simulation study to verify that the Cartesian-to-Cartesian conversion algorithm presented in Algorithm 2 holds above inequality.
Consider a robot team with two robots , and . The global pose of and is and , respectively (the format of the pose vector is , where and coordinates are given in while the orientation is given in rad). The objective is to find the global pose of given the global pose of , relative pose of with respect to , and the associated uncertainties. We compared the statistics we obtained from Algorithm 2 with those calculated by a Monte-Carlo simulation which used 10000 samples. Table 3 and Figure 10 illustrate the comparison of the statistics calculated from two methods. It can be seen that the mean values obtained from the proposed algorithm are approximately overlapped with those calculated by a Monte-Carlo simulation which used 10000 samples. Therefore, the conversion is unbiased. Further, it can be seen that the covariance ellipses of the cubature-point-based approach are always larger than that of the Monte-Carlo simulation. This implies that the proposed Cartesian-to-Cartesian transformation holds the inequality given in (A.2). Additionally, principal axis of covariance ellipse of the proposed approach is approximately overlapped with those of the Monte-Carlo localization. Therefore the proposed coordinate transformation algorithm is consistent.
(a) plane
(b) plane
(c) plane
(d) 3D
Competing Interests
The authors declare that they have no competing interests.
Acknowledgments
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.