Navigation Strategies for Cooperative Localization

Navigation Strategies for Cooperative Localization based on a Particle-Filter Approach Fernando Gomez Bravo

∗

Departamento de Ingeniería Electrónica, Sistemas Informáticos y Automática Escuela Politécnica Superior, Univ. de Huelva P-21819 La Rábida, Huelva, Spain [email protected]

Alberto Vale Albatroz Engenharia S.A. – Pólo Tecnológico de Lisboa, CID Estrada do Paço do Lumiar – P-1600-546 Lisboa, Portugal [email protected]

Maria Isabel Ribeiro Institute for Systems and Robotics, Instituto Superior Técnico Av. Rovisco Pais 1, 1049-001 Lisboa, Portugal [email protected]

Abstract This paper proposes a set of navigation strategies to cooperatively localize a team of robots by applying a Particle-Filter approach. The main mission consists in exploring unknown, isolated and unstructured scenarios, where no Global Positioning System is available. By means of the cooperative localization technique and a behaviour based controller the robots have to return to the initial configuration. The proposed approach relies on distance and orientation measurements between the individual robots, and between the robots and a fixed active beacon. Simulation results are presented.

∗ Corresponding

author: phone: +34 959 21 76 38; fax: +34 959 21 73 04;

1

1

Introduction

Autonomous mobile robotic systems have proven very useful in extending the human capability to explore unknown and remote scenarios. In fact, from the current scientific point of view, the presence of robots in inaccessible scenarios to accomplish very different types of mission is one of the most challenging of problems. Although individual robots may have limited exploring capabilities, a team of robots working cooperatively, however, can handle more complex tasks. The use of multi-robot systems, when compared to a single robot, has evident advantages in many applications, in particular when the exploration of a large area is required. Recent research has met with considerable success in developing cooperation in groups of robots [19, 12, 20, 15, 13]. At this point, the mobile robot localization techniques are derived from the need to accomplish certain types of predetermined tasks demanding a high level of precision. Typically, in remote and inaccessible scenarios no map and no global positioning systems are available. From this perspective, cooperation between robots may be essential for their localization, and, consequently, the robots could work together to perform the estimation of their position [23]. Cooperative localization is a key component of cooperative navigation for a team of robots, it explores the decentralized perception that the team supports to enhance the localization of each robot. These techniques have been studied recently [28, 15, 24, 1]. An example on the scientific agenda of such missions, is the exploration of planet surfaces. At the present technological stage, the difficulty of human exploratory missions justifies the development of teams of robots that can carry out, autonomously or semi-autonomously, a set of exploration tasks. Surface exploration may benefit greatly from having a team of cooperative robots rather than having a single robot or a set of isolated robots. In either case, and depending on the particular mission allocated (following a predefined path and returning to the initial configuration, for instance), the robots may have to be localized relative to a fixed beacon or landmark, most probably the launcher vehicle that carried them from Earth. This paper pays special attention to this type of mission and proposes several navigation strategies for improving the cooperative localization of a team of robots based on a Particle-Filter (PF) approach [26, 11]. The cooperative localization relies on distance and orientation measurements between the robots and between these and a fixed beacon. It is assumed that the fixed beacon has sensorial capabilities of range and orientation measurement, but has a maximum detectable range. Each robot has limited range detection along a limited field of view. For localization purposes, the proposed approach propagates a PF for each robot. The particle weight update is based on the measurements (distance and orientation) that each robot acquires relative to the other robots and/or the fixed beacon. The proposed strategies are directed towards an exploration mission and it is assumed that all the robots start

2

the mission close to the fixed beacon, evolve in the environment (the team of robots could be carried to the target scenario by a single platform which could be used as a reference beacon) and, due to the sensorial limitations of the robots, may eventually loose detection of the fixed beacon and the other robots. Moreover, besides cooperative localization, each single robot presents a behavior based navigation controller that allows the robots to avoid collision and follow predefined paths. Hence, the major contribution of this paper is the development of several navigation and motion strategies for improving the cooperative localization based on the use of a PF approach. These strategies allow the robots to follow a reference path in unstructured scenarios where no map and no global positioning systems are available and human intervention is not possible. Finally, a number of simulations have been performed, where the proposed approaches have been implemented with fleets of two and three car-like vehicles. This paper is organized as follows. Section 1 presents the reason for this work, Section 2 is devoted to describing the probabilistic localization techniques. Section 3 introduces the main vehicle characteristics, the notation and the principles of Particle-Filtering. Section 4 explains the main contributions of the paper, namely the cooperative localization and motion strategy using PF in a team of robots. Simulation results obtained from robots working in an environment with obstacles are presented in Section 5. Section 6 concludes the paper and presents directions for further work.

2

Probabilistic Localization Approaches

The localization problem has to deal with the uncertainty in the motion and the observations. Common techniques of mobile robot localization are based on probabilistic approaches that are robust relative to sensor limitations, sensor noise and environment dynamics. In [26], the author identifies the global localization problem as particularly challenging. Using a probabilistic approach, the localization turns into an estimation problem with the evaluation and propagation of a probability density function (pdf), see [4]. Within the context of mobile robot localization, this type of approach is often referred to as Markov Localization (ML) or Hidden Markov Models (HMM), see [5]. There are several approaches to the probabilistic localization problem, one of which is the Particle Filter approach, a technique which is based on the Monte-Carlo simulation method. The main objective of particle filtering is to track the state variables of a system that typically present non-Gaussian and potentially multi-modal probability distribution functions. For this purpose, multiple copies (particles, i p) of the state are used. The set of all i p is known as the particle cloud. The evolution (propagation) of the values associated with the particles is based on the model of the system (typically, the kinematic model is used for robot localization). Thus, all the particles are propagated by applying the input signals to the model and considering its uncertainty. However, given the associated uncertainty, each 3

