A Method for Active Global Localization in Multi-robot System

Abstract

In multi-robot system the ability to exchange information can reduce the uncertainty in the estimated location when robots can see each other. In this paper, a kind of dynamically evolving coordination architecture is proposed for cooperative localization according to the relative positions between robots. And to further improve the efficiency of cooperative localization, a decision theory based mechanism is proposed to make the robots cooperate actively during the localization process. Since stably tracking the multi-hypothesis of the robots' own position and their partners' position is of great importance for making a good decision of where to go in active localization, the co-evolution based adaptive Monte Carlo localization method in which samples are clustered into species to represents a hypothesis of robot's pose in a higher level than a single sample is adopted. Experiments are designed and carried out to prove the efficiency and stability of the proposed method.

Keywords

cooperative localization active localization Monte Carlo Loclization

1. Introduction

Global localization is a challenging problem, in which robots are required to estimate their pose by local and incomplete observed information under the condition of uncertain initial pose. In recent years, much work has been done for the single robot global localization and several approaches based on probabilistic theory are proposed, including grid-based approaches (Burgard, W., Fox, D., Hennig, D. & Schmidt, T., 1996), topological approaches (Kaelbling, L. P., Cassandra, A. R. & Kurien, J. A., 1999), Monte Carlo localization method(Dellaert, F., Fox, D., Burgard, W. & Thrun, S., 1999)(Thrun, S., Fox, D., Burgard, W. & Dellaert, F., 2001) and multi-hypothesis tracking (Dellaert, F., Fox, D., Burgard, W. & Thrun, S., 1999).

In many robot applications, a multi-robot system has to be used to complete the task cooperatively. As in a single robot system, knowing their relative positions and their global positions in the environment is the precondition for performing tasks and coordination, that is, localization is also of great importance in multi-robot system. A simple way for multi-robot localization is to determine their positions independently. However, fusion of information in multi-robot system can reduce the uncertainty in the estimated location(Fox, D., Burgard, W., Kruppa, H. & Thrun, S., 2000). So more and more researchers have been attracted by the cooperative localization problem, and several methods have been proposed such as Extended Kalman Filtering (Roumeliotis, S. I. & Bekey, G. A., 2002)(Raj, M., Kingsley, F. & Lynne, E. P., 2004) (Hidaka, Y.S., Mourikis, A.I. & Roumeliotis S.I., 2005), Particle Filtering (Fox, D., Burgard, W., Kruppa, H. & Thrun, S., 2000) (Ioannis, M.R., Gregory, D. & Evangelos, M., 2003), Maximum Likelihood estimation (Howard, A., Mataric, M. J. & Sukhatme, G. S., 2002), and Set Membership approaches(Marco, M. D., Garulli, A., Giannitrapani, A. & Vicino, A., 2003). At the same time some special problems for cooperative localization including sensor fusion (John, R. S., 2003) and propagation of uncertainty (Roumeliotis, S.I. & Rekleitis, I.M., 2004) are also discussed.

An area that has received some attention in the single-robot case (Fox, D., Burgard, W. & Thrun, S., 1998) and very little attention in the multi-robot case is the active localization. If a robot can explore actively in the process of localization, it can get more useful information for global localization. For single robot system, a method using entropy to evaluate the utility of the robot's action was proposed (Fox, D., Burgard, W. and Thrun, S., 1998). And Jensfelt et al. proposed an active localization method with a topological map (Jensfelt, P., & Kristensen S., 2001). Also a method using Bayes network for sensor planning in localization was proposed (Hongjun, Z. & Shigeyuki, S., 2002). What's more an approach that actively selects the orientation of the laser range finder to improve the localization results was proposed (Rainer, K., Patrick, P., Rudolph, T. & Wolfram B., 2007).

In this paper, a kind of dynamically evolving coordination architecture is proposed for cooperative localization according to the relative positions between robots. And to further improve the efficiency of the cooperative localization, a decision theory based mechanism is proposed to make the robots cooperate actively during the localization process. Since in active global localization it is very important to make the robot stably track multi-hypothesis of its own position and its partners' positions in order to make a proper decision, CEAMCL (Ronghua, L. & Bingrong, H., 2004) method is used for cooperative localization, in which samples are clustered into species and a co-evolution strategy is applied to prevent the premature convergence of the traditional MCL.

2. Dynamic Architecture of Active Localization in Multi-robot System

A dynamically evolving coordination architecture is proposed for our active localization approach. We assume that robots can determine their relative positions when they see each other. And the relative positions between robots will also be updated according to their motion model for some time when one robot disappear from the eye of the other robot.

