Cooperative Localization Approach for Multi-Robot Systems Based on State Estimation Error Compensation

In order to improve the localization accuracy of multi-robot systems, a cooperative localization approach with communication delays was proposed in this paper. In the proposed method, the reason for the time delay of the robots’ cooperative localization approach was analyzed first, and then the state equation and measure equation were reconstructed by introducing the communication delays into the states and measurements. Furthermore, the cooperative localization algorithm using the extended Kalman filtering technique based on state estimation error compensation was proposed to reduce the state estimation error of delay filtering. Finally, the simulation and experiment results demonstrated that the proposed algorithm can achieve good performance in location in the presence of communication delay while having reduced computational and communicative cost.


Introduction
In recent years, multi-robot systems have received widespread attention as a team of robots could increase reliability and performance when compared to an individual robot. In addition, localization accuracy can be improved by teamwork through using shared information. At present, multi-robot systems have been applied in many fields such as target tracking [1,2], data collection [3], rescue [4][5][6], and formation [7,8].
Cooperative work is the greatest advantage of multi-robot systems, and precise positioning is the basis for completing cooperative work. Positioning methods are an important research direction of multi-robot systems. The Global Positioning System (GPS)is one way to localize robots in multi-robot systems, however, the GPS signals are not easily received in the room, and the direction information cannot be obtained directly. The cooperative localization (CL) method is an alternative approach where each robot infers its own position according to the position of other robots in a multi-robot system [9]. Since the previous works of Kurazume et al. [10] in 1994, the cooperative localization problem of multi-robot systems has attracted the interest of many researchers in the past few years [11,12].
A cooperative localization method based on the extended Kalman filter (EKF) was proposed in [13], where the position and posture information of each robot measured by the sensor was updated in time, and the entropic criterion was utilized to ensure that the optimal measurement was used to reduce the uncertainty of the robot pose estimation. In [14], to reduce the influence of uncertainty on the accuracy of CL, the upper bound of uncertainty was described as the characteristic function of the robot sensor time and noise. In this case, the CL algorithm ignores the influence of uncertainty in state propagation and relative position measurement, thereby the computational efficiency is improved.
sin 0 Then, the linearized augmented state equation can be described as As previously stated, there are N filtering periods during the entire communication process. Let Z(k) be the measurements that Robot b received at time t 4 , the relative range and absolute position information received from Robot a are used to update the measurements of the systems. Due to the existence of delay, this state vector used for measurement update is actually measured at time t 2 . Therefore, the problem this paper dealt with is, was how to improve the location accuracy and reliability of the systems under the time-invariant delay.

The Augmented State Motion Model with Delay
Consider the augmented state model with time delay. For the convenience of analysis, the leader-follower structure was used for the multi-robot systems in this paper, and we use symbol t s and t k as substitutes for t 2 and t 4 in the following equations.
The states of the systems are denoted as where the subscript l and f denote the leader robot and the follower robot, respectively; x l (s), y l (s) is the state of the leader robot at time t s ; x f (k), y f (k), θ f (k) are the state of the follower robot at time t k . Then, the linearized state equation of the follower robot is described as follows where X f (k) denotes the state of the follower robot in cooperative positioning, and and δt is the period time of sampling; v f (k) and θ f (k) are the linear and rotational velocity of the robot at time t k , respectively. ω f (k) is the system noise due to the errors in the linear and rotational velocity measurements of the follower robot. The system inputs are denoted as Then, the linearized augmented state equation can be described as (7) and the system noise covariance is given by and Φ(k) and Γ(k) are the Jacobian matrices of the state vector and the error vector, respectively.

Measurement Model with Delay
Suppose that the follower robot receives the ranging information from the leader robot. The range measurement model with communication delay can be described shortly as According to the states defined in Equation (1), the states of the leader robot at time t s and the states of the follower robot at time t k are included in the states of the leader-follower robot system. However, the measurement of the system only consists of the self-location of the leader robot and the relative position information at time t s . Consider that the measurement at t k equals the measurement at t s after N filtering periods. Then, the measurement model can be reconstructed as By utilizing the system state transition matrix, we have where the relationship of the one-step prediction state of the follower robot at time t k and the system state at time t s is described in Equation (10). Substituting Equation (10) in Equation (9) yields: Then, the equivalent measurement equation of the system at time t k is deduced from the above formula. The system state transition matrix is given as Then, the measurement equation of the multi-robot systems with time delay can be rewritten as where and the covariance matrix of the measurement noise is