particle follows a different path. Consequently, some particles are closer to the real state than others, and so the particles are weighted in order to determine the importance of each one. If zt represents the set of observations at the time t, the particle’s weight must be determined according to the following relation:

i

w(t) ∼ P (i p|zt )

(1)

i.e. the weight must be equivalent to the particle’s probability of representing the real values of the system, based on the actual measurements and considering the uncertainty associated with the observations. Another relevant issue of the method is the re-sampling procedure, where the less probable particles are removed from the cloud, bounding the uncertainty of the system, [11]. This procedure is accomplished in accordance with their weights. It addresses two issues: the re-sampling method and the appropriate time instant to re-sample. The author of [14] suggests the application of the Effective Sample Size (ESSt ), obtained from the number of particles, M , and the coefficient of variation cvt2 ,

cvt2 =

M var(wt (i)) 1 X = (M w(i) − 1)2 E 2 (wt (i)) M i=1

(2)

Thus the ESSt is calculated as:

ESSt =

M 1 + cvt2

(3)

When the ESSt is greater than a threshold value, the particles are resampled. Finally, the value of the estimated state, pˆ, is obtained from the average of the particle cloud based on their associated believes. In [11] different methods for computing the estimated state are presented. In the present approach the pose of the robots is estimated according to the following expression

pˆ =

X

i

p · i w, i = 1 . . . n / i w > wmin

(4)

i

where wmin is a value that determines the minimum weight a particle must have in order to be taken into account in the pose estimation. 4

A more detailed description of the P.F. approach is presented in [11].

3

Single Robot Localization and Navigation

This section describes the robots used in the work and presents the basis for the PF localization approach and the navigation strategies for a single robot in the particular application described in Section 1.

3.1

Robot Characterization

It is assumed that each robot is equipped with a 2D laser scanner with a maximum range capability, ρmax , over a limited rear and front angular field of view of width 2ϕmax , as represented in Fig. 1-right. Consequently, the robots are able to measure the distance, ρ, and the direction, ϕ, to obstacles in their close vicinity and are prepared to avoid obstacles. This sensor supports the robot perception needed to evaluate the distance and the orientation by which the beacons (the fixed beacon and/or the other robots in the team) are detected. It is also assumed that each robot is able to recognize the other robots and the fixed beacon by means of the same laser scanner or using, for instance, a vision system. Manoeuvring car/cart-like vehicles is a difficult task when compared with other types of vehicle, for instance mobile robots with a differential kinematic system. Due to this fact, car/cart-like robots have been considered so as to emphasize the capabilities of the proposed localization technique when applied to these types of vehicle. The kinematic model of a car/cart-like vehicle, presented in Fig. 1, is expressed by 







   cos(θ) 0   x˙  v(t)       y˙  =  sin(θ) 0  ·        tan φ(t)     v(t) l 0 1 θ˙

(5)

where (x, y) is the position, θ is the vehicle’s heading, both relative to a global referential, v(t) is the linear velocity and φ(t) is the steering angle that defines the curvature of the path and l is the distance between the rear and front wheels (see Fig. 1-left). The proposed navigation strategy is optimized for car/cart-like vehicles (as described in Section 4), but the approach can be implemented in robots with similar capabilities of motion and perception.

5

3.2

Localization Based on Particle-Filtering

The pose of a single robot j is estimated by appliying a PF, [27]. Each particle i in the filter represents a possible pose of the robot, i.e., i

pj =

i


xj , i yj , i θj .

(6)

All the particles are propagated by means of a dead reckoning technique and considering the uncertainty associated whith the odometry measurements. Thus, the evolution of the particle i is obtained by applying to the kinematic model the inputs:

i

vj (t) = vj (t) + ξv

(7)

i

(8)

φ(t) = φj (t) + ξφ

where vj (t) and φj (t) are the theoretical inputs applied to the robot j and ξv and ξφ are variables representing the odometry uncertainty (in the experiments presented in this paper uncertainties are modeled as Gaussian, zero mean, random variables, but, in general, this approach can be applied to other probabilistic models). The particles are classified according to the measurement obtained by the robot at its real pose. Simulations of the measurements are performed, considering each particle as the actual pose of the robot and taking into account the probabilistic model of the sensor. The likelihood of each particle being the real pose of the robot is evaluated by considering the difference between the real and the simulated measurements. In contrast to other approaches, where a set of fixed beacons are distributed for localization purposes, [17], this paper considers the existence of a single fixed beacon L at (xL , yL ), as represented in Fig. 2- a), that provides a fixed reference each time the robot observes it. The fixed beacon has sensorial capabilities similar to those of the robots, with limited range detection, although in this case over a 360◦ field of view (see Fig. 2-a). In this way, the particles associated with the robot are weighted taking into account the observations made by the robot and by the fixed beacon. Let ρLB and ϕLB be the distance and the relative direction of the robot B obtained from the fixed beacon measurement (Fig. 2-a). Similarly, ρBL and ϕBL are the distance and the relative orientation of the beacon obtained from the robot measurement (Fig. 2-b). Additionally, each particle associated with the robot B defines i ρLB and i ϕLB (which represents the fixed beacon measurement if the robot was placed over the particle i):

6

i i

ϕLB

p

(xL − i xB )2 + (yL − i yB )2 + ξρL i  ρLBy = arctan i + ξϕL ρLBx

ρLB =

(9) (10)

where ξρL and ξϕL represent the range and angular uncertainties of the laser sensor of the fixed beacon. The simulated measurements for each particle define two values, i PρL and i PϕL , related to the acquired sensorial data as,

i i

PρL

PϕL

−1 = κρL · ρLB − i ρLB −1 = κϕL · ϕLB − i ϕLB

(11) (12)