At each point of time, the state of the system can be summarized by a graph structure where the nodes are individual robots and the edges represent the relationship between robots (see Fig. 1). An isolated node represents that the robot doesn't known its relative position to that of the other robots and cannot communicate with other robots. In this case the robot, tries to determine its global position by itself as in a single robot system. Another kind of the relationship between robots is that they do not know their relative positions but they can communicate with each other, which is represented by dotted line in Fig.1, such as robot 2 and robot 3. In this case there may be several hypotheses of their relative positions. In order to actively verify their relative positions, the two robots are arranged to meet one another at a rendezvous point. As shown in Fig 1 (b), robot 2 will manage to meet robot 3 so as to determine its position more quickly with the help of the information gotten by robot 3. If the robots fail to meet, the hypothesis will be rejected and they continue to select the other hypothesis for verification. A key substructure of our architecture is the connected group indicated by the shade areas in Fig.1. In the connected group, two robots that know their relative positions and can communicate with each other are connected by a solid line. Each group determines one robot as a leader to be responsible for the coordination of their exploration strategy (robot 4 and robot 9).

Fig. 1.

The dynamic relationship between robots

Since to maintain a single state for all the robots is infeasible, a hierarchical structure is used. In a group, each robot maintains its own belief function that models its own uncertainty in a distributed way, i.e. the information of its position estimated by other robots will be fusion by each robot itself. And several hypotheses of their global positions will be summarized and transmitted to the leader. The leader will estimate the most likely positions that the robots are located. Then several possible actions and their corresponding utilities and costs will be calculated according to the estimated position of each robot. The leader will choose an action for each robot to maximize the utility for the group. The active coordination structure of multi robots is shown in Fig.2.

Fig. 2.

Multi-robot active localization architecture

3. Cooperative Localization in Multi-robot System

Monte Carlo localization (MCL) which is based on sequential Monte Carlo importance sampling (Andrieu, C. & Doucet, A., 2002), can represent non-linear and non-Gaussian models well. So MCL has become the most popular localization method, and many improved versions of MCL have been proposed including mixture-MCL (Thrun, S. et al, 2001), adaptive particle filtering (Fox, D., 2003), clustered particle filtering (Milstein, A. et al, 2002) and CEAMCL (Ronghua, L. & Bingrong, H., 2004). In CEAMCL samples are clustered into species which will evolve according to a co-evolutionary model derived from the competition of ecological species, and the size of the species will adaptively change according to the state of the robot. In this way CEAMCL can not only prevent premature convergence of MCL but also improve its efficiency, so CEAMCL is selected for cooperative localization.

In this section we discuss the application of co-evolution based adaptive Monte Carlo localization in multi-robot system. The co-evolution based adaptive Monte Carlo localization is briefly reviewed in the first sub-section, and cooperative localization of multi robots based on CEAMCL is discussed in the second sub-section.

3.1 Coevolution Based adaptive Monte Carlo Localization

In CEAMCL samples are clustered into groups which are also called species. And the Lotka-Volterra model derived from the competition of ecological species is introduced to make the species evolve cooperatively, so the premature convergence can be prevented. And genetic operators are used for intra-species evolution to search for optimal samples in each species. So the samples can represent the desired posterior density better, and precise localization can be realized with a small sample size.

Let us assume the samples of the r-th robot are clustered into 2 species (clusters) at time t. Inspired by ecology, when competing with other species, the population growth of a species can be modeled using the Lotka-Volterra competition model. The Lotka-Volterra competition model for the 2 species includes two equations of population growth for the two competing species respectively.

\frac{d N_{r}^{(1)}}{d t} = η_{r}^{(1)} N_{r}^{1} (1 - \frac{N_{r}^{(1)} + α_{r}^{(12)} N_{r}^{(2)}}{K_{r}^{(1)}})

(1)

\frac{d N_{r}^{(2)}}{d t} = η_{r}^{(2)} N_{r}^{(2)} (1 - \frac{N_{r}^{(2)} + α_{r}^{(21)} N_{r}^{(1)}}{K_{r}^{(2)}})

(2)

Where $η_{r}^{(1)}$ and $η_{r}^{(2)}$ are the maximum possible rate of population growth, $N_{r}^{(1)}$ and $N_{r}^{(2)}$ are the population size, $K_{r}^{(1)}$ and $K_{r}^{(2)}$ are the upper limit of population size of species 1 and species 2 respectively that the environment resources can support, and $α_{r}^{(12)}$ refers to the impact of an individual of species 2 on the population growth of species 1. Actually, The Lotka-Volterra model of interspecific competition also includes the effects of intra-specific competition on the population of species. When $α_{r}^{(12)}$ or $N_{r}^{(2)}$ equals 0, the population of species 1 will grow according to the logistic growth model which models the intra competition between the individuals of a species.

