Decentralized Cooperative Multi-Robot Localization with EKF
Multi-robot localization has been a critical problem for robots performing complex tasks cooperatively. In this paper, we propose a decentralized approach to localize a group of robots in a large featureless environment. The proposed approach only requires that at least one robot remains stationary as a temporary landmark during a certain period of time. The novelty of our approach is threefold: (1) developing a decentralized scheme that each robot calculates their own state and only stores the latest one to reduce storage and computational cost; (2) developing an efficient localization algorithm through the extended Kalman filter (EKF) that only uses observations of relative pose to estimate the robot positions; (3) developing a scheme has less requirements on landmarks and more robustness against insufficient observations. Various simulations and experiments using five robots equipped with relative pose-measurement sensors are performed to validate the superior performance of our approach.
In the study of autonomous mobile robots, localization is one of the most fundamental problems. Mobile robot localization, as known as position estimation, is the process of determining the pose of a robot relative to a given map of the environment. Compared to a single robot, multi-robot systems are more robust and efficient to perform complex tasks within complex environments, including rescue and disaster management, surveillance and monitoring, and so on. For the autonomous multi-robot systems, the performance of most functionalities heavily relies on the accuracy of robot localization results, where individual localization results easily interfere with each other.
The main multi-robot localization approaches can fall into three categories: (1) external, (2) internal, and (3) cooperative. The external methods help each robot to acquire their position information independently through those external active landmarks such as Global Position Systems (GPS) or Ultra-Wideband (UWB) systems. The localization uncertainty of the external methods can be bounded only when those external signals are reliable and available. The internal methods enable individual robots to localize their positions by utilizing the sensors on each robot such as camera or laser scanner to detect the features of the environment. However, these methods can not work well in those environments without rich feature information. The cooperative methods, also known as cooperative localization (CL), allow a group of communicating robots to estimate their states jointly using relative observations of each other. Those methods are advantageous in their low cost and high scalability, receiving increasing attentions recently, especially for those robot systems working in featureless environments.
However, many technical challenges need to be overcome in using cooperative localization methods. Generally speaking, the estimation accuracy in featureless environments is inferior to that in environments with rich feature information. Besides, as the group of robots becomes larger, the cost of computation and communication grows much higher. In practice, only using relative observations of each other may not be enough to localize multiple robots given various uncertainties and noises.
A number of approaches have been proposed to address above challenges. Dead-reckoning(DR) is one common method which estimates the robot pose from its previous pose information by using proprioceptive sensors, but the estimation errors will be accumulated over a long distance. To improve the accuracy and consistency of estimated robot states, various data fusion algorithms have been developed, such as Markov localization (ML), extended Kalman filter (EKF) localization and Monte Carlo localization (MCL). The main idea of these schemes is to optimally combine the data derived from proprioceptive and exteroceptive sensors and filter out the motion and measurement noises to reduce the uncertainty of robot pose estimation. Usually, the dependency on landmarks limits the applications of those approaches.
Generally, the schemes to calculate the data fusion in CL are either centralized, multi-centralized or decentralized. In a centralized system, the computations for all robot data are processed in one central robot. Central robot provides feedback and required sensory information from all other robots . The major disadvantages of this scheme include the high expense of computation and the high vulnerability of the whole system. In a multi-centralized system, there are more than one robot performing as the central robot to process all the data of entire group. Consequently, this system needs higher computational power. The decentralized system means each robot estimates its own pose respectively through processing their own sensors data and updates it locally. Nonetheless the problem of double-counting or data incest may occur when two uncorrelated robots share information. The decentralized system yield the least computational cost and highest system scalability.
In this paper, we present a decentralized approach for CL, the approach is illustrated as Fig. 1. In this approach, at each time instant only one random robot in the group remains stationary, while the other moving robots estimate their state by fusing the data of odometry and observations of relative observation between two robots. The rearward moving robots utilize the observation with the forward moving robots to improve the accuracy and stability of the estimator. And the order depends on the relative distance. The contributions of this work include:
1) A decentralized scheme for CL is proposed, where each robot estimates their own state that reduces the cost of computation.
2) An EKF Based algorithm is proposed, where each robot only maintains its own pose and corresponding covariance, and the communication between two robots only occurs when one robot detect another at each time instant, yielding a computational complexity of O(N).
3) This approach only requires the odometry data and relative pose measurements so that it can perform in GPS-denied featureless environments. Even though there are insufficient relative pose measurements, the proposed approach can still guarantee a moderate performance.
The rest of this paper is organized as follows. Section II describes the related work. Section III presents the system setup and the localization problem. Section IV gives the details of the proposed algorithm. Section V provides simulation and experiment results and discussions. Section VI concludes the paper and outlines future works.
Ii Related Work
In the problem of multi-robot localization, the study of CL receives considerable attention for a long period, because of its scalability and superior performance. Many approaches on CL have been proposed based on various schemes and algorithms as mentioned above. In this section, we describe several relevant approaches on CL to highlight the novelty of this work.
To estimate the state of robots with high accuracy and consistency, various filter algorithms have been tested for CL. An EKF-based algorithm for heterogeneous outdoor multi-robot localization is described in , where individual robot equipped with encoders and a camera maintains an estimated pose by using sensor data fusion through EKF. An improved EKF on CL is studied in . Additionally, a recent approach that called recursive decentralized localization based on EKF is presented in . The proposed algorithm approximates the inter-robot correlations and performs with asynchronous pairwise communication. Other algorithms such as particle filter (PF)  and maximum a posteriori (MAP) are also extensively studied for CL. The main limitation of these approaches is that in certain case, none of the robots in group has an access to the absolute state information that will reduce the estimation accuracy and consistency. In our approach, the stationary robot at each time can improve the accuracy efficiently by an optimized EKF algorithm.
To reduce the cost of computation, various decentralized schemes are proposed. Luis C et al. present a Covariance Intersection (CI)-based algorithm for reducing the complexity of CL, where each robot locally maintains its state and covariance. Amanda et al. provide a PF-based algorithm that is completely decentralized and a low-cost particle clustering method to reduce the computational complexity . In a decentralized system, it is important to avoid the problem of double-counting or data incest. Masinjila et al. presents a heuristically tuned EKF to address this problem , which proposes an empirical methodology for improving the consistency of EKF estimates by means of artificial inflation of the landmark covariance. This problem is also discussed in  and . Our approach also employs the similar decentralized scheme to reduce the system complexity.
The approach of CL with stationary robots was first studied by Kurazume et al.. The author proposes a method called ¡±Cooperative Positioning System¡±(CPS). Within the CPS, a team of robots are divided into a stationary group, which acts as landmarks, and a mobile group. The roles of groups would reverse at the next time instant until the team reaches the destination. More related issues are discussed in  and . A leader-assistive approach is presented in  and , where a team of robots are divided into two groups, the performance-plus robots act as leader and the rest as child. The leaders have high-resolution estimators and help to localize the child robots. Such an approach is further extended in . The main limitation of these approaches is that too many constrains(such as the order of movement) are required on the robot motion. In our approach, each robot moves with its own path.
Iii System Setup and Problem Statement
Consider a group of N robots navigating in 2D environment, each robot is equipped with proprioceptive and exteroceptive sensors. The state of individual robot at time is denoted as , which contains the information of position and orientation. In this approach, we adopt the velocity motion model with the assumption that a mobile robot can be controlled through a rotational and translational velocity denoted by and respectively. Thus, the control vector is denoted as . It derives from the proprioceptive sensors such as encoder. On the basis of motion model, at time , the state of robot can be presented as following equation:
where is the duration of a time step, is the motion noise in an additive Gaussian with zero mean and covariance matrix .
Additionally, when one robot detects another by exteroceptive sensors at time , the relative measurement , and are relative distance and bearing respectively between two robots. The measurement model for robot measuring robot at time is
Here, and are the actual state of robot and robot . is the additive Gaussian noise with zero mean and covariance matrix .
At each time step , each robot is provided with an estimate of its own state and the corresponding covariance matrix which represents the uncertainty. The available data is odometry of each robot and all relative measurements between each two robots. For centralized scheme, center robot stores and processes all data including the state and measurement of each robot. It can be denoted as:
For our decentralized scheme, each robot only stores its last state, odometry and measurement to estimate current state. Depending on the odometry and motion model, each robot has a predicted state:
When the robot have several relative measurements with robots. The objective of our approach is to find a proper estimator which meets the following equation:
Iv Proposed Approach
The purpose of our approach is to localize a group of mobile robots navigating in featureless environment depending on the relative measurement between two robots. We have the following assumptions: i) Each robot is equipped with proprioceptive and exteroceptive sensors to obtain data of odometry and relative measurement. ii) Each robot is marked with tags in order to be detected. iii) Robot is able to communicate with each other. In our localization algorithm, at any given time, firstly, one robot is chose randomly to remain stationary. Secondly, the moving robot which is closest to the stationary robot estimates its state by localization algorithm with available relative measurement. Sequentially, the rest of moving robots can utilize the measurement with former estimated robot. This process is usually broken into two steps, namely prediction and correction. In the following subsection, we would derive the equation of this process.
Suppose that there are one stationary robot and several moving robots at time . Robot predicts its state mainly by the odometry noted as . Thus, the predicted state of stationary robot is same as that at previous time step :
Moving robots change its state during each time step. Therefore, depending on standard EKF, the predicted state and the corresponding covariance matrix can be derived by the following equation:
where, and is the estimated state and corresponding covariance matrix at time . represents the uncertainty of robot and provides an approximate mapping between the motion noise in control space to the motion noise in state space. is the covariance matrix of the noise in control space. And Jacobian is the derivative of the function with respect to the state vector, is the derivative of the function with respect to control vector.
The estimate is calculated based on the odometry in the prediction step. However, this step is similar to DR that would have a cumulative error over a long distance. Hence, we need fuse the relative measurement to correct the estimate and reduce the uncertainty.
The order of the robots to be corrected with the predicted state depends on the relative distance to the stationary robot . We assume the closest one is robot , its predicted state and corresponding variance matrix at time are and . Hence, we have the prediction of relative measurement between and :
We utilize the difference between the measurement collected from exteroceptive sensors and the predicted measurement calculated in (8) to correct the estimated state. There are three factors that would influence the accuracy of measurement, including the uncertainty of stationary robots, the uncertainty of moving robot and the Gaussian noise in exteroceptive sensors. Hence, we have:
Here, is the Jacobian of with respect to the state vector of moving robot . is the Jacobian of with respect to the state vector of stationary robot. is the covariance matrix of measurement noise. This equation represents the noise mapping into the measurement space.
Then, depending on standard EKF, we can calculate the Kalman gain as following:
Fusing the previous formulas, the estimate and corresponding variance matrix of moving robot can be updated by the following equation:
Here is the identity matrix of the same dimensions as .
So far, we obtain the estimate state and corresponding covariance matrix of moving robot by fusing the odometry and relative measurement with stationary robot at time . Essentially, we have the assumption that all probabilities of measurement between robots are independent that enable us to incrementally add the information from multiple robots into our filter. Therefore, the rest of the robots can utilize the measurement of estimated moving robot like robot to estimate its state efficiently. For the remainder moving robot and estimated robot:
Accordingly, the belief derive from the stationary robot propagates to all the robots that reduces the uncertainty of moving robot and improves the stability.
Iv-C Accuracy and Consistency
The estimated state of robot is represented by the approximation and corresponding covariance matrix. To evaluate the accuracy of this estimator, we calculate the root mean square error(RMSE) between two state vectors. The definition is as following:
and is the actual and estimated state. RMSE indicates how similar the actual and estimated state are over time steps.
V Simulation and Experiment
The performance of our approach was tested in simulation with robots navigating in an area of approximately 10 m10 m for 1000 time steps, each time step has a duration of 0.05 sec. In order to validate the performance, we have the following setting:
Each robot has individual translational velocity m/s and rotational rad/s, the is a normally distributed random number.
The initial pose error(m,m,rad)=, the initial actual position(m,m,rad)= and the measurement error(m,rad)=.
Under the above setting, the position of one robot calculated from our approach, decentralized EKF(De-EKF) and the dead-reckoning(DR) are shown in Fig. 2. We can see that, the DR and De-EKF have similar performance in the beginning. However, as the distance increases, estimator from DR have a cumulative error, while that from De-EKF accompanies the groundtruth constantly.
Then, we compare the performance of the following approaches: i) DR which estimates the state depending on odometry only. ii)L-EKF that one robot estimates its state by four known landmarks. iii)our approach(DE-EKF) that five robots estimates their states mainly utilize the relative measurement. We employ RMSE to indicate the accuracy of these approaches. The result of one robot is depicted in Fig. 3. As expected, the DR has the worst performance, while the L-EKF has the best performance because the known landmark with absolute position can reduce the uncertainty of robot efficiently. Our approach is less accurate than the L-EKF, since the portable landmarks which are stationary robot or estimated robots have their own uncertainty. However, our approach do not need the known landmark. And it’s capable for featureless environment. Additionally, the accuracy of our approach is close to that of L-EKF.
In order to test the stability and robustness of our approach, we simulate the condition with insufficient relative measurement. The performance of our approach with 10%-90% measurement in each time step is compared in Fig. 4. It depicts that with over 50% available measurement, our approach has normal performance as usual. Even though there are 20% available measurement, our approach still has a moderate performance that the RMSE in position is approximately 0.12m.
An experiment with five modified Turtlebot2 is performed to validate the performance of our approach. The hardware and software used in experiment are specified in table I and II. Turtlebot2 is a low-cost personal wheeled robot kit with open-source software. The structure of modified Turtlebot2 is illustrated in Fig. 5. Each Turtlebot2 is equipped with encoder to acquire odometry. The exteroceptive sensors are structure light camera kinect and full HD camera located in front and back of this robot to acquire relative measurement. The actual information of robot position and rotation are provided by motion capture system(OptiTrack) with error less than 0.3mm and respectively. Two features are applied on Turtlebot2. One is the rigid body marker, used to be detected by OptiTrack. Another is Apriltag, used to be detected by camera. The data of each robot is processed in mini PC which is a Inter NUC on our Turtlebot2.
The experiment environment is shown in Fig. 6. Five Turtlebot2 navigate in this environment with the motion strategy as we mentioned previously. The experiment result is shown in Fig. 7 and Fig. 8. We can see the state of five robots calculated by our approach approximates to the groundtruth. The RMSE of position and orientation is less than 0.04m and 0.04 rad respectively.
|Turtlebot2 base||YMR-K01-W1||Provide Odometry|
|Full HD Camera||Microsoft LifeCam 1425||Relative pose detection|
|Kinect||Microsoft Kinect V1||Relative pose detection|
|rigid body marker||-||Unique feature|
|Intel NUC Kit||J57645-309||Data processing|
|Optitrack Camera||Prime 13||Provide ground truth|
|Optitrack Motive||2.0||System driver|
|usb cam||0.3.6||ROS camera driver|
|Openni camera driver||1.11.1||ROS Kinect driver|
|Apriltags2 ROS||0.9.8||Provide relative pose|
|ROS||Kinetic||Robot operating system|
|Mocap Optitrack||2.8.3||Optitrack driver|
Vi Conclusion and Future Work
In this paper, a decentralized approach with improved EKF for cooperative localization is proposed. Compared with other multi-robot localization methods, the proposed approach is advantageous in high accuracy, scalability and robustness of state estimation, as well as low hardware cost and computational complexity. This approach provides a solution for localizing a group of robots in featureless environments. The experiment results show that the proposed approach can yield similar localization accuracy as the landmark-based method does, without using any pre-known landmarks. For five robots, the RMSE of position and orientation is less than 0.04m and 0.04 rad respectively. When the measurements are insufficient, the proposed approach still can yield a moderate localization performance. However, the system performance would decline when the robots move at high speeds. Future work will be focused on addressing this problem through using high sampling rates and faster estimation algorithms.
-  I. J. Cox, “BlancheâAn Experiment in Guidance and Navigation of an Autonomous Robot Vehicle,” IEEE Transactions on Robotics and Automation, vol. 7, no. 2, pp. 193–204, 1991.
-  S. Saeedi, M. Trentini, M. Seto, and H. Li, “Multiple-Robot Simultaneous Localization and Mapping: A Review,” Journal of Field Robotics, vol. 33, no. 1, pp. 3–46, 2016.
-  J. Borenstein and L. Feng, “Umbmark: A method for measuring, comparing, and correcting dead-reckoning errors in mobile robots,” 1994.
-  H. R. Everett, J. Borenstein, and L. Feng, “Sensors for Mobile Robots,” no. November 1983, 1994.
-  R. Masinjila and P. Payeur, “Consistent multirobot localization using heuristically tuned extended Kalman filter,” Proceedings - 2017 IEEE 5th International Symposium on Robotics and Intelligent Sensors, IRIS 2017, vol. 2018-Janua, pp. 297–303, 2018.
-  A. Howard, M. Matark, and G. Sukhatme, “Localization for mobile robot teams using maximum likelihood estimation,” IEEE/RSJ International Conference on Intelligent Robots and System, vol. 1, pp. 434–439, 2002. [Online]. Available: http://ieeexplore.ieee.org/lpdocs/epic03/wrapper.htm?arnumber=1041428
-  E. D. Nerurkar and S. I. Roumeliotis, “Asynchronous multi-centralized cooperative localization,” IEEE/RSJ 2010 International Conference on Intelligent Robots and Systems, IROS 2010 - Conference Proceedings, no. i, pp. 4352–4359, 2010.
-  L. Luft, T. Schubert, S. I. Roumeliotis, and W. Burgard, “Recursive decentralized localization for multi-robot systems with asynchronous pairwise communication,” International Journal of Robotics Research, pp. 1–16, 2018.
-  R. Madhavan, K. Fregene, and L. E. Parker, “Distributed heterogeneous outdoor multi-robot localization,” in Robotics and Automation, 2002. Proceedings. ICRA’02. IEEE International Conference on, vol. 1. IEEE, 2002, pp. 374–381.
-  A. Martinelli, F. Pont, and R. Siegwart, “Multi-robot localization using relative observations,” Proceedings - IEEE International Conference on Robotics and Automation, vol. 2005, no. April, pp. 2797–2802, 2005.
-  A. Howard, “Multi-robot simultaneous localization and mapping using particle filters,” The International Journal of Robotics Research, vol. 25, no. 12, pp. 1243–1256, 2006.
-  E. D. Nerurkar, S. I. Roumeliotis, and A. Martinelli, “Distributed maximum a posteriori estimation for multi-robot cooperative localization,” Proceedings - IEEE International Conference on Robotics and Automation, pp. 1402–1409, 2009.
-  A. Franchi, G. Oriolo, and P. Stegagno, “Mutual localization in multi-robot systems using anonymous relative measurements,” International Journal of Robotics Research, vol. 32, no. 11, pp. 1302–1322, 2013.
-  A. Prorok, A. Bahr, and A. Martinoli, “Low-cost collaborative localization for large-scale multi-robot systems,” in Robotics and Automation (ICRA), 2012 IEEE International Conference on. Ieee, 2012, pp. 4236–4241.
-  L. Luft, T. Schubert, S. I. Roumeliotis, and W. Burgard, “Recursive decentralized collaborative localization for sparsely communicating robots.” in Robotics: Science and Systems, 2016.
-  R. Kurazume, S. Nagata, and S. Hirose, “Cooperative positioning with multiple robots,” in ICRA, vol. 2, 1994, pp. 1250–1257.
-  R. Kurazume, S. Hirose, S. Nagata, N. Sashida, and K. Nakahara-ku, “Study on Cooperative Positioning System,” International Conference on Robotics and Automation, no. April, pp. 1421–1426, 1996.
-  R. Kurazume and S. Hirose, “Study on cooperative positioning system: optimum moving strategies for cps-iii,” in Robotics and Automation, 1998. Proceedings. 1998 IEEE International Conference on, vol. 4. IEEE, 1998, pp. 2896–2903.
-  ——, “An experimental study of a cooperative positioning system,” Autonomous Robots, vol. 8, no. 1, pp. 43–52, 2000.
-  T. R. Wanasinghe, G. K. Mann, and R. G. Gosine, “Distributed collaborative localization for a heterogeneous multi-robot system,” in Electrical and Computer Engineering (CCECE), 2014 IEEE 27th Canadian Conference on. IEEE, 2014, pp. 1–6.
-  ——, “Distributed leader-assistive localization method for a heterogeneous multirobotic system,” IEEE Transactions on Automation Science and Engineering, vol. 12, no. 3, pp. 795–809, 2015.
-  B. E. Nemsick, A. D. Buchan, A. Nagabandi, R. S. Fearing, and A. Zakhor, “Cooperative inchworm localization with a low cost team,” Proceedings - IEEE International Conference on Robotics and Automation, pp. 6323–6330, 2017.
-  S. Thrun, W. Burgard, and D. Fox, Probabilistic robotics. MIT press, 2005.