where κρL and κϕL are coefficients that allow comparison of both distances. Thus, a weight that determines the quality of each particle according to the beacon measurement is given by

i

wLB =

   ηLB · i PρL · i PϕL 1

 

if

VLB = 1

if

VLB = 0

(13)

where ηLB is a normalization factor and VLB is a logic variable whose value is 1 if L observes B, and 0 otherwise. On the other hand, and considering the uncertainty model of the robot’s laser (ξρB and ξϕB ), each particle also defines i ρBL , i ϕBL (which represents the robot measurement of the distance and angle with which the fixed beacon will be detected if the robot were placed over the particle i, i.e., in i B, see Fig. 2-b) and VBL (a logic variable whose value is 1 if B observes L and 0 otherwise). The weight that determines the quality of each particle according to the measurement of the robot is similar to (13), with the indices “B” and “L” switched. Therefore, the total weight of the particle i associated with robot B, i wB , is given by

i

wB = i λB · i wLB · i wBL

(14)

where i λB is a normalization factor. Given the formulation of the weights, if the vehicle is not able to observe the fixed beacon and the fixed beacon is not able to detect the robot, or equivalently VLB = 0 and VBL = 0, all the particles have the same importance. As will be shown in Section 5, when no observations are available, re-sampling is not possible and therefore the localization uncertainty increases. Once the mobile robot again observes the fixed

7

beacon or vice-versa, re-sampling is possible and the uncertainty decreases.

3.3

Navigation Strategies

Due to the limited range of perception characterised by the sensors, the robot may navigate in areas where the perception of the fixed beacon is not available. To cope with this constraint the navigation strategy should assure that the vehicle will return to the initial configuration, i.e., the initial position where the vehicle and the beacon are able to detect each other. Different strategies can be applied to drive the robot to the initial configuration. One strategy is based on placing a set of way points along a closed path. However, the environment is still ambiguous and some way points might be over an obstacle. When this undesirable situation occurs the navigation algorithm becomes locked on an unreachable point leading to circular paths around the obstacle. To overcome this issue, another strategy is adopted: the building of a closed continuous path that the robot has to follow with obstacle avoidance capability. Then the navigation strategy is based on two reactive behaviors: path following and collision avoidance. Both behaviors are combined so that the robot motion is biased to desirable directions avoiding unreachable configurations.

3.3.1

Path Following Behaviour

Different approaches have been proposed for path following, where the robot position is estimated by Extended Kalman Filter (EKF) using Global Positioning System (GPS) and odometry [18]. In the present paper no GPS is available, and, rather than using EKF, a PF approach is used. To accomplish accurate navigation, each robot follows a previously defined path and a modified version of the “Pure-pursuit” algorithm [2] is applied for path-tracking. By this algorithm, a point of the path is selected at each time instant so that the control inputs of the vehicle are determined for the purpose of making the robot navigate toward this point. The steering angle of the path-following behaviour is obtained according to the expression:

φpf = arctan(l ·

24 ) L2H

(15)

where l is the distance between the front and the real wheels (defined in Fig. 1) and LH is a parameter called the look-ahead, which represents the distance between the target point and the robot’s current position, and 4 is the lateral distance between the robot and the target point (see Fig. 3-left). With this control law the vehicle will follow a circular arc which contains the target point and the position of the robot [2]. Usually the point of the path is selected by the following procedure: a) firstly, the nearest point, (xn , yn ), to the vehicle is obtained; b) secondly, the target point (xt , yt ) is chosen as the one which is a constant distance s, from (xn , yn ) in the direction of the 8

desired robot motion, (see Fig. 3-right). Other criteria for selecting the target point according to the velocity and the shape of the path can be found in [3] In this way at each time instant, from an estimated robot configuration, a new point of the path is selected and the vehicle navigates following the desired direction without being trapped by any unreachable way point.

3.3.2

Collision Avoidance Behaviour

A mobile robot that navigates in an environment where no map is available, has to deal with several obstacles along the path. To this end, the obstacle avoidance behavior is applied when the vehicle is near an obstacle. In order to describe the obstacles in a complex environment, the general perception vector approach is applied [9]. This technique was introduced in [21] and is based on reducing the environment measurements to a vector, which can be calculated with low computational cost. From the measurements of the 2D laser scanner, a set of perception vectors di are defined (see Fig. 4-left) whose orientations are the directions of the measurement of the laser, and whose lengths are defined as:

|di | = ρmax − ρi

(16)

where ρmax is the maximum range capability and ρi is the measured distance at the direction i Thus the general perception vector D is defined as: P di D = |ρi |min · Pi | i di |

(17)

where |ρi |min is the minimum value of the distances measured by the laser. This vector provides a general perception of the surroundings from the measurement of the laser. It is characterized by the orientation ϕD and the module |D| (see Fig. 4-rigt). The value of ϕD will be applied for calculating the reactive navigation whereas |D| will be applied to combine the navigation behaviours. Thus, reactive navigation is implemented by means of a control law which provides the steering angle as a function of ϕD :

φr = fr (ϕD ) · φmax

(18)

where φmax is the maximum steering angle and fr is a function which represents the repulsive behaviour of the collision avoidance navigation and takes values over [-1, 1]. There are several approaches for developing this strategy. In [5] the following odd function is proposed for a differential drive robot:

9

2

fr (ϕP ) = k1 ϕD · e−k2 ϕD

(19)

The reactive behaviour can also be implemented by means of a fuzzy system which uses linguistic terms for the description of ϕD . In [9] a fuzzy control system for the reactive driving of cart-like vehicles (also controlling the vehicle velocity) is presented.

3.3.3

Combining Behaviours

The path-tracking and the obstacle avoidance behaviours can be combined in such a way that the robot is able to avoid the obstacle and continue the path-tracking once it has been circumvented. For this purpose, the following expression has been applied:

