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.

1. 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. A sample-based Monte Carlo localization (MCL) algorithm was introduced in [15] to improve the accuracy of the cooperative positioning of multi-robot systems. The MCL algorithm was extended in [16] to the case of two robots, if a map of the environment is available to both robots. However, the drawback of the MCL approach is that it can only be used in known environments.

Vision-based navigation technology is also used in cooperative positioning. In order to deal with uncertainty and external disturbance in the measurement, Kalman filters were also employed in [17,18,19] to estimate the position and orientation, and improve the measurement accuracy and robustness of the system.

In practical application, the communication constraints should be taken into account, the information from different types of sensors in the multi-robot systems need to be fused. However, due to the different working frequency and the different information processing time of these different proprioceptive sensors, it causes many problems such as time-delay, which means that delayed measurements are used when state filtering the sensor measurement outputs. Unexpected positioning errors may increase or even lead to filtering divergence if the influence of the communication delay is ignored. In order to solve this problem, many methods have been proposed, and the commonly used method to solve information delay is to use robust theory to predict and compensate the random delay model. These methods are generally used in complex network systems with more sensors, shorter time delay, and disorderly measurement [20,21,22,23,24,25,26,27,28]. A delayed extended Kalman filter (DEKF) method was proposed to solve the communication delay problem in CL [29,30], The method proposed in [29,30] first augmented all Autonomous Underwater Vehicle(AUV) state quantities to each AUV’s positioning filter, and then the state quantity with time delay was subjected to delay processing for filtering estimation. However, this method has a large amount of calculation and requires a lot of time for the measurement information.

Considering the reasons above, to compensate for the positioning error caused by communication delay in cooperative positioning, an extended Kalman filtering (EKF) based on state estimation error compensation was designed in this paper to deal with the communication delay. In the proposed method, the delayed measurements are predicted in advance to compensate the state estimation error; the time delay is introduced into the reconstructed system state and measurement equation; and then the delayed extended Kalman filter based on measurement update is designed. The proposed cooperative localization algorithm is applied to the leader–follower robot formation to demonstrate the effectiveness of the proposed strategy.

2. Problem Formulation

In this paper, we considered that each individual robot in the group carries sensors to exchange information within the multi-robot systems. As shown in Figure 1, assume at time that Robot b sends a request for communication and ranging to Robot a; after data transmission time , Robot a receives the requests from Robot b at time , and it takes time to process this information. Then, Robot a sends the relative distance and pose estimation to Robot b at time , and Robot b receives the relative measurements from Robot a at time after data transmission time . During the communication between Robot a and Robot b, the time delay we considered in this paper included data transmission time , , and the information processing time . In practical work, the data transmission time and are typically very short, therefore, this paper will pay more attention to the information processing time . We considered the situation that the information processing time caused by communication device was usually fixed, then, the time-invariant delay was considered in this paper.

Figure 1

Timeline of the robot-to-robot communication.

As previously stated, there are filtering periods during the entire communication process. Let be the measurements that Robot b received at time , 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 . 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.

2.1. 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 and as substitutes for and in the following equations.

The states of the systems are denoted as where the subscript and denote the leader robot and the follower robot, respectively; , is the state of the leader robot at time ; , , are the state of the follower robot at time . Then, the linearized state equation of the follower robot is described as follows where denotes the state of the follower robot in cooperative positioning, and and is the period time of sampling; and are the linear and rotational velocity of the robot at time , respectively. 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 where and the system noise covariance is given by and and are the Jacobian matrices of the state vector and the error vector, respectively.

2.2. 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 and the states of the follower robot at time 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 . Consider that the measurement at equals the measurement at after 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 and the system state at time is described in Equation (10). Substituting Equation (10) in Equation (9) yields:

Then, the equivalent measurement equation of the system at time 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

3. 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 is the equivalent measurement generated by the measurement information sent by the leader robot at time after filtering cycles. In this section, the equivalent measurement at time 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 and denote the one-step prediction states of the leader robot at time and the leader robot at time , 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 is an arbitrary filter gain, which requires minimum error estimation. The value of 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 and the estimation states among Kalman filters.

When communication delays are considered, that I,s the measurement is unknown at time . Then, the above formula can be rewritten as

Then, the error compensation of the one-step predictive state at time can be obtained as follows:

Note that in Equation (25) is different to , which is the filter gain without communication delays. is the filter gain without considering the system delay condition. is the filter gain considering the system delay condition, and the difference between and is whether the measurement information is added into the system at time . At time , 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 and the system state estimation error of the filter periods from to .

Therefore, the error compensation formula of the one-step predictive state at time can be written as

Finally, the cooperative localization algorithm for multi-robot systems can be summarized as follows:

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.

4. Simulation Analysis

4.1. 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.

Figure 2

Experimental setup used for testing the proposed algorithm.

4.2. 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.

Figure 3

The true trajectory and the estimated trajectory for the leader–follower robot team.

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.

Figure 4

The local enlarged true trajectory and the estimated trajectory for the leader–follower robot team.

Figure 5, Figure 6 and Figure 7 illustrate the comparative error curves of the follower robot by using the traditional EKF method in the first 100 steps and the proposed method in the next 200 steps, respectively. It can be seen that the estimation errors of the proposed algorithm were bounded in a smaller range when compared with the traditional EKF method.

Figure 5

X-position estimates (the first 100 steps using the extended Kalman filtering method without considering the communication delays, the next 200 steps using the proposed method).

Figure 6

Y-position estimates (the first 100 steps using the extended Kalman filtering method without considering the communication delays, the next 200 steps using the proposed method).

Figure 7

Orientation estimates (the first 100 steps using the EKF method without considering the communication delays, the next 200 steps using the proposed method).

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.

Figure 8

The computation time of each step.

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.

5. 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.