In CEAMCL the genetic operators, crossover and mutation, are applied to search for optimal samples in each species independently. The intra-species evolution will interact with inter-species competition: the evolution of individuals in a species will increase its ability for inter-species competition, so as to survive for a longer time. Besides the inter-species competition and intra-species evolution, an environment source model is built according to the living domain of each species which represent the uncertainty of localization to adjust the size of species adaptively.

3.2 Cooperative Localization Based on CEAMCL

Multi-robot localization is to integrate measurements taken at different platforms. The simplest way to integrate the information from different platforms is to maintain a single state for all the robots, i.e. if there are R robots, the state of the system will be of 3R dimension. But in this case the state of the system will be too large even for a small number of robots, thus to estimate the distribution of the pose of all the robots is infeasible. So a distributed representation is used in our system similar to the method in(Fox, D., Burgard, W., Kruppa, H. & Thrun, S., 2000), in which each robot maintains its own belief function that models its own uncertainty. The posterior of position is given by:

p (x_{1}^{(t)}, \dots, x_{R}^{(t)} ∣ d^{(t)}) = p (x_{1}^{(t)} ∣ d^{(t)}) \cdot \dots \cdot p (x_{R}^{(t)} ∣ d^{(t)})

(3)

Where R is the number of the robots, d^(t) is the data items collected by all robots up to time t.

The distributed representation ensures that the estimation of the posteriors is conveniently carried out locally on each robot. When the l-th robot knows the position of the r-th robot relative to itself, the relative position $o_{l r}^{(t)}$ between them estimated by the l-th robot and the sample set of robot l are transmitted from the l-th robot to the r-th robot. Robot r will integrate the information from robot l in following way:

\begin{aligned} p (x_{r}^{(t)} ∣ d^{(t)}) = p (x_{r}^{(t)} ∣ d_{r}^{(t)}) p (x_{r}^{(t)} ∣ d_{l}^{(t)}) \\ = p (x_{r}^{(t)} ∣ d_{r}^{(t)}) \int p (x_{r}^{(t)} ∣ x_{l}^{(t)}, o_{l r}^{(t)}) p (x_{l}^{(t)} ∣ d_{l}^{(t)}) d x_{l}^{(t)} \end{aligned}

(4)

Where $d_{r}^{(t)}$ is the data items collected by robot r up to time t. And the same detection can be used to constrain the l-th robot's position based on the belief of the r-th robot because of the symmetry.

During the integration of information, two cases are considered in the calculation of $p (x_{r}^{(t)} ∣ x_{l}^{(t)}, o_{l r}^{(t)})$ : (1) the l-th robot can see the r-th robot; (2) the r-th robot has just gone out the eye of the l-th robot. In the first case, $o_{l r}^{(t)}$ is observed by the l-th robot; $p (x_{r}^{(t)} ∣ x_{l}^{(t)}, o_{l r}^{(t)})$ is the detection model of the l-th robot. In the second case, $o_{l r}^{(t)}$ is calculated according to their odometry data, and the information of their relative positions is used to refine their global positions since the relative positions of the two robots are much more certain than the global positions. In both cases $p (x_{r}^{(t)} ∣ x_{l}^{(t)}, o_{l r}^{(t)})$ is learnt from data.

And to solve Eq. 4, firstly the sample set $S_{r}^{(t)}$ of robot r and sample set $S_{l}^{(t)}$ of robot l at time step t are calculated respectively based on the sample set $S_{r}^{(t - 1)}$ and $S_{l}^{(t - 1)}$ ; and then robot r draws sample set $S_{l r}^{(t)}$ according to $\int p (x_{r}^{(t)} ∣ x_{l}^{(t)}, o_{l r}^{(t)}) p (x_{l}^{(t)} ∣ d_{l}^{(t)}) d x_{l}^{(t)}$ . Notice that equation (4) requires the multiplication of two densities. Since a straightforward correspondence cannot be established between individual sample sampled according to $p (x_{r}^{(t)} ∣ d_{r}^{(t)})$ and sample sampled according to $\int p (x_{r}^{(t)} ∣ x_{l}^{(t)}, o_{l r}^{(t)}) p (x_{l}^{(t)} ∣ d_{l}^{(t)}) d x_{l}^{(t)}$ , the sample set $S_{l r}^{(t)}$ are transformed into density functions ${\hat{p}}_{l r}^{(t)} (x_{r j}^{(t, i)})$ using the density tree (Fox, D., Burgard, W., Kruppa, H. & Thrun, S., 2000). And the algorithm of the cooperative localization is shown in Table 1, $S_{r}^{(t, i)}$ represents the sample set of species i of robot r at time step t and $x_{r j}^{(t, i)}$ represents a sample drawn from $S_{r}^{(t, i)}$ .

Table 1.

Algorithm for cooperative localization

/*Initialization*/

1. cluster samples of each robot r into Ω_r⁽⁰⁾ species, dN_r⁽ⁱ⁾/dt:= 0 and t:=1;