φ = φpf · g(|D|) + φr · (1 − g(|D|))

(20)

where g(|D|) is a function which depends on the module of D. In [5] the following function is proposed:

g(|D|) =

1 1 + arctan[k3 (|D| − k4 )] 2 π

(21)

There are other approaches which can be applied for behaviour blending. For instance, in [7] and [8] behaviour combination is implemented by means of fuzzy logic, and in [10] a sequential hybrid control is presented for the same purpose.

4

Cooperative Localization and Navigation

Having dealt with the localization and navigation problems for a single robot in this type of mission, in this section the cooperative localization and navigation of a team of robots will be addressed. It illustrates the cooperation between robots to improve their localization when the detection of the fixed beacon is not available. The method takes advantage of the measurements of the teammates in order to estimate the pose of each robot. First, the extension of the P.F. formulation to cooperative localization is introduced. Then, three cooperative motion strategies are presented. These allow the team of robots to perform a more efficient localization. Robots are identified by the numbers from 2 to m with the fixed beacon being numbered 1.

10

4.1

Robot Cooperative Localization Based on Particle-Filtering

The pose of each robot is estimated by the PF technique, considering all the robots involved, i.e., the measurements acquired by all the robots. The weights of the particles are updated only when the corresponding robot moves. However, the estimation of position using PF is performed taking into account the measurements acquired by the sensors of that robot and the others. In similar fashion to the case involving the fixed beacon, if B is a robot moving and A is a static robot (see Fig. 5-a), the observation of robot B performed by the robot A defines the values ρAB and ϕAB and consequently each particle associated with robot B defines :

i i

p (ˆ xA − i xB )2 + (ˆ yA − i yB )2 + ξρA i  ρABy = arctan i + ξϕA ρABx

ρAB =

(22)

ϕAB

(23)

where ξρA and ξϕA represent the range and angular uncertainties of the laser sensor of robot A. In this equation, instead of using the known position of the fixed beacon, the estimated position of the robot A (ˆ xA , yˆA , θˆA ) is employed. In the same way, the observation of robot A (see Fig. 5-b), performed by robot B defines i ρBA and i ϕBA (which represents the measurement of the distance and angle with which Robot A will be detected if Robot B were placed over the particle i). Therefore, from the measurement of robot A and B, the weights i wAB and i wBA , which determine the quality of each particle according to both measurements, can be obtained. The weights are calculated by expressions similar to (11), (12) and (13). As a consequence of equation (22), the estimated pose provided by the PF depends on the estimated position of the other robots and it will therefore include cumulative errors. In order to avoid the influence of these errors, the weight of the particles will be influenced only by the observations involving the fixed beacon when these measurement are feasible, i.e., the observations between robots will be taken into account only when the observations of the beacon are not available. Thus, the weight of the particle i of the robot j is given by    i wj =  

i

λj1 · i w1j · i wj1

if

V1j + Vj1 = 1

if

V1j + Vj1 = 0

(24) i

λj0 ·

Qm

n=2,n6=j

i

wnj · i wjn

where i λj1 and i λj0 are normalizing factors introduced so that the sum of the weight is equal to one. It should be 11

noted that the value of i wj depends on the logic variables Vj4 (equal to 1 if the robot j can observe the fixed beacon) and V4j (equal to 1 if the fixed beacon is able to observe the robot j). In this way, each time the observations of the fixed beacon are available, the vehicles will apply them for their localization, using a fixed reference, whose uncertainty does not increase with time.

4.2

Navigation Strategies

In this paper, the objective is the exploration of an unknown environment. For this purpose, the robots could perform different navigation tasks at the same time: surveillance services, the application of mapping techniques, or the following of a specific trajectory, for instance. At this point, this approach presents different strategies for improving the localization of a team of robots according to the techniques presented in sections 3 and 4.1. Similar to the strategy described in section 3.3, closed paths are defined for each unit of the team to follow. Thus, for a correct path following, localization is carried out using the PF approach described in section 3.2. Nevertheless, as the path may be such that the fixed beacon is not detected, the cooperative localization techniques presented in section 4.1 are also applied. In this case, certain robots act as beacons to the others, with this role interchanged between them according to a given motion strategy. In [22, 16] the same idea is applied to a team of Milibots which is organized in such a way that part of the team remains stationary providing known reference points for the moving robots. This approach, known as “leapfrogging” behaviour, uses trilateration to determine the robots’ positions. To apply trilateration, at least two beacons are required. However, in the navigation problem considered in this paper, there are situations where only a single robot is visible and trilateration is not the best option. In [11] a collaborative exploration strategy is applied to a team of two robots where one is stationary, and acts as a beacon, while the other is moving, with the roles being switched between them. In this approach, the PF technique is applied for cooperative localization and mapping. Nevertheless, it is assumed that the heading of the observed robot can be measured. Since this estimation is not trivial, this paper presents a different approach for PF. In the following section three strategies have been evaluated, considering teams with different numbers of robots in order to determine the option which presents the best performance.

4.2.1

First Strategy

The idea of this first approach is to take advantage of the mutual observations between the robots when detection of the fixed beacon is not available. If the robots can detect each other the uncertainty may decrease and the pose

12

estimation will improve. In order to make these observations feasible, a team of two robots is organized. Both robots will follow the same predefined path but in different directions. In this way, the robots will meet at some point along the path, and detect each other. Fig. 6 illustrates the operation of this technique. When the robots navigate beyond the range of the fixed beacon the estimation of the poses gives a weak performance (see Fig. 6-a). Once the observations are available the uncertainty decreases and the estimated pose is closer to the real configuration (see Fig. 6-b). Nevertheless, this strategy presents some drawbacks. If one of the robots presents a large estimation error, the observations in common could decrease the accuracy of the pose estimation of the other robot in the team. Therefore, as will be illustrated in section 5, this strategy presents good results when the length of the path is not large. If, however, the meeting between the robots takes place far from the last observation of the beacon, the estimation error could increase and the performance of the method would be low.

