Experiments with Simultaneous Environment ...

Experiments with Simultaneous Environment Mapping and Multi-Target Tracking Abedallatif BABA and Raja CHATILA LAAS-CNRS 7 Avenue du Colonel Roche, 31077 Toulouse Cedex 4, France {ababa and raja.chatila}@laas.fr Abstract. This paper presents experimental results for building a dynamic environment model based on an occupancy grid, while detecting and tracking mobile objects using a particle filter. The idea is to represent all the objects in the robot’s environment as groups of cells, and mobile groups such as people are distinguished from the fixed parts and are not included in the built map, and their motion is tracked.

1

Introduction

Local perception would be sufficient for a robot to navigate freely in a populated environment and to avoid obstacles, but if the robot needs to locate itself with respect to its environment (not just with odometry or dedicated means), and to plan its motion based on the fixed features, then it needs to have a map. This map has to be correct, permanently updated and clear from the traces of moving objects trajectories, and built while the robot is navigating. This is hence a SLAM (Simultaneous Localization And Mapping) problem. Approaches assuming a static environment for achieving SLAM are in this case not adequate. For this reason, the robot has to detect and track mobile objects while it is mapping its environment. In the next section, we briefly overview related works. Then in section 3 we recall mapping using occupancy grid. Section 4 presents our strategy to describe the environment as groups of cells. Section 5 explains our method to detect dynamic targets, each detection state is sufficient to trigger one tracking process. In section 6, our tracking method is presented as a variation of an approach [7] that relies on both bearing-only tracking and particle filter techniques. This approach is adapted to achieve multi-target tracking. Section 7 shows experimental results and section 8 concludes with future directions.

2

Related Work

There have been recently a number of results on robot mapping and localization in populated environments. Most approaches suppose a known map, and focus on distinguishing mobile objects from the static map. For example, Burgard et al. [5] update a static map using the most recent sensory input to deal with people in the environment. Montemerlo et al. [12] present an approach to simultaneous

localization and people tracking using a conditional particle filter. Arras et al. [1] use line features for localization and filter out range measurements reflected by persons. Fox et al. [6] use a probabilistic technique to filter out range measurements that do not correspond to the environment model. Our technique, in contrast, does not require a given map. On the other hand, there are a variety of techniques for tracking persons, for predicting future poses of persons, or for adopting the behavior of the robot according to the information obtained about the persons in its vicinity [3, 11, and 13]. However, these techniques do not consider the measurements corresponding to persons in order to improve the model of the environment. Our approach is to build the map while tracking people circulating in the environment to avoid including them in the map. Wang and Thorpe [14] addressed this issue. Their approach segments the data to extract features. Recently, Hashimoto et al. [10] presented a method for tracking multiple moving objects detected from laser image via a heuristic rule and an occupancy grid based method; moving objects are tracked based on Kalman filter and an assignment algorithm. In their experiments the grid cell size is 30x30 cm², which decreases the precision of the described environment. In addition to that, to distinguish between a static object and dynamic one they need 10 laser hits, as they need more than 13 successive scans to make a tracking decision. Hahnel, Schulz, and Burgard [8] presented a probabilistic approach to filtering people out of sensor data and to incorporate the results of the filtering into the mapping process. Their approach is very sound, but it needs to use scan alignment for each two successive maps to detect the dynamic objects, which may be time consuming. The approach we use in this paper doesn’t need specific features. It’s not using scan alignment neither, the environment is represented by occupancy grids which enables to be independent of any kind of representation or features, and the grid representation is less complex to deal with that the scans themselves. Human motion is detected by comparing the successive occupancy grid models, only in the zone of the robot’s view to economize the time of this step, and tracking is accomplished depending on bearing only tracking approach using a particle filter.

3

Map Building Using an Occupancy Grid

Because of its simplicity to be constructed, we have adopted the occupancy grid as a method for building the environment map. In this representation the environment is decomposed in cells, and to each cell is assigned a random variable describing its probable state of being occupied, free or unknown. The algorithm used to calculate and update the occupancy probability of cells exploits the Discrete Bayes Filter (DBF), which is very convenient with this environment representation, x being a cell and z a measurement, we have at time t the two fundamental steps of this filter as follows: (1) pˆ ( xt = occ ) = Σ xt −1 p ( xt = occ | z t , xt −1 ) * p ( xt −1 )

This first step calculates the cell probability to be occupied as a sum of products of two probabilities, the first p ( xt −1 ) is the occupancy probability of the same cell at previous moment, and the second p ( xt = occ | z t , xt −1 ) is the probability of the measurement z t to cause a transition of cell state from previous moment to the current moment.