2. for r := 1 to R do

3. for i:=1 to Ω_r^(t) do

4. S_r^(t,i):= φ;

/*Calculate the sample size*/

5. N_r⁽ⁱ⁾ := max(N_r⁽ⁱ⁾ + dN_r⁽ⁱ⁾ / dt, 0)

/* Resampling from species i */

6. Resample N_r⁽ⁱ⁾ samples from S_r^(t-1,i);

/* Importance sampling */

7. for j:=1 to N_r⁽ⁱ⁾ do

/*Predict next state using motion */

8. Sample x_rj^(t,i) from p(x_t | x_rj^(t-1,i), u_t-1);

9. w_rj^{(t, i)} := p(o_r(t) |x_rj^(t,i))

10. S_r^(t,i) = S_r^(t,i) ∪ (x_rj^(t,i), w_rj^{(t, i)})

11. end for /*end of loop for j*/

12. Intra-species evolution of sample set S_r^(t,i);

13. end for /* end of loop for i */

14. if the robot receives the information from the l-th robot then

15. for i:=1 to Ω_r^(t) do

/* Merge the information from the l-th robot */

16. for each sample in set S_r^(t,i) do

17.

w_{r j}^{(t, i)} := w_{r j}^{(t, i)} {\hat{p}}_{l r}^{(t)} (x_{r j}^{(t, i)})

18. end for

19. end for

20. end if

21. for each species normalize importance factors;

22. splitting and merging of species to get Ω_r^(t+1) new species

23. for new species calculate sample size increment dN_r⁽ⁱ⁾ / dt;

24. Output the position hypotheses summarize from the species;

25. end for /* end of loop for r */

26. t=t+1, goto step 2;

4. Decision-theoretic Coordination

The researchers have realized that active exploration strategy is important to improve the efficiency of global localization. But little attention has been paid to the active localization in multi-robot system. In this paper we proposed a strategy based on the decision theoretic for coordination between robots. The strategy includes three steps: in the first step the robots in the same group submit their global position hypotheses to the leader robots who will determine the most likely positions of the robots in the group; in the second step several candidate action are generated for each robot according to the most likely positions, and the pay-offs of the actions are calculated; in the last step the leader will solve the conflictions between robots and choose a next action for each robot so as to get the largest pay-offs for the connected groups.

4.1 Position Estimation of the Connected Group

In multi-robot cooperative localization based on CEAMCL, each species represents a hypothesis of the position of the robot. The summarized hypotheses of positions and the relative positions between robots are transmitted from the robots to their leader. And the position of the leader will be estimated again according to the information from other robots. Suppose the leader of a group is the l-th robot and the r-th robot is a member under its leadership. The probability that the leader will be in the position represented by the hypothesis h_li^(t) according to the data collected by the r-th robot and the relative positions between them can be represented by the following equation:

p (h_{l i}^{(t)} ∣ d_{r}^{(t)}) = \sum_{j = 1}^{Ω_{r}^{(t)}} p (h_{l i}^{(t)} ∣ h_{r j}^{(t)}, o_{r l}^{(t)}) p (h_{r j}^{(t)} ∣ d_{r}^{(t)})

(5)

Where h_li^(t) is a hypothesis of the position summarized by the species i of the l-th robot, h_rj^(t) is a hypothesis of the position summarized by the species j of the r-th robot, Ωr^(t) is the number of species of the r-th robot, o_rl^(t) is the relative position between the l-th robot and the r-th robot, $p (h_{r j}^{(t)} ∣ d_{r}^{(t)})$ which is the average importance factor of the species j of the r-th robot represents the probability that the r-th robot is at the position h_rj^(t), and $p (h_{l i}^{(t)} ∣ d_{r j}^{(t)}, o_{r l}^{(t)})$ is a probability model which can be learnt from the experimental data. The probability that the l-th robot is at the position represented by the hypothesis h_li^(t) according to the data collected by all the robots in the cluster will be:

p (h_{l i}^{(t)} ∣ d^{(t)}) = α \sum_{r = 1}^{R} p (h_{l i}^{(t)} ∣ d_{r}^{(t)})

(6)

Where α is a normalization parameter. The hypothesis h_lmax^(t) with the largest probability is supposed to be the position of the l-th robot. And the hypothesis h_rmax^(t) which has the largest value of $p (h_{l max}^{(t)} ∣ h_{r j}^{(t)}, o_{r l}^{(t)})$ is supposed to be the position of the r-th robot:

h_{r max}^{(t)} = \arg max_{h_{r j}^{(t)}} p (h_{l max}^{(t)} ∣ h_{r j}^{(t)}, o_{r l}^{(t)})

(7)

Then the position hypothesis will be transmitted from the leader to the robots under its leadership. For a robot that is not connected with any other robots, its global position is calculated from the species with the largest average importance factor.