4.2.2

Second Strategy

In this second approach the team is divided into two categories of robots. One of the robots is considered the “master” having the responsibility of performing an accurate path following of a previously planned trajectory. The other two robots, the “slaves”, play the role of mobile beacons. The master robot is identified by the number 2, and the slave robots by 3 and 4 (see Fig. 7). Initially, the master follows the planned path until it is not able to detect at least one slave. Once this occurs, the master stops and the slave robots start to navigate sequentially. They try to reach different poses where they will act as a beacon for the master. These poses can be determined previously, taking into account the path and a criterion of good coverage, or can be established during navigation, taking into consideration the position where the master stopped. In both cases, the objective is to “illuminate” the master navigation with the slave mobile beacons in such a way that it can be located accurately. At this stage, once the master stops and one of the slaves is moving, the master and the other slave play the role of beacon for the slave which is moving. In accordance with the above and considering the type of sensors and the angular field of view, a strategy for the motion of the slaves is considered. The strategy is based on the idea of building a triangular formation after the master stops, which happens when the master is not able to detect at least one slave. Using this triangular configuration the vehicles are able to estimate their position by means of the measurement of their relative positions with the minimum possible error. Furthermore, slave robots give the master a large position estimation coverage. This configuration is fundamental if the view angle of the sensors is constrained, if not, the robots can see each other in any configuration and the triangular formation becomes unnecesary.

13

Once the master has stopped, two points P2 and P3 are generated (see Fig. 7) which the slaves have to reach with the same orientation as the master so that they can see each other. P2 and P3 are calculated by taking into account the angular field of view of the robots. Hence, P2 is located along the master’s longitude axis and P3 is such that the master, P2 and P3 define an isosceles triangle. In this way, when the master robot stops, a path is generated such that it connects each current slave position with the goal point (P2 or P3 ). This path also has to accomplish the curvature constraint and allow the slave vehicles to reach the goal point with the correct orientation. For this purpose, β-Splines curves, [6], have been applied as represented in Fig. 7. Master and slave perform the path following and collision avoidance sequentially. Slave 3 starts moving when the master has stopped. Slave 4 begins to move once slave 3 reaches its goal. Finally, the master starts moving again when slave 4 has reached its target configuration. The main drawback to this method is the tight dependency of the efficiency in the pose estimation on the number of robots. As will be shown in Section 5, localization improves as the number of robots in the team increases, although the time required to close a loop also increases with the number of team members. In the next section a different strategy is developed so that the time the robots take to close the loop does not increase with the number of robots.

4.2.3

Third Strategy

The present strategy aims to develop a method in which localization does not rely on the number of robots in the team. It is based on the idea of propagating uncertainty [25] and takes advantage of the robustness of the PF approach to deal with situations where no observations are available. In this strategy there is no master or slave robots. In contrast, there is one robot navigating close to the fixed beacon. As a consequence, there will be frequent observations between the beacon and this robot. Hence, the weight of the particles of this robot will be frequently updated taking into account these observations. Due to the fact that the uncertainty associated with the fixed beacon position is low, the localization procedure will provide an accurate pose estimation for this robot. As a consequence, if any teammate observes or is observed by this robot the pose estimation of this teammate will be improved and its associated uncertainty will decrease. This effect is known as the ’certainty propagation’, and this type of robot is called the certainty propagator robot. The graphics in Fig. 8 are derived from the experiments presented in the following section. They show the effect of the certainty propagation. In Fig. 8-a) the robot Pr is the propagator and robot B is navigating beyond the range of the fixed beacon and robot Pr. As a result, there is no observation involving robot B (the dotted arcs

14

represent the boundaries of the detection ranges). The real path of B is represented by a dark line, whereas the estimated path is represented by a light line and the reference path is drawn as a dotted black line. Observe in this figure that there is a huge uncertainty associated with robot B (the particle cloud is distributed over a vast amount of space and the estimation error is large). However, in Fig. 8-b) and c) robots Pr and B can detect each other, and, as a consequence, the pose estimation of robot B has been improved and its certainty has increased (the particle cloud has shrunk). In this situation, a significant consideration has to be taken into account. If the propagator robot used the other robot observation for updating its particle’s weight, experience shows that the accuracy of the pose estimation of the propagator would decrease. Therefore, the PF of the propagator robot will only use observations involving the fixed beacon, i.e. equation (24) is modified and the weight of the particle i of the propagator robot is:

i

wP = i λP · i w1P · i wP 1

(25)

In the same way, the weight of the particles of robot B are updated according to the expression:

i

wB =

  

i

λB1 · i w1B · i wB1

if

V1B + VB1 = 1

  i λB0 · i wP B · i wBP

if

V1B + VB1 = 0

(26)

i.e. robot B will only take into account the observations of the propagator robot if the fixed beacon observations are not available. From this concept, the basis of the motion strategy is the following. On one hand, there will be one propagator robot (there could be more than one, although for the sake of simplicity only one is considered in this paper). On the other hand, a team of robots, called the scouts, will navigate in zones where the beacon’s observations are not available (in this paper two scout are considered). Similarly to the previous strategies, all the robots will follow predefined paths and apply reactive behaviour when possible collisions are detected. Nevertheless, and differently from the previous approaches, each robot will navigate autonomously with respect to the other robots’ motion, following different paths. Furthermore, the paths of the scouts will be designed in such a way that: a) scouts navigate in separate zones; b) meetings between the propagator and the scouts (i.e. common observations) will be provided. Each time a scout meets a propagator it will improve its pose estimation. The propagator will then memorize the places where it is observed by the scouts, and, subsequently, will stop over these zones and