Cooperative Localization with Communication Delays
The system and measurement model with communication delay has been expressed in the previous section. Based on the measurement update, this paper proposed a delayed extended Kalman filter (DEKF) to deal with the problem of cooperative localization with communication delays.
Considering that Z(k) is the equivalent measurement generated by the measurement information sent by the leader robot at time t s after N filtering cycles. In this section, the equivalent measurement at time t k is introduced into the measurement equation, and the optimal estimation of the system is obtained by using the principle of minimum variance estimation of error.
First, the one-step prediction state of the multi-robot system is given as follows: where X l (s + 1, s) T and X f (k + 1, k) T denote the one-step prediction states of the leader robot at time t s and the leader robot at time t k , respectively. Based on the linearized augmented state models (4), the one-step state prediction is given as follows: and its covariance are also given as follows: The estimative state of the system is where K(k) is an arbitrary filter gain, which requires minimum error estimation. The value of K(k) is determined according to the following formula: If the possibility of communication delays is not a consideration, the general KF algorithm can be used to deduce the error prediction as follows: The above formula describes the correspondence between one-step predictive state at time t k and the estimation states among N Kalman filters.
When communication delays are considered, that I,s the measurement is unknown at time t s . Then, the above formula can be rewritten as Then, the error compensation of the one-step predictive state at time t k can be obtained as follows: Note that K in Equation (25) is different to K , which is the filter gain without communication delays. K is the filter gain without considering the system delay condition. K is the filter gain considering the system delay condition, and the difference between K and K is whether the measurement information is added into the system at time t s . At time t s , the measurement matrix and the measurement noise covariance matrix can be estimated, then Equation (25) can be rewritten as where It can be seen from the above formula that the state compensation includes the system state matrix Φ(k − i) and the system state estimation error M(s) of the N filter periods from t s to t k .
Therefore, the error compensation formula of the one-step predictive state at time t k can be written as Finally, the cooperative localization algorithm for multi-robot systems can be summarized as follows: Algorithm 1: The cooperative localization algorithm based on State Estimation Error Compensation 1:Initialize: Assume that each robot in the system initially knows its pose with respect to a given reference coordinate frame. As Figure 1 shows, consider that at time t k , the follower robot receives the pose information from the leader robot with time delay after N Kalman filters at time t s .

2:
State prediction and compensation: Give the one-step state prediction and covariance matrix:

4:
Compute the filter gain:

6:end
With the cooperative localization algorithm proposed above, the extended Kalman filtering based on state estimation error compensation with communication delays can be designed to improve the location accuracy.

Setup
In this section, the performance of the proposed algorithm was evaluated by a group of two robots with a leader-follower structure as shown in Figure 2. The group of robots moved in a rectangular area in an indoor environment. Both robot carried an orientable range finder and wheel encoders for odometry. The range finder was used to compute the measurements aimed at the leader robot. Both the range measurements and the odometric measurements were supposed to be affected by a zero-mean white Gaussian noise.

Results
In the simulations, it was assumed that the mean velocity of the robots was 1 m/s and the system noise and measurement noise covariance matrices used were as follows:  Communication delays are supposed to be time invariant such as 0.1 s. The filtering period was 0.8 s. As shown in Figure 3, the true trajectory for the leader-follower robot team was compared to the trajectory estimated using the proposed cooperative localization algorithm.    Communication delays are supposed to be time invariant such as 0.1 s. The filtering period was 0.8 s. As shown in Figure 3, the true trajectory for the leader-follower robot team was compared to the trajectory estimated using the proposed cooperative localization algorithm. Communication delays are supposed to be time invariant such as 0.1 s. The filtering period was 0.8 s. As shown in Figure 3, the true trajectory for the leader-follower robot team was compared to the trajectory estimated using the proposed cooperative localization algorithm.  Figure 4 shows the local enlarged drawing of true trajectory and estimated trajectory. From the above simulation results, the effectiveness of the proposed method were verified.              In order to verify the computational efficiency of the DEKF filtering algorithm, Figure 8 shows the computation time of each step when the DEKF method and traditional EKF method were used for cooperative positioning.
It can be seen from Figure 8 that the computational efficiency of the DEKF algorithm was better than traditional EKF algorithm even though the time delay was added to the filtering process. In order to verify the computational efficiency of the DEKF filtering algorithm, Figure 8 shows the computation time of each step when the DEKF method and traditional EKF method were used for cooperative positioning.
It can be seen from Figure 8 that the computational efficiency of the DEKF algorithm was better than traditional EKF algorithm even though the time delay was added to the filtering process. With the continuous movement of the multi-robot system, more and more information needs to be computed, but only the nearest state information can be used to locate the robot itself, and all the original information does not need to be saved. Therefore, a limited storage capacity can be maintained by deleting unnecessary storage content.

Conclusions
In this paper, the delayed cooperative localization problem for a leader-follower robot system was considered. An extended Kalman filter approach based on state estimation error compensation was proposed to keep positioning accuracy with communication delays. The simulation and experiment results demonstrated that the estimation accuracy of the proposed method was comparable with the traditional EKF localization method. However, this method must preserve the state of the system during the entire period of the delay. If the delay is long, it may impose high requirements on the system storage hardware. In addition, if there are data loss, it will affect the algorithm. How to reduce the amount of storage and deal with the loss of packets without the loss of precision is the next topic to be studied. With the continuous movement of the multi-robot system, more and more information needs to be computed, but only the nearest state information can be used to locate the robot itself, and all the original information does not need to be saved. Therefore, a limited storage capacity can be maintained by deleting unnecessary storage content.

Conclusions
In this paper, the delayed cooperative localization problem for a leader-follower robot system was considered. An extended Kalman filter approach based on state estimation error compensation was proposed to keep positioning accuracy with communication delays. The simulation and experiment results demonstrated that the estimation accuracy of the proposed method was comparable with the traditional EKF localization method. However, this method must preserve the state of the system during the entire period of the delay. If the delay is long, it may impose high requirements on the system storage hardware. In addition, if there are data loss, it will affect the algorithm. How to reduce