p ( xt = occ) = p ( z t | xt = occ) * pˆ ( xt = occ) * p dist

(2)

The second step is related to the fact that measurements are noised, so it is important to describe this noise by p ( z t | xt = occ) which is the conditioned probability to have a measurement at moment t if the measured cell is occupied at the same moment. The cell probability to be occupied has also to take in its account the sensor model which is explained as follows. p dist = (distMax − distMeasur ) / distMax (3) distMax is a maximal distance can be measured by laser sensor, and distMeasur is a measured distance of one cell from the robot. So, a closer occupied cell of the robot has a larger probability to be occupied. Now to calculate the cell probability to be free, we have: (4) pˆ ( xt = free) = Σ xt −1 p( xt = free | z t , xt −1 ) * p ( xt −1 )

p ( xt = free) = p ( z t | xt = free) * pˆ ( xt = free)

(5)

The cell unknown state is its state when it is hidden, and the sum of the three probabilities of each cell to be occupied, free and unknown equal to on, then: p ( xt = unk ) = 1 − ( p( xt = free) + p( xt = occ)) (6) And all cells in the grid are initialized to have the unknown state. p ( xt =0 = free) = 0 , p ( xt =0 = occ) = 0 and p ( xt =0 = unk ) = 1

(7)

Fig. 1. Example of occupancy grid built in our laboratory with a 2D scanning laser range finder. The occupancy value is coded as follows: green: unknown; white: free; blue: occupied. The area is 4x4m²; a cell is 5x5cm².

4

Occupied Cells Grouping and Moving Target Detection

When a dynamic environment is represented by an occupancy grid, it becomes a practical solution to regroup occupied neighbouring cells existing in the robot vicinity, in the form of clusters or segments, considering them to be only one target and to provide the tracking step with the central coordinates of a dynamic group. This reduces the number of trackers. This also compensates for very noisy measurements, clusters being more stable than individual cells. Group detection is explained next. The occupancy grid representation enables to compare successive maps (represented by a matrix of numbers in [0, 1]) to detect changes rather easily. A sudden change in occupancy state for a given cell between two successive scans will be sufficient to trigger a detection procedure, which will search the group for which this cell belongs. Then it evaluates the occupancy state for all the members in that group. If they all have a liberation state (a drastic change in occupancy), the group will be considered a dynamic object in motion, and its central coordinates are computed and sent for one tracking step.

Fig. 2. Example of an occupancy grid with an object moving towards the left. The occupancy probabilities change drastically when the object moves.

5

Target Tracking

Tracking people in the robot’s environment is not a new problem. Stated otherwise, this is a classical target tracking problem present in a large number of applications. The Kalman filter, quite popular for SLAM, was actually devised for this application. In our context, we however need a recursive Bayesian filter,

which does not have the limitations of linearity nor gaussianity. Particle filters are a common tool for coping with this problem. Bearing angle T

Detection and measurements

R

Fig. 3. Two virtual layers, first one is the elaborated occupancy grid, the second represents the level of targets tracking, where Robot-Target geometry and the bearing angle are illustrated. The idea of our approach is illustrated in Figure 3, where we exploit the occupancy grid elaborated in the first layer to detect dynamic objects, and to take measurements which are indispensables for the particle filter, working in the second layer, to achieve target tracking process based on bearing angle. This process can be stated as follows: given several targets defined by their state (position, velocity, acceleration) measured by noisy sensors, compute at each time step the estimate of the target state, and to simplify this tracking problem, we have considered only bearing information. 5.1 Single Target Tracking The tracking approach used here, can be considered as a variation of bearing only tracking approach developed in [7] as follows: In the 2D plane, we suppose a target X t defined by its position and velocity. The robot state is also defined by its position and velocity, and the relative target state vector is their difference. This is the state to be estimated. From the system state dynamic equation X t = f t −1 ,ν t −1 and sensor measurements Z t = ht ( X t , wt ) where ν t −1 and wt are considered to be white noises with known probability density functions and mutually independent, we want to compute the posterior P ( X t Z t ) . The target is supposed to be changing directions (maneuvering target), which is quite natural for humans in an indoors environment. Given a straight trajectory, the behavior models of the target are defined as moving straight ahead (noted 1), turning clockwise (2), and turning anti-clockwise (3). Then, S = {1, 2, 3} denotes the set of three models illustrated in Figure 4 (a). These three models are more convenient to estimate the movement of an aircraft, where its maximum turning angle is limited and precalculated, than estimating a human’s random trajectory that doesn’t has any limit, it’s possible at any moment that a person changes suddenly his direction. In addition to that, person tracking is more complex mission, because of receiving from his two moving legs, two detected signals have to be attributed to the same