15

wait for the visit of the respective scout robots. At the same time, the paths could be reshaped according to the data collected during navigation so that the number of meetings increases. In this way, the certainty of the fixed beacon will be periodically propagated to the scouts. Clearly, this basic strategy can be improved in various ways depending on key practical issues. For instance, if the robots could continuously exchange information about their pose, the propagator would only stop if the scout is close to the meeting point, thus avoiding possible delays. Another improvement, would be the use of a team of propagator robots so that the number of meetings could increase. Results from simulations following this basic strategy and involving one propagator and two scouts will be illustrated in the following section.

5

Simulation Results

This section is dedicated to presenting different experiments which were implemented to test the proposed approaches. Comparisons between them are also included. For this purpose several trajectories were generated in order to discover the method offering the best localization performance. Likewise, the effect of the length and the shape of the path on the estimation error has been evaluated. Additionally, the influence of the number of robots on the localization accuracy has also been taken into account. The algorithms were tested with the robots performing both clockwise and anti-clockwise loops and using the same scenario for all the strategies. A closed path returning to the fixed beacon was established for comparing the first and the second strategies, while a closed path far out of range of the beacon were used for testing the third strategy. In all the experiments, the estimation error was compared with that obtained when no cooperative navigation was applied. To this effect, Fig. 9-a) presents the results relating to the technique described in Section 3, i.e., a fixed beacon and a single robot with no cooperative localization applied. In this figure the desired path is represented by a dotted-line, the real path by a continuous line and the estimated path by a dashed-line. The fixed beacon is represented by the unfilled circle. Fig. 9-b) presents the evolution of the particle cloud during the navigation process, illustrating the associated uncertainty. The cloud shrinks when the robot observes the fixed beacon and enlarges when no observation is available. In contrast, Fig. 10-a) and b) present the results obtained by applying the first strategy. In this experiment the team consist of two robots following the path in opposite directions. Fig. 10-a) illustrates the paths of the robots (continuous line) and the estimated path (dashed-line). In this case the robots closed the loop three times. It is notable that the particle cloud shrinks when mutual observations are feasible (see Fig. 10-b). 16

In like fashion, Fig. 11-a) and 12-a) illustrate the evolution of the real and estimated trajectories of the master robot when the second strategy is applied to a team of two and three robots respectively, following the same desired path. Fig. 11-b) and 12-b) show the evolution of the particle cloud for the master robot in each case. The estimation is improved, especially in the case of three robots and consequently the navigation of the master robot is closer to the desired path. In each experiment the robots completed the loop three times. The improvement in the pose estimation as a result of applying the cooperative localization techniques can clearly be seen in these figures by the fact that the real and the estimated paths are closer than in the single robot experiment. Fig. 13 presents the error of the position estimation in the previous experiments during one loop. It should be note that in the second strategy the error decreases when the number of the robots in the team increases. However, the time the robots take to close the loop increases with the number of teammates, since the motion strategy for cooperative localization requires that part of the team remains stationary while one of the robots is navigating. Given that the second strategy offers the best pose estimation, it was applied to different paths in order to study the influence of the length and shape of the trajectory on the localization performance. In particular, Fig. 14 illustrates two experiments where a team of three robots closed the loop several times. In the case of Fig. 14-b), the path (which is referred to as the Daisy path for its resemblance to the flower) brought the robots close to the beacon three times. Fig. 15 presents the error of the position estimation during one loop of the experiments represented in Fig. 14-a), b) and 11-a). It illustrates that, as expected, shorter paths provide lower error and therefore better performance. In addition, this figure also demonstrates that the pose estimation can be improved by modifying the shape of the path, as is the case in the experiment involving the Daisy path (Fig. 14-b). In this experiment, the error remains bounded, presenting lower values than the other experiments. This improvement is due to the effect of navigating near the beacon and applying its perception for updating the weight of the robots’ particles. Obviously, the pose estimation improves with an increase in time periods during which the robot navigates near the beacon. One conclusion is that, if the exploration of large wide spaces is required, trajectories similar to the daisy path are to be preferred, i.e., trajectories in which the team returns within the range of the beacon several times. Finally, the third strategy was tested with a team of three robots (two scouts and a propagator). The path for the scout robots was defined in such a way that observations of the fixed beacon were not available. Thus, only observations between the propagator and the scouts were feasible. Fig. 16 presents the results of applying this technique. It illustrates the real and the estimated trajectories of the scouts and the propagator. The dotted circle represents the limit of range of the beacon. Fig. 17 presents the estimation error of one of the scout robots

17

compared with the results obtained when the other navigation strategies are applied for following the same path. One relevant conclusion of this comparison is that the second and the third strategies provide an acceptable and more accurate pose estimation. The second presents better performance, although it requires more time to close the loop than the third strategy. This is because in the second strategy the robots frequently stop, whereas in the third the scout robots never stop. It is also interesting to note that in this experiment the first strategy results in a poor performance, even worse than the single robot localization. In this case, mutual observations decrease the accuracy of the estimation because, due to the length of the path, the estimated pose had become considerably degraded by the time the robots met.

6

Conclusions

This paper presents three navigation strategies for improving cooperative localization based on a Particle-Filter approach for a small fleet of car-like vehicles in scenarios without a map or GPS and with no human intervention. A team of robots and a fixed beacon have been considered in such a way that the robots take advantage of cooperative techniques for both localization and navigation. Each robot serves as an active beacon for the others, working as a fixed reference and, at the same time, providing observations to the robot which is moving. With this approach, robots are able to follow a set of predefined paths, which it may not be possible to follow precisely due to unexpected obstacles. Furthermore, the proposed approach has been validated by various simulated experiments. Different paths and numbers of robots have been considered. The experiments illustrate that the a higher number of teammates decreases the estimation error. Likewise, the length of the planned path affects the quality of the estimation process. However, for paths of similar length, the estimation procedure can be improved by changing its shape. The next step is the implementation of this approach in a team of real car-like robots. There are still open issues requiring further research, in particular the number of particles and other tuning parameters, the matching between beacons and other mobile robots and the implementation of active beacons.