4.2 Exploration Strategy

The world model in our system is represented by a hybrid map of grid and topology. For a grid map a Voronoi Diagram is produced using an approach similar to the one proposed in (Thrun, S., 1998). The grid map is partitioned into disjoint regions using the critical lines, as shown in Fig.3(a), and each region corresponds to one or more topological nodes which is the furcate point of Voronoi Diagram or the middle point if there is no furcate point as shown in Fig. 3(b).

Fig. 3.

Hybrid map of grid and topology a) Disjoint regions of grid map b) topological nodes in Voronoi Diagram

By using the position hypothesis h_lmax^(t), we can get the topological node in the environment around the group. At any point in time the robot in each group can be assigned either to a topological node or to a rendezvous point of another robot. Coordination can be phrased as the problem of finding the assignment that maximizes the utility-cost trade-off. More specifically, let W denote an assignment that determines which robot should move to which target (topological nodes and rendezvous point) then each robot is assigned exactly to one target and W(r, j)=1 if the i-th robot in the connected group is assigned to the j-th target. Among all the assignments we choose the one that maximize the expected utility and minimize the expected cost:

W^{*} = \arg max_{W} \sum_{(i, j) \in W} W (r, j) (U (r, j) - C (r, j))

(8)

The utility and cost of each robot target pair (r, j) can be calculated as follows.

Utility: We suppose the position of the r-th robot is h_rmax^(t) at time point t, and it is assigned to the target j. Then a command a that can drive the robot from the position h_rmax^(t) to the position of the target j can be calculated. The utility of driving the r-th robot to the target with the command a can be computed using the entropy of the belief, obtained by the following formula:

H (x_{r}^{t)}) = \sum_{j = 1}^{N_{r}^{(t)}} p (x_{r j}^{(t)}) \log p (x_{r j}^{(t)})

(9)

The entropy of the belief measures the uncertainty of the robot position. We can measure the utility U(r, j) of performing an action a by the decrease in the uncertainty:

U (r, j) = H (x_{r}^{(t)}) - E_{a} (H (x_{r}^{(t + 1)}))

(10)

If the target is a topological node $E_{a} (H (x_{r}^{(t + 1)}))$ denote the expected entropy of the robot after having performed the action a and fired the sensors at time t+1.

\begin{aligned} E_{a} (H (x_{r}^{(t + 1)})) = \sum_{s} H (x_{r}^{(t + 1)} ∣ s, a) p (s ∣ a) \\ = - \sum_{s, x_{r}^{(t + 1)}} p (s ∣ x_{r}^{(t + 1)}) p (x_{r}^{(t + 1)} ∣ a) \log \frac{p (s ∣ x_{r}^{(t + 1)}) p (x_{r}^{(t + 1)} ∣ a)}{p (s ∣ a)} \end{aligned}

(11)

Where s is the sensor information. If the target is a rendezvous point with another robot not in the group $E_{a} (H (x_{r}^{(t + 1)}))$ is the expected entropy after having performed the action a and detected the other robot at time t+1.

\begin{aligned} E_{a} (H (x_{r}^{(t + 1)})) = \sum_{o_{r k}} H (x_{r}^{(t + 1)} ∣ d^{(t + 1)}, o_{r k}) p (o_{r k} ∣ a) \\ = \sum_{o_{r k}, x_{r}^{(t + 1)}} p (o_{r k} ∣ a) p (x_{r}^{(t + 1)} ∣ d^{(t + 1)}, o_{r k}) \\ \log p (x_{r}^{(t + 1)} ∣ d^{(t + 1)}, o_{r k}) \end{aligned}

(12)

Where $p (o_{r k} ∣ a)$ represents the probability that the relative position between the r-th robot and the k-th robot is o_rk after having perform the action a. The probability $p (x_{r}^{(t + 1)} ∣ d^{(t + 1)}, o_{r k})$ can be calculated by:

\begin{aligned} p (x_{r}^{(t + 1)} ∣ d^{(t + 1)}, o_{r k}) \\ = p (x_{r}^{(t)} ∣ d_{r}^{(t)}) \int p (x_{r}^{(t + 1)} ∣ x_{k}^{(t + 1)}, o_{r k}^{(t + 1)}) \\ p (x_{k}^{(t + 1)} ∣ d_{k}^{(t)}) d x_{k}^{(t + 1)} \end{aligned}

(13)

Cost: using the occupancy grid map we can get the cost-optimal path from the current location of the robot to the target location. And we use the cost of following the optimal path as the cost of assigning the r-th robot to the target j. Let p_occ (x_r^(t+1)) denote the probability that location x^(t+1) is blocked by an obstacle. The robot has to compute the probability that the target point is occupied. Recall that the robot does not know its exact location; thus it must estimate the probability that with the action a the robot will reach an occupied point:

p_{o c c} (a) = \sum_{x_{r}^{(t + 1)}} p (x_{r}^{(t + 1)}) p_{o c c} (f_{a} (x_{r}^{(t + 1)}))

(14)

Where f_a (x^(t+1)) represent the location relative to the robot defined by action a when the robot is at the position x^(t+1). Based on p_acc(a), the expected path length and the cost-optimal policy can be obtained through value iteration. And the cost C(r, j) can be calculated according to the length of the optimal path.

5. Computational Cost Analysis of Cooperative CEAMCL with Active Exploration Strategy

Compared with MCL, CEAMCL requires more computation for each sample. But the sampling process is more efficient in CEAMCL, so it can considerably reduce the number of samples required to represent the posterior density.

The resampling, importance factor normalization and calculating statistic properties have almost the same computational cost per sample for the two algorithms. We denote the total cost of each sample in these calculations as T_r. The importance sampling step involves drawing a n dimensional-vector according to the motion model $p (x_{t} ∣ x_{t - 1}^{(j)}, u_{t - 1})$ and computing the importance factor whose computational costs are denoted as T_s and T_f respectively. The main additional computational cost for cooperative CEAMCL with active exploration strategy arises from the time of intra-species evolution step, the time of the splitting-merging process, the time of fusion information from other robots, and that of the strategy determination. As discussed in our previous paper (Ronghua, L. & Bingrong, H., 2004), the computational cost for each sample in evolution step can be approximated by T_f + T_s. And the splitting or merging probability of species in each time step is small, so the computational cost of the splitting-merging process denoted as T_m is small.

The process of merging the information from the other robot l includes three sub-steps: to calculate the density function ${\hat{p}}_{l r}^{(t)}$ using KD-Tree method whose cost is T_k ⋅ (log₂ N) for each sample in the worst case where N is the sample size and T_k is the time of compare operation, to calculate ${\hat{p}}_{l r}^{(t)} (x_{r j}^{(t, i)})$ for each sample j, whose cost is also T_k ⋅(log₂ N) for each sample in the worst case, and lastly to recalculate the weight of the sample whose cost is a time of multiplication. So the computational cost for each sample in this process can be approximated by T_o = 2T_k ⋅ log₂ N.

The strategy determination includes calculating the utility and cost whose computational cost includes several times of drawing a n dimensional-vector according to a density function such as $p (x_{r}^{(t + 1)} ∣ a)$ and several times of calculating the most likely observation in the next step such as $p (s ∣ x_{r}^{(t + 1)})$ . The time for calculating $p (x_{r}^{(t + 1)} ∣ a)$ and $p (s ∣ x_{r}^{(t + 1)})$ are almost the same as T_s and T_f respectively. So the computational cost for this process can be approximated by s ⋅ T_s + f ⋅ T_f, where s and f are small at most of the time.

The computational costs of other steps in CEAMCL are not related to the sample size, so they can be neglected. Defining N_M and N_C as the number of samples in PF and CEAMCL respectively, the total computational costs for one of the iteration in localization T_M and T_C are given by:

T_{M} = N_{M} (T_{f} + T_{s} + T_{r}),

(15)

T_{C} \approx N_{C} ((2 + f) \cdot T_{f} + (2 + s) T_{s} + T_{r} + T_{m} + T_{o}) .

(16)

The most computationally intensive procedure in localization is the computation of the importance factor which has to deal with the high dimensional sensor data, so T_f is much larger than the other terms. It is safe to draw the following rule:

T_{C} \approx (2 + f) \cdot T_{M} (N_{C} / N_{M}) .

(17)

Since it is not necessary to make a new exploration strategy in every time step, the parameter f can be controlled to be less than 1.

6. Experiment Results

In this section we conduct the experiments with three robots: one is the Pioneer2 equipped with 16 sonar sensors and a CCD camera; the second one is the Pioneer3 equipped with 16 sonar sensors, a front laser range finder and a CCD camera and the third one is the HIT-Ghost which is equipped with two cameras (see Fig.4.). The robots can detect each other using their CCD cameras, and can communicate with each other with the wireless Ethernet. Experiments are carried out in our lab building whose map is shown in Fig. 5(a). In the experiments the 2D hybrid map built by the robot shown in Fig. 5(b) are used to estimate the location of the robot including its position(x, y) and its orientation θ. In order to make HIT-Ghost can localize only with visual information, some color marks are pasted on the doors or on the floor.

Fig. 4.

Picture of robots used for experiment.

Fig. 5.

The map of the environment. a) The map drawn by hand; b) the map built by the robot using laser range finder.