person, and the second signal caused by his second leg movement has always an inverse instant direction of his global trajectory direction. So, for having an approach takes in its account all those complexities, we have made the modification illustrated in Figure 4 (b), where the same two target estimated states for S = 2 and S = 3 are calculated using the same value of target estimated velocity, but with an inverse direction.

Xˆ tS =1 Xˆ tS =3

Xˆ tS =1 Xˆ tS =2

X t −1 (a)

Xˆ tS =3

X t −1

Xˆ tS =2

(b)

Fig. 4. (a) tracking approach as it is in [7], it calculates three estimated states for three behaviour models; (b) modified tracking approach which is more convenient with human’s movement, it takes in its account the coming back and any sudden change of his trajectory direction. A transition probability matrix defines the state change probability among the three behaviours is:

⎛ π 11 π 12 π 13 ⎞ ⎛ .32 .34 .34 ⎞ ⎜ ⎟ ⎜ ⎟ Π ab = ⎜ π 21 π 22 π 23 ⎟ , The experiments values are ⎜ .34 .34 .32 ⎟ ⎜π ⎟ ⎜ .34 .32 .34 ⎟ ⎝ ⎠ ⎝ 31 π 32 π 33 ⎠ Where π ij ≥ 0 , and the sum along the lines is equal to one. Here we have chosen the probability for the target to go straight ahead π 11 , less than the others, i.e., the person prefers to change directions randomly during movement. We have also chosen probabilities π 23 and π 32 to be also less than the other values, because there is less chance for one person to change suddenly directions from right to left or the opposite. 5.2 Particle Filter As a convenient technique that achieves non-linear filtering with switching dynamic models, which is the case of our problem, we have chosen Multiple Model Particle Filter (MMPF) [7]. In our application, it covers each one of three places where the target is estimated to be, concerning with its three behaviour

models, by a cloud of n particles, Figure 5 (a). To each particle is assigned a weight

wti which is the particle probability to describe the real state.

wti = p( z t | µ ti (rt )) * p(rti | rt −1 ) , where i = 1,...., n .

(8)

zt is a measurement received from the tracked target at moment t , rt ∈ S is a regime variable takes at each moment the values of three behavior models,

µ ti (rt )

is a particle estimated state conditioned by the regime variable value, and

p (rt | rt −1 ) ∈ Π ab is a particle transition probability. i

After one step that kills all less probable particles and chooses the most weighted (probable) one, the desired estimated state is determined and a new estimation step can be started Figure 5 (b).

Xˆ tS+=13

X t −1

zt (a)

Xˆ tS+=12

Xt

Xˆ tS+=11

(b)

Fig. 5. (a) One hundred particles in each one of three clouds, z t is a measurement; (b) choosing the most probable particle at moment t , and starting a new iteration.

5.3 Multi-Target Tracking In the robot’s vicinity, the laser sensor makes its scan over 180 degrees with a frequency of 10 Hz. When there is a measurement, and there are old existing targets (active targets) from previous scans in the robot’s vicinity, a weighting step has to be done over all estimated particles for all those targets. And the measurement will be attributed to the target that generated the most weighted particle. But if the probability of that selected particle is very small, this measurement can’t be attributed, and this case will be considered as a first measurement caused by a new entering target in the robot’s vicinity. If there are more received measurements in the same scan (Figure 6) other weighting steps have to be done for all active targets, to attribute each coming measurement to its convenient target. In the case when a target doesn’t have no more measurement to be attributed for two successive scans, it has to be eliminated. And when the robot’s vicinity is free the list of tracked targets has to be initialized.

Fig. 6. Two tracked targets in the robot’s vicinity, the first measurement z1 has a small probability; so it is not attributed and in the next step it will be considered as a new target, second received measurement z2 in the same scan is attributed to the target 1, because the most probable particle belongs to him, and the third measurement z3 is attributed to the target 2 using the same principle.

6

Experimental Results

In these experiments results we have chosen the number of particles for each one of three behavior models to be 100 particles. In this case, the needed time to estimate one step of one target trajectory is less than 200 ms. The results shown in Figures 7 and 8 have good accuracy (a few cm). Tracking can be done while the robot is constructing its map with the occupancy grid which enables to detect the moving targets and to supply the tracker with their central coordinates, and to discard them from the map. With a laser scanner, the occupancy grid algorithm distinguishes between the dynamic and the static cells using the occupancy probability. Each time the algorithm detects a dynamic group, it starts up a tracker chain for this detected group.