Acknowledgments Work partially supported by the Autonomous Goverment of Andalucia and the Portuguese Science and Technology Foundation (FCT) under Programa Operacional Sociedade do Conhecimento (POS_C) in the frame of QCA III. First author also acknowledges the support received by the ISR.

18

References [1] A. Martinelli, F. Pont, and R. Siegwart. Multi-robot localization using relative observations. Proc. of the 2005 IEEE Int. Conf. on Robotics and Automation, 2005, pp. 2808-2813. [2] A. Ollero. Robotica: Manipuladores y Robots moviles. Marcombo, 2001. [3] A. Ollero, A. G. Cerezo, and J. Martinez. Fuzzy supervisor path tracking of mobile robots. Control Engineering Practice, 2(2), 1994, pp. 313-319. [4] A. Papoulis. Probability, Random Variables and Stochastic Processes. McGraw-Hill, 1991. [5] A. Vale. Mobile Robot Navigation in Outdoor Environments: A Topological Approach. PhD thesis, Instituto Superior Técnico, Universidade Técnica de Lisboa, Portugal, June 2005. [6] B. Barsky. Computer graphics and geometric modelling using β-Splines. S.-Verlag, 1987. [7] F. Cuesta, F. Gomez-Bravo, and A. Ollero. A combined planned/reactive system for motion control of vehicles manoeuvres. Proc. of the IFAC W. on Motion Control, 1998, pp. 303-308. [8] F. Cuesta and A. Ollero. Intelligent Mobile Robot Navigation. Springer, 2005. [9] F. Cuesta, A. Ollero, B. Arrue, and R. Braunstingl. Intelligent control of nonholonomic mobile robots with fuzzy perception. Fuzzy Set and Systems, 134, 2003, pp. 47-64. [10] F. Gomez-Bravo, F. Cuesta, and A. Ollero. A new sequential hybrid control structure for car and tractor-trailer parallel parking. Proc. of the 7th IFAC Symposium on Robot Control (SYROCO), 2003, pp. 605-610. [11] I. Rekleitis. A particle filter tutorial for mobile robot localization. (TR-CIM-04-02), 2004. [12] J. Fenwick, P. Newman, and J. Leonard. Collaborative concurrent mapping and localization. IEEE Conf on Robotics and Automation - Washington, DC, May 2002. [13] J. O. Coronel, F. Blanes, P. Pérez, M. Albero. G. Benet and J. E. Simó. Dynamic Resource Allocation In Real-Time Control System: Aplication to Robot Control. Revista Iberoamericana de Automática e Informática Industrial, 3(2), 2006, pp. 71-78. [14] J. Liu, R. Chen, and Logvinenko. A theoretical framework for sequential importance sampling and resampling. In Sequential Montecarlo in Practice, Springer-Verlag, 2001.

19

[15] K. W. Tang and R. A. Jarvis. An evolutionary computing approach to generating useful and robust robot team behaviours. Proc. of 2004 IEEE/RSJ IROS, 2004, pp. 2081-2086. [16] L. E. Navarro-Serment, R. Grabowski, C. Paredis, and P. Khosla. Localization techniques for a team of small robots. IEEE Rob. and Aut. Magazine, 9, 2002, pp. 31-40. [17] M. Betke and L. Gurvits. Mobile robot localization using landmarks. IEEE Transaction on Robotics and Automation, 13(2), 1997, pp. 251-263. [18] M. S. Grewal and A. P. Andrews. Kalman Filtering Theory and Practice. Prentice-Hall, Englewood Cliffs and New Jersey, 1993. [19] M. Pasquiera, C. Quek, T. Tanb and S. Cheec. Opportunistic planning for a fleet of transportation robots. Engineering Applications of Artificial Intelligence, 14(3), 2001, pp. 329-340. [20] P. Lima, L. Custódio, M. I. Ribeiro, and J. Santos-Victor. The rescue project: Cooperative navigation for rescue robots. Proceedings of the 1st. International Workshop on Advances in Service Robotics, ASER’03, March 2003. [21] R. Braunstingl, P. Sanz, and J. M. Ezquerra. Fuzzy logic wall following of a mobile robot based on the concept of general perception. Proc. of the Seventh International Conference on Advanced Robotics, 1, 1995, pp. 367-376. [22] R. Grabowski and P. Khosla. Localization techniques for a team of small robots. Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, 2, 2001, pp. 1067-1072. [23] R. Madhavan, K. Fregene, and L. E. Parker. Distibuted cooperative outdoor multirobot localization and mapping. Autonomous Robots, (17), 2004. pp. 23-39. [24] S. S. Ge and C.-H. Fua. Complete multi-robot coverage of unknown environments with minimum repeated coverage. Proc. of the 2005 IEEE ICRA, 2005, pp 727-732. [25] S. I. Roumeliotisand and I. Rekleitis. Propagation of uncertainty in cooperative multirobots localization: Analysis and experimental result. Autonomous Robots, 17(1), 2004, pp. 41-54. [26] S. Thrun. Probabilistic algorithm in robotics. Artificial Intelligence Magazine, 21(4), 2000, pp. 93-109. [27] S. Thrun, D. Fox, W. Burgard, and F. D. Murphy. Robust monte carlo localization for mobile robot. Artificial Intelligence Magazine, 128(1-2), 2001, pp. 99-141. 20

[28] T. Gustavi, X. Hu, and M. Karasalo. Multi-robot formation control and terrain servoing with limited sensor information. Preprints of the 16th IFAC World Congress, July 2005.