Since even in multi-robot system the robots have to perform localization by themselves when they do not connect with each other, we first evaluate the quality of CEAMCL with active exploration strategy for single robot localization, in which the initial sample size is 1500, crossover probability equals 0.85 and maximum possible rate of population growth of species $η_{r}^{(i)}$ is 0.2. Firstly, the Pioneer3 is placed in one of the rooms and its destination is the door of the building. During the process of going to its destination, the initial position of the robot is unknown and it has to make global localization using the laser range finder (see Fig.6(a)). After running randomly several meters, most of the samples will move to several small areas because of the symmetry of the environment, so CEAMCL can cluster the samples into clusters naturally (see Fig.6(b)). And in the active localization, the robot can determine its action according to the estimated location of the species. At the moment shown in Fig. 6(b), the robot will make a right turning and go to another corner of the rooms to determine the room it will be in as is shown in Fig. 6(c). Then the robot will go to the door of the room and only two most likely hypotheses are remained (see Fig. 6(d)).

Fig. 6.

CEAMCL with active exploration strategy in single robot system

And experiments are carried out to evaluate the localization errors of CEAMCL and A-CEAMCL (CEAMCL with active exploration strategy) for single robot with laser range finder and with front camera only. The average localization errors of 20 times of experiments with 1500 initial samples are shown in Fig.7, in which the labels of CEAMCL-LF and CEAMCL-FC represent localization errors of common CEAMCL method with laser range finder and front camera respectively; and the labels of A-CEAMCL-LF and A-CEAMCL-FC represent localization error of active CEAMCL with laser range finder and front camera respectively. The experimental results show that the error of CEAMCL-FC is much larger than CEAMCL-LF. Since performing localization with camera the robot can only see several color landmarks in the environment, and at the same time A-CEAMCL performs much better than common CEAMCL. Since the robot can get much more useful information with the active exploration strategy.

Fig. 7.

Localization error in single robot system with different methods and sensors

Then we evaluate the multi-robot localization based on CEAMCL with 1500 initial samples for each robot. The three robots are randomly placed in the environment and explore actively by themselves as in single robot localization at the first stage (see Fig.8). In Fig.8 Pioneer3 is represented by red particles, Pioneer2 is represented by green particles and HIT-Ghost is represented by blue particles. When the two Pioneer robots can communicate with each other, a rendezvous point R is determined by them to manage to meet each other (see Fig.8(a)). When they can see each other their observation information will be exchanged, so their global positions can be refined and only two localization hypotheses are remained for each robot (see Fig.8(b)). And Pioneer3 is assigned to be the leader to coordinate their actions to go to an optimal topological node (see Fig.8 (c)). Since the color marks in the environment are sparse and specious, the global position of the HIT-Ghost is very uncertain. When the HIT-Ghost can communicate with the two Pioneer robots, the utility to meet the other two robots is much larger than to go to a topological node, so the HIT-Ghost will manage to meet the Pioneer3 (see Fig.8(d)). The time to determine the global position of the robots using no-cooperative localization, cooperative localization and cooperative active localization based on CEAMCL, which are termed CEAMCL, C-CEAMCL and CA-CEAMCL for short respectively, are compared. In 20 times of experiments, the time needed for the localization of CA-CEAMCL and C-CEAMCL are 62% and 89% of that of CEAMCL respectively.

Fig. 8.

Multi-robot active localization based on CEAMCL

And at last, the localization errors of C-CEAMCL and CA-CEAMCL for multi-robot system are compared as shown in Fig.9. The labels of C-CEAMCL-LF and C-CEAMCL-FC represent the localization errors of the cooperative CEAMCL method for the robot having laser range finder (LF) and only having front camera (FC) respectively. And the labels of CA-CEAMCL-LF and CA-CEAMCL-FC represent the localization error of cooperative CEAMCL method with active exploration strategy for the robot having laser range finder (LF) and only having front camera respectively. From Fig.7 and Fig.9 we can see that the cooperative localization of multi-robot can remarkably reduce the localization error especially for the robots which are equipped with sensors of poor quality such as the front view camera in our experiment, since in cooperative localization the robot can get useful information from the other robots which can perform localization better. We can also see that the active exploration strategy in cooperative localization is useful too, since the robots can cooperate with each much more efficiently by using the active exploration strategy.

Fig. 9.

Localization Error of cooperative licalization in multi-robot system with different methods and sensors

7. Conclusions

A novel method for the active localization of multi-robot is proposed. By using the CEAMCL method, the problem of premature convergence can be solved, so the hypothesis of the robots' positions can be tracked stably. And the decision theory- based coordination strategy can efficiently coordinate the action of the robots so as to maximize the utility-cost trade-off. Experimental results have proved the efficiency of the method of the active localization in multi-robot system.

References

Andrieu

and Doucet

: Particle Filtering for Partially Observed Gaussian State Space Models, J. the Royal Statistical Society Series B (Statistical Methodology), 64(4) (2002), 827–836.

Burgard

Fox

Hennig

and Schmidt

: Estimating the absolute position of a mobile robot using position probability grids, in: Thirteenth National Conference on Artificial Intelligence, AAAI-96, Oregon, USA, 1996, pp. 896–901.

Dellaert

Fox

Burgard

& Thrun

(1999) Monte Carlo localization for mobile robots. in: IEEE International Conference on Robotics and Automation, IROS '99, Michigan, USA, 1999, Vol. 2, pp. 1322–1328.

Fox

(2003). Adapting the sample size in particle filters through KLD-Sampling, Internat. J. Robotic Res. Vol. 22, pp. 985–1004

Fox

Burgard

Kruppa

and Thrun

: A Probabilistic Approach to Collaborative Multi-Robot Localization, Autonomous Robots 8(3) (2000), 325–344.

Fox

Burgard

and Thrun

(1998). Active Markov Localization for Mobile Robots, Robotics and Autonomous Systems, Vol. 25, No. 3–4, pp. 195–207.

Hidaka

Y.S.

Mourikis

A.I.

& Roumeliotis

S.I.

(2005). Optimal formations for cooperative localization of mobile robots. In: Proc. of the IEEE International Conference on Robotics and Automation, pp. 4137–4142, IEEE Press, Barcelona, Spain

Hongjun

& Shigeyuki

(2002). Sensor planning for mobile robot localization using Bayesian network inference. Advanced Robotics, Vol. 16, No. 8, pp. 751–771

Howard

Mataric

M. J.

& Sukhatme

G. S.

(2002). Localization for mobile robot teams using maximum likelihood estimation. In Proc. of the IEEE/RSJ International Conference on Intelligent Robots and Systems, pp. 434–59, Lauzanne, Switzerland

10.

Ioannis

M.R.

Gregory

& Evangelos

(2003). Probabilistic cooperative localization and mapping in practice. In: Proc. of the IEEE International Conference in Robotics and Automation, pp.1907–1912, Taipei, Taiwan

11.

Jensfelt

, and Kristensen

(2001). Active global localization for a mobile robot using multiple hypothesis tracking, IEEE Trans. Robotics Automation, Vol.17, pp. 748–760.

12.

John

R. S.

(2003). Sensor fusion techniques for cooperative localization. PHD thesis, University of Pennsylvania

13.

Kaelbling

L. P.

Cassandra

A. R.

Kurien

J. A.

: Acting under uncertainty: Discrete Bayesian models for mobile-robot navigation, in: IEEE/RSJ International Conference on Intelligent Robots and Systems, IROS '96, Osaka, Japan, 1996, Vol. 2, pp. 963–972.

14.

Marco

M. D.

Garulli

Giannitrapani

& Vicino

(2003). Simultaneous localization and map building for a team of cooperatingrobots: a set membership approach. IEEE Transactions of Robotics and Automation, Vol. 19, No. 2, pp. 238–248

15.

Milstein

Sanchez

J. N.

and Wiamson

(2002). Robust global localization using clustered particle filtering. In: Eighteenth National Conference on Artificial Intelligence, AAAI-2002, pp. 581–586

Edmonton, Canada

16.

Rainer

Patrick

Rudolph

& Wolfram

(2007). Monte Carlo localization in outdoor terrains using multi-level surface maps. In: Proc. of the International Conference on Field and Service Robotics, pp. Chamonix, France

17.

Raj

Kingsley

& Lynne

E. P.

(2004). Distributed cooperative outdoor multirobot localization and mapping. Autonomous Robots, Vol. 17, No. 1, pp. 23–39

18.

Robert

L. D.

and Arvin

: Simulation and control of distributed robot search teams, Computers and Electrical Engineering 29(5) 2003, 625–642.

19.

Ronghua

& Bingrong

(2004). Coevolution Based Adaptive Monte Carlo Localization (CEAMCL), International Journal of Advanced Robotic Systems, Vol. 1, No. 3, pp. 183–190

20.

Roumeliotis

S. I.

& Bekey

G. A.

(2002). Distributed multirobot localization. IEEE Transactions on Robotics and Automation, Vol. 18, No. 5, pp. 781–795

21.

Roumeliotis

S.I.

& Rekleitis

I.M.

(2004). Propagation of Uncertainty in Cooperative Multirobot Localization: Analysis and Experimental Results. Autonomous Robots, Vol. 17, No. 1, pp. 41–54

22.

Thrun

(1998). Learning metric-topological maps for indoor mobile robot navigation, Artificial Intelligence, Vol, 99, No. 1, pp. 21–71.

23.

Thrun

Fox

Burgard

& Dellaert

(2001). Robust Monte Carlo localization for mobile robots. Artificial Intelligence, Vol. 128, pp. 99–14