Fig. 7. Tracking a person moving in the robot vicinity (Blue cells: measured trajectory; red cells: estimated trajectory).

3 2

1

Fig. 8. Multi-Target Tracking. While the robot is moving to construct its map, it tracks simultaneously the trajectories of two persons moving in its vicinity, (number one and number two). And in other part of its trajectory, the robot tracks a new target, (number three). Green cells represent the measured trajectories; red cells represent the estimated trajectories and blue cells are static parts in the environment. The cells occupied by the robot during its movement are illustrated by a bar of red cells.

7

Conclusion and Perspective

In this paper we have presented a modified Bearing-Only Tracking approach to track dynamic objects, while the robot is building a map of its environment using an uncertainty grid. We have shown how we used DBF to evaluate the occupancy probability of each cell, as we have illustrated our strategy to regroup these cells in the form of clusters or segments and how to detect dynamic groups which will be tracked. Our target tracking approach estimates at each moment three possible states will be taken by a tracked target in the next moment. After one step of diffusing one hundred particles in each one of those three estimated states, and tacking one measurement to evaluate the particles weights, only the most weighted (probable) particle will be chosen as a desired estimated state, and from it a new estimating step can be started. Future work will deal with including visual data to ameliorate the efficiency of this approach, e.g., to attribute several received measurements to their dynamic sources (targets) even in the case of two trajectories intersection, or to identify people moving around the robot even if a person comes to stop.

References 1.

2. 3.

4. 5.

6.

7. 8.

9.

10.

11.

12.

13.

14.

K.O. Arras, R. Philippsen, M. de Battista, M. Schilt, and R. Siegwart. A navigation framework for multiple, mobile robots and its applications at the Expo.02 exhibition. In Proc. of the IROS-2002 Workshop on Robots in Exhibitions, 2002. A. Baba and R. Chatila. Simultaneous Environment Mapping And Mobile Target Tracking. IAS-9, IOS Press, 2006, ISBN 1-58603-595-9, pp.145-152. M. Bennewitz,W. Burgard, and S. Thrun. Using EM to learn motion behaviors of persons with mobile robots. In Proc. of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2002. D. Beymer and Konolige K. Tracking people from a mobile platform. In IJCAI-2001 Workshop on Reasoning, with Uncertainty in Robotics, 2001. Burgard, A.B. Cremers, D. Fox, D. Hähnel, G. Lakemeyer, D. Schulz, W. Steiner, and S. Thrun. Experiences, with an interactive museum tour-guide robot. Artificial Intelligence, 114(1-2), 2000. D. Fox, W. Burgard, and S. Thrun. Markov localization for mobile robots in dynamic environments. Journal of Artificial Intelligence Research (JAIR), 11:391427, 1999. N. Gordon, B. Ristic, S. Arulampalam,. Beyond the Kalman filter, particle filter for tracking applications. Artech House 2004. D. Hähnel, D. Schulz, and W. Burgard. Mapping with mobile robots in populated environments. In Proc. Of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2002. D. Hähnel, R. Triebel, W. Burgard, S. Thrun. Map Building with Mobile Robots in Dynamic Environments. In Proc. of the IEEE International Conference on Robotics and Automation (ICRA), 2003. M. Hashimoto, S. Ogata, F. Oba and T. Murayama. A Laser Based MultiTarget Tracking for Mobile Robot. IAS-9, IOS Press, 2006, ISBN 1-58603595-9, pp.135-144. B. Kluge, C. Koehler, and E. Prassler. Fast and robust tracking of multiple moving objects with a laser range finder. In Proc. of the IEEE International Conference on Robotics and Automation (ICRA), 2001. M. Montemerlo and S. Thrun. Conditional particle filters for simultaneous mobile robot localization and people-tracking (slap). In Proc. of the IEEE International Conference on Robotics and Automation (ICRA), 2002. C. Stachniss and W. Burgard. An integrated Approach To Goal-directed Obstacle Avoidance Under Dynamic Constraints For Dynamic Environments. In Proc. of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2002. C.-C.Wang and C. E. Thorpe. Simultaneous Localization And Mapping With Detection And Tracking of Moving Objects. In Proc. of the IEEE International Conference on Robotics and Automation (ICRA), 2002.