21

Figure Captions

Fig. 1: Robot’s kinematic model (left) and sensorial capabilities (right)

Fig. 2: a) Robot and a fixed beacon observation; b) each particle and robot observations

Fig. 3: Pure-pursuit algorithm: definition of LH (left) and selection of the target point (right)

Fig. 4: Definition of the perception vectors di (left) and definition of the global perception vector D(right)

Fig. 5: a) Observations of robot B and particles by the robot A; b) observations of robot A by the particle Bi and by the robot B

Fig. 6: First strategy: a) no observations are available between robot A and B; b) improvement of the pose estimation after the observations between robots A and B

Fig. 7: β-spline generation

Fig. 8: Third strategy: a) robot B is detected by neither the beacon nor robot Pr; b) observations between robot B and Pr are available; c) the certainty has been propagated

Fig. 9: a) Single robot navigation and b) evolution of the cloud of particles

Fig. 10: The first strategy: a) Two robots in opposite directions; b) evolution of the cloud of particles

Fig. 11: The second strategy: a) two robots in a big loop and b) evolution of the cloud of the master robot

Fig. 12: The second strategy: a) three robots in a big loop and b) evolution of the cloud of the master robot

Fig. 13: Position estimation error during the big loop with respect to a single robot, and the first and the second strategies

22

Fig. 14: The second strategy: cooperative navigation over two different paths: a) a short loop and b) a Daisy loop

Fig. 15: The second strategy: position estimation error over one loop for a team of three robots along different paths

Fig. 16: The third strategy: one propagator robot and two scout robot

Fig. 17: Comparing the position estimation error over one loop of the three strategies

23

FIGURES

Y

l

φ

ρ

ρmax θ

ϕmax

y

ϕmax x 

ϕ

ρmax

X

Fig. 1: Robot’s kinematic model (left) and sensorial capabilities (right)

24

Cloud of particles Y

i

i

i

( xB, yB, θB ) i

ρLB ρ LB

i

B

( x B, y B, θ B )

ϕLB ϕ LB

L yL xL

X a)

i

B B

i

θB

i

ρBL

i

ϕ BL

ϕBL

θB

ρ BL

L ρ BA

b) Fig. 2: a) Robot and a fixed beacon observation; b) each particle and robot observations

25

Target point

∆

(xt , yt)

Path s Target point

r LH

r (xn , yn)

∆

Current robot’s position

Fig. 3: Pure-pursuit algorithm: definition of LH (left) and selection of the target point (right)

26

ρ2

d2

ρ3

ϕD

ρ4

d3

D

d1 d4

Fig. 4: Definition of the perception vectors di (left) and definition of the global perception vector D(right)

27

i

i

i

( xB , yB , θB ) i

ρAB ρ AB

B ( xˆ B, yˆ B, θˆ B )

i

ϕAB ϕ AB

θˆ A

A

( xˆ A, yˆ A, θˆ A )

a) iB

B

i

θB

i i

ρBA

ϕ BA

ϕBA

θB

ρ BA

A b) Fig. 5: a) Observations of robot B and particles by the robot A; b) observations of robot A by the particle Bi and by the robot B

28

Real path of robot B

Estimated path of robot A

Particles of robot B

Particles of robot A Range limit of robot A

Robot B

Robot A

Range limit of robot B

Real path of robot A

Estimated path of robot B

a)

Robot A

Robot B

Improvement of the estimation

b) Fig. 6: First strategy: a) no observations are available between robot A and B; b) improvement of the pose estimation after the observations between robots A and B

29

1 Beacon

θ

P3

θ 2 Master

4 Slave

θ

3 Slave

P2 Fig. 7: β-spline generation

30

Particles of robot B

B Real path

Particles of robot B Range limit of robot B

Reference path

B

Range limit of robot Pr

Pr

Estimated path

Pr

Range limit of the beacon

Beacon

Beacon

a)

b) Improvement of the path estimation B

Pr

Beacon

c) Fig. 8: Third strategy: a) robot B is detected by neither the beacon nor robot Pr; b) observations between robot B and Pr are available; c) the certainty has been propagated

31

a)

b) Fig. 9: a) Single robot navigation and b) evolution of the cloud of particles

32

Robot B

Robot A

a)

Meeting between the robots

b) Fig. 10: The first strategy: a) Two robots in opposite directions; b) evolution of the cloud of particles

33

SLAVE

a)

b) Fig. 11: The second strategy: a) two robots in a big loop and b) evolution of the cloud of the master robot

34

SLAVE 3

SLAVE 2 MASTER

a)

b) Fig. 12: The second strategy: a) three robots in a big loop and b) evolution of the cloud of the master robot

35

60

Error

Single robot

50

First strategy 40

Second stretegy (two robots) 30

Second strategy (three robots) 20

10

0

0

100

200

300

400

500

600

700

800

t(s)

900

Fig. 13: Position estimation error during the big loop with respect to a single robot, and the first and the second strategies

36

SLAVE 3 SLAVE 2 MASTER

a)

SLAVE 3 SLAVE 2 MASTER

b) Fig. 14: The second strategy: cooperative navigation over two different paths: a) a short loop and b) a Daisy loop

37

Fig. 15: The second strategy: position estimation error over one loop for a team of three robots along different paths

38

700

600

Range limit 500

Propagator

400

300

Scout 200

100

Scout 0 0

100

200 300third strategy: 400 Fig. 16: The one 500 propagator600 robot and700 two scout 800 robot

39

900

1000

120

Error First strategy 100

80

Single robot 60

Third strategy 40

Second strategy 20

0

0

100

200

300

400

500

600

700

t(s)

800

Fig. 17: Comparing the position estimation error over one loop of the three strategies

40