real-time tracking of multiple moving objects using...

ISSN 0005–1144ATKAAF 46(1–2), 39–48 (2005)

António Almeida, Jorge Almeida, Rui Araújo

Real-Time Tracking of Multiple Moving Objects Using Particle

Filters and Probabilistic Data Association

UDK 004.932’1IFAC 5.9.3;2.5

Original scientific paper

Abstract-Mobile robots and vehicles are increasingly used in dynamic environments populated by humans andother moving objects and vehicles. In this context, tracking of surrounding moving objects is important for obsta-cle avoidance and motion planning. In this paper we present a method for detection and tracking of multiplemoving objects using particle filters to estimate the object states, and sample based joint probabilistic data asso-ciation filters to perform the assignment between the features detected in the input sensor data and filters. Filtersmanagement operations are required for appropriate integration of the currently perceived features. A real-timearchitecture, developed to implement the tracking system, is briefly described. Experimental results obtained witha laser range scanner will be presented demonstrating the feasibility and effectiveness of the presented methods.1)

Key words: mobile robots, particle filters, real-time tracking, probabilistic data association

1) This work was partially supported by FCT, Project POSI/SRI/42043/2001.

1 INTRODUCTION

With the continuous evolution of mobile roboticsthere will be autonomous vehicles in environmentswhere other moving objects, including humans and,vehicles, evolve. Applications include transportation,home, factory, and office contexts. Learning modelsof dynamic environments is an important aspect forautonomous robot navigation [1]. Sensor-based met-hods for modeling and predicting the environmentdynamics would increase the degree of anticipatoryadaptation to environment evolution. In particular,tracking moving objects is important for motionplanning and to anticipate and avoid collisions: gi-ving correct state information of surrounding mov-ing objects to a trajectory planning algorithm wouldand improve robot motion in dynamic worlds.

One of the problems of tracking is that dynamicobjects move in patterns that are highly non-linear.Another important problem is simultaneously track-ing multiple moving objects from a common set ofsensor data. The object state estimation problemhas been tackled using Kalman filters or extendedKalman filters (e.g. [2]). Kalman filters give opti-mal estimates for linear system and measurementmodels compounded with unimodal Gaussian noises.However, the Kalman approximation is often notaccurate enough to model the non-linear, non-Gaus-sian, multi-modal characteristics of the system (ob-

ject(s)) and sensors present in tracking. ExtendedKalman filters permit the approximation of non-li-near problems by linear models. Recently particlefilters were introduced to estimate states for pro-blems with non-linear non-Gaussian process andmeasurement models [3, 4].

This paper presents a method for tracking inreal-time multiple moving objects in dynamic envi-ronments using particle filters. Particle filters arebased on probabilistic representations of states bya set of samples (particles), with the advantage ofmaking possible the representation of non-linearsystem and measurement models, and multi-modalnon-Gaussian density states. For tracking severalmoving objects using a common sensor data set, aSample-based Joint Probabilistic Data AssociationFilters (SJPDAF) algorithm [5] is used to estimateassignment probabilities between isolated segmentson the perceived sensorial data vector (features),and the objects moving on the sensory perceptualrange.

Section 2 briefly overviews particle filters, andSection 3 presents the SJPDAF framework. Section4 presents details about perception and tracking.Section 5 presents the real-time system implemen-tation architecture. Section 6 presents experimentalresults demonstrating the feasibility and effective-ness of the presented methods. Section 7 makesconcluding comments.

AUTOMATIKA 46(2005) 1–2, 39–48 39

2 ESTIMATION BY PARTICLE FILTERS

Particle filters are state estimation methods forsystems with non-linear process and measurementmodels corrupted with noise which may be non--Gaussian and multimodal. These are recursiveMonte Carlo (MC) statistical computing methods.Particle filters are an important alternative to Kal-man filters which are optimal to linear systems cor-rupted with Gaussian noise.

Several methods for position determination andnavigation use particle filters [6]. In this paper wedescribe the application of particle filters for track-ing moving objects, where estimation of objects po-sitions are based on measurements from a laserrange finder.

The key idea of particle filters is to representand maintain the posteriori density function by aset of random samples with associated weights andto compute the state estimate from those samplesand those weights. As the number of samples be-comes very large, this MC characterization becomesan equivalent representation to the usual functiondescription of the posteriori probability densityfunction (PDF), and the method approaches theoptimal Bayesian estimate.

Let denote a random measure that

characterizes the posteriori PDF p(x0:k|z1:k) whereis a set of points with associated

weights and

and are the set of all states andmeasurements up to time k, respectively. Theweights are normalized such that . In this

way the posteriori density at k can be approximatedas

(1)

and we get a discrete approximation of the trueposteriori probability , where δ(•) is the Dirac function, and the particle weights are given by

(2)

To get equation (2), the principle of importancesampling [7, 8] was used. This principle states thatthe weights are taken from an importance densityq(•).

After some studies in this area, results concludethat this algorithm has a degeneracy problem, andthat an acceptable measure for this value is the ef-fective sample number, Neff, that was introduced by[9] and [10], and is represented by

1

1

1

( ) ( ).

( , )

i i ik k k ki i

k k i ik k k

p p

qω ω

−−

−

=zz xx xx xx

xx xx zz

0: 1:( )k kp xx zz

0: 1: 0: 0:1

( ) ( ),sN

i ik k k k k

i

p ω δ=

≈ −∑xx zz xx xx

1iki

ω =∑

1: { , 1, , }k j j k= =zz zz K

0: { , 0, , }k j j k= =xx xx K { , 1, , }ik si Nω = K

0:{ , 1, , }ik si N= Kx

0: 1{ , } sNi ik k ix ω =

(3)

where is named the»true particle weight«.

The exact value of equation (3) is not determi-nable. Therefore, an approximation is given by

(4)

where is the normalized weight. Neff is alwaysless or equal than Ns, thus, when Neff is much smal-ler than Ns there is a big degeneracy.

This problem could be solved with a good choiceof the sampling importance to calculate the correctparticle weights, and with a resampling step intro-duced in the particle filter dynamics.

In our implementation, the importance density isfrom which we need to take samples.

A sample can be generated by first

generating a process noise sampling and setting , where pv(•) is thePDF of vk−1. For this particular choice of the im-portance density the weights are given [7] by

(5)1 ( ).i i ik k k kpω ω −∝ zz xx

1 1( , )i i ik k k kf − −=xx xx vv

1 1( )ik v kp− −vv vv�

1( )i ik k kp −xx xx xx�

1( )ik kp −xx xx

ikω

2

1

1

( )seff N i

ki

Nω

=

=∑

^

1: 1( ) / ( )i i i ik k k k k kp qω∗

−= xx zz xx xx zz

+1

seff i

k

NN

Var ω∗=

40 AUTOMATIKA 46(2005) 1–2, 39–48

Real-Time Tracking of Multiple Moving Objects ... A. Almeida, J. Almeida, R. Araújo

Fig. 1 Overview of the systematic resampling algorithm

In our implementation, a resampling step (Syste-matic Resampling [11], Figure 1) is applied. Thisstep permits the reduction of the effects of degene-racy, observed in the basic particle filter algorithm.The basic idea is to eliminate particles that havesmall weights and to concentrate on particles withlarge weights [7]. After the resampling step, appliedat every time index, all the particles take the sameweight. In this way , and it followsfrom equation (5) that

(6)

The algorithm is presented in Figure 2 and illu-strated in Figure 3.

( ) .i ik k kpω ∝ zz xx

1 1/ik N iω − = ∀

3 SAMPLED-BASED JOINT PROBABILISTIC DATA

ASSOCIATION FILTERS

One way to track various moving objects withparticle filters is to estimate the compound state ofall objects. However, this method becomes imprac-ticable even for a small number of objects sincecomputation grows exponentially in the number ofobjects. This problem can be overtaken by trackingthe objects in individually. A data association prob-lem problem arises in this context: to determinewhich measurement is caused by which object. Inthis paper we apply Joint Probabilistic Data Asso-ciation Filters (JPDAF) [5] for this purpose.

The JPDAF algorithm is an extension of Proba-bilistic Data Association algorithm, that is able totrack various moving objects at the same time andwith the same set of measurements. The JPDAF,calculates the probabilities of association from thelast set of measurements zk to the various objects.Each object has its prediction and measurementmodels – state estimation is performed separatelyfor each object.

Let denote the states of the T

moving objects being tracked at instant k. Each isa random variable in state space of a unique object.Let be a set of measurements at

instant k, where is a feature from that set. Zk =is the sequence of measurements ob-

served up to instant k. The key idea for tracking is how to associate the observed features to the indi-vidual objects.

In the JPDAF model, a joint association event θis a set of pairs ( j, i)∈ {0,...,mk} × {1,...,T}. Each θuniquely determines which feature is assigned towhich object. Feature is used to model currentlyundetected objects – no feature found for such ob-jects. Let Θji denote the set of all valid joint associ-ations events which assign feature j to the object i.At time k, the JPDAF computes the posterioriprobability that feature j is caused by object i ac-cording to

(7)

Assuming that the estimation problem is Marko-vian and using probability theory, the probabilityP(θ|Zk) of an individual joint association event canbe calculated according to

(8)

Here the state of the objects must be known todetermine associations θ. Conversely, θ needs to beknown in order to determine the objects positions.An incremental approximation is applied to over-come this issue. The key idea is to approximate

by the belief about thepredicted state of the objects, i.e. the predictioncomputed using all measurements perceived beforetime-step k. According to this, we obtain

(9)

(10)

where α is a normalizer factor ensuring thatsums up to one over all θ. The termcorresponds to the probability of the as-

signment θ given the current objects states, and isapproximated as a constant. Assuming that eachfeature is detected independently from the others,we get

(11)

where is the probability of all false alarmskm θγ

−

d( )

1( , )

( , )

( ) ( ) ,k

k k

m p q q qkk k k kp q

P

p pθ

θ

θ

γ−

−∈

=

∏ ∫

zz xx

zz xx xx ZZ xx

( )kP θ ZZ( )kP θ ZZ

d

d

1

1

( ) ( , ) ( )

( , ) ( ) ( )

k k k k k k

k k k k k k

P P p

P P p

θ θ

α θ θ

−

−

≈

=

∫

∫

ZZ zz xx xx ZZ xx

zz xx xx xx ZZ xx

1( )k kp −xx ZZ1( , )k k kp −xx zz ZZ

d1( ) ( , ) ( , ) .k k k k k k kP P pθ θ −= ∫ZZ zz xx xx zz ZZ xx

( ).

ji

ji kPθ Θ

β θ∈

= ∑ ZZ

0kzz

{ }1, , k= zz zzK

jkzz

{ }1 , , kmk k k=zz zz zzK

ikxx

{ }1 , , Tk k k=xx xx xxK

AUTOMATIKA 46(2005) 1–2, 39–48 41

A. Almeida, J. Almeida, R. Araújo Real-Time Tracking of Multiple Moving Objects ...

Fig. 2 Overview of the particle filter algorithm

Fig. 3 Particle filter estimation cycle

(features without object in a perception cycle) in zkgiven θ. Using (11) in (10), and inserting the resultin equation (7) we obtain

(12)

Once the assignment probabilities are calculated,the updated estimate of the states is obtained asfollows

(13)

where is the measurement model of thesystem and is the previous estimate pro-

jected to instant k using the system model.

Since particles are used to describe the densityfunction, we use the SJPDAF proposed in [12], sothe method can be applied to a discrete represen-tation. The idea is to represent the density

by a set of N random samples, or par-

ticles that constitutes a discrete approximation ofa PDF. Here, each particle consists on a pair

, where is the state and is the

importance factor. The prediction step of Bayesian

filtering is performed by drawing samples from the

set computed in the previous iteration and by up-

dating their state according to the prediction model

. In the correction step, a measure-

ment zk is integrated into the samples obtained inthe prediction step. With samplebased representa-tion the integration of equation (13) can be doneby summing over all samples generated after theprediction step, and we get

(14)

To help understand equation (14) lets look atFigure 4. This figure represents several possiblecombinations between the detected features and theactive particle filters in the present moment. Theupper right matrix represents a combination wherean object has some probability of being occludedby another. While in all the other combinations allthe present objects are detected and attributed toone one of the features produced by the presentmeasurements. Making all the possible combinationfor each pair (i, j) (represented by equation (14)),the value of each βji could be represented by a ma-trix like the one represented in Figure 5.

With assignment probabilities computed, theweight of each particle can be calculated by

( ) ,( , )

1

1( ) .k

ji

Nm p q n

ji p q k kn

pN

θθ

θ Θ

β αγ−

∈∈ =

= |

∑ ∑∏ zz xx

1( , )i ik kp t−|xx xx

,( )i nkω

,( )i nkxx, ,( , )i n i n

k kωxx

1( )ik kp −|xx ZZ

1( )ik kp −|xx ZZ

( )i ik kp |zz xx

10

( ) ( ) ( ),km

ji i ik k ji k k kk

j

p p pα β −=

= | |∑xx ZZ zz xx xx ZZ

d( )

( , ) 1( ) ( ) .

ji

k

ji

m p q q qp q kk k k kp p

θ Θ

θθ

β

αγ

∈

−∈ −

=

| |

∑

∏ ∫ zz xx xx ZZ xx

(15)

α is a normalizing factor, such that all weights sumto one.

4 PERCEPTION AND TRACKING

A SICK LMS200 laser range finder was used assource of sensory data. The laser was programmedto transmit 361 measurements per scan, evenly dis-tributed in a 180 degrees angular range (0.5 de-grees resolution).

Constant resolution grid representations are usedin several processing steps for abstracting the sen-sor measurements to the filter. Space is divided in-to square cells with 10 cm side.

A. Grid Models

The occupation grids contains an estimate of theoccupation probability of each cell, in the currentinstant. This probability is estimated as follows foreach cell (x, y)

(16)

where Nxy represents the number of points in thelaser measurement vector that falls inside the (x, y)cell. x and y are the coordinates of the point ofthe cell that is closest to the origin. ψ is an adjust-ment factor and Mxy is the maximum number of

xyxy

xy

Np

Mψ=

, ,

0

( ).km

ji n i njik k k

j

pω α β=

= |∑ zz xx

42 AUTOMATIKA 46(2005) 1–2, 39–48


Fig. 4 Possible combinations of feature/object

Fig. 5 Possible values of feature/object combinations (βji)

laser points that cell (x, y) can receive. This maxi-mum number depends on the position of the cellwith respect to the laser and is calculated as fol-lows:

(17)

where the value 180 is due the laser angular reso-lution of 180 sensor points on the half laser aper-ture of 90 degrees. Mxy is used in the computationof other grids discussed below. Using (16) and (17)a probability distribution P(occupiedx,y |Z(k)) is ob-tained.

The new occupation grid contains, the probabilityof each cell being currently occupied but not occu-pied in the previous iteration. This grid is construc-ted by confronting the actual occupation grid withthe grid of the last iteration as follows

(18)

where is the probability that

an object has moved into cell (x, y) [12].

The occlusion grid has in each cell the probabili-ty of that cell being occluded by a object (movingor not) relative to the laser. This grid is usedfor direct treatment of occluded objects duringtracking. Each point of the laser measurementvector is analyzed and the cells containing segmentsof the point's occlusion line are updated onthe occlusion grid. For each cell (x, y) a probability

is obtained.

B. Segmentation

To identify the mobile objects in the laser moni-toring space we use a segmentation process in Car-tesian coordinates. This process consists of grou-ping the neighboring points, in such a way that wecan perform a segment, according to a used crite-rion. In this way we can work with sets of pointsinstead of the laser measurements. A segment istherefore a set o adjacent laser measurements valu-es close to each other. We stipulate that two con-secutive points belong to the same segment if thedistance between these points is lower that a thres-hold value D, given by

(19)

where are the coordinates of point j,

and are the coordinates of the point

j + 1 of the laser measurements vector. The D valueis selected in order to adjust the process perfor-mance.

and1 1j jk kx y+ +

andj jk kx y

1 12 2 2( ) ( ) ,j j j jkk k k kx x y y D+ +− + − =

,( ( ))kx yP occlusion k| ZZ

,( ( ), ( 1))kx yP new k k| −ZZ ZZ

, ,

,

( ( ), ( 1)) ( ( ))

(1 ( ( 1))),

kx y x y

x y

P new k k P occupied k

P occupied k

| − = | ×

× − | −

ZZ ZZ ZZ

ZZ

180,

1xyM

x y=

+ +

Figure 6 illustrates an example of the segmenta-tion process. There are eight different segments(that are distinguished with red outline). Noticethat the isolated points are ignored (that are distin-guished with green outline). These points do notactually correspond to any object existing in theenvironment. They are noisy laser measurementsthat result from the laser incidence in a transitionarea, and from the fact that, in this way, the corre-sponding reflected laser beam becomes diffused andwith reduced power.

C. Mobile objects

The final step in the perception involves the fu-sion of segmentation results and the grid results toidentify the moving objects. They are identified byconfronting the minimum grid and the new occupa-tion grid with the segmentation results. After thisprocess we have a new grid with probability distribu-tion , that indicates the cells

occupied by moving objects. This new grid is theinput of the SJPDAFs method.

D. Tracking

With SJPDAF theory, it is possible to track vari-ous mobile objects using a particle filter for eachobject. The SJPDAF algorithm returns the assign-ment probabilities that permit each feature be as-sociated to one filter.

When a new object steps into the laser perceptu-al range (PR), a new filter must be initialized andstarts to track that object. To initialize the new fil-ter around the new object, it is checked which fea-ture does not have a filter tracking it. This verifica-tion is performed by analyzing the distances be-tween the mean points of the features, and themean points of the particles of the filters. A newfilter is associated to feature

(20)

where mtj and mli are the mean points of the fea-tures, and the mean points of the particles of the

newFeatures Filters

argmax min ( , ),j ij i

j d mt ml∈ ∈

=

,( ( ))x yP new object k| ZZ

AUTOMATIKA 46(2005) 1–2, 39–48 43


Fig. 6 Laser range scan where exists eight segments

filters. Particles of the new filter are spread aroundmtjnew. Each particle is a vector containing position,orientation and velocity components: (x, y, θ, ϑ).This process is iterated until the number of filtersequals the number of features.

When an object steps out of the laser PR, oneof the active filters must be closed. For each filter,i, an accumulated discounted average of theparticles weights sum before the normalization step

is maintained:

(21)

where η is a constant that regulates the inertia ofthe process. For each of the T − mk filters with theleast value of there is a variable ρi initialized to Max_Cont when the feature is no longer percei-ved on the sensor data, and ρi is decremented whilethis situation persists. When ρi attains 0, the filteris deactivated. If the feature reappears in the mea-surements while the filter is in this transition phase,then the filter is not deactivated, and variable ρi isre-initialized to Max_Cont. These variables permitthat a filter will not be initialized just because atemporary individual feature, or a filter will not beclosed just because a feature disappears temporarily.

The overall tracking algorithm can be divided in-to five parts: (i) Prediction step; (ii) Correction step;(iii) Assignment probabilities calculation using (14);(iv) Particles weights update using (15); (v) Resam-pling step.

The following prediction model was used on theparticle filters is described by equations (22)–(25):

(22)

(23)

where h is the sample period. Direction parameterθ and velocity parameter ϑ, at every time index,are affected by an independent Gaussian noise. θand ϑ equations are

(24)

(25)

where n1 and n2 are zero-mean Gaussian randomprocesses with unit variance. Factors v1 and v2 ad-just the variance for orientation and velocity, re-spectively.

In the correction step, the calculation of mea-surements probabilities given the state vector is per-formed. In this step is calculated.

This is needed to compute the assignment probabi-lities βji. For j = 0, we have the probability of theobject not being detected:

,( ( ) )i nj kp k |zz xx

1 1 1

1 2 2

,

,

i ik k

i ik k

v n

v n

θ θ

ϑ ϑ

+

+

= +

= +

1

1

cos( ),

sin( ),

i i i i ik k k k k

i i i i ik k k k k

x x x h

y y y h

ϑ θ

ϑ θ

+

+

= +

= +

ikΛ

^

1(1 ) ,i i ik k kΛ η Λ ηΛ−= − +^ ^

ikΛ

ikΛ

^

(26)

where represents

the probability of particle n from the filter i be oc-cluded at time k, as obtained from occlusion grid(see also Sec. 4-A). for j = 1, . . . , k, isobtained from the grid that represents cells occu-

pied by moving object j (cf. Sec. 4-A):

(27)

where is the pro-

bability that particle n of filter i is on a moving object position.

5 SYSTEM ARCHITECTURE

This section is divided in two subsections: one todescribe the real-time tracking architecure in thePC, and the other to describe the CAN communi-cation between the PC and the Laser range finder.

A. Real-Time Tracking Architecture

Figure 7 depicts the architecture of mobile ob-jects tracking. The first step is to get the measure-ments vector from laser range finder. With this vec-tor an »Occupation Grid« is built and, at the sametime, this vector is decomposed in several segments,with the criteria described in Section 4. The seg-ments that correspond to minima in the laser dis-tances vector are used to build the spatial distribu-tion of the probability of features which is calledthe »Features map«. With the aid of the occupationgrid from the last iteration process, we can deter-mine which grid elements are newly occupied inthe laser monitored space. In this way we can cre-ate a »New Occupation Grid«. With the conjugation

cell i.e. ,

, ( , ), ( )i n

i n kx y p movobject∈ xxxx

,,

,

( ( ) ) ( )

( ( )),

i ni nj k k

kx y

p k p movobject

P movobject k

| = =

= |

xxzz xx

ZZ

,( ( ) )i nj kp k |zz xx

cell i.e. ,

, ( , ), ( )i n

i n kx y p occluded∈ xxxx

,,0

,

( ( ) ) ( )

( ( )),

i ni nk k

kx y

p k p occluded

P occlusion k

| = =

= |

xxzz xx

ZZ

44 AUTOMATIKA 46(2005) 1–2, 39–48


Fig. 7 Tracking architecture diagram

of these Feature Map and the New OccupancyGrid maps we create another grid that only con-tains the mobile features. Figure 8 illustrates thisprocess of determining the mobile objects. Oncewe have the separated mobile segments we can up-date the probability of association between mobilefeature and particle filters, using equation (14), toperform tracking of these mobile features.

B. CAN Architecture

The communication between the PC running thealgorithms for tracking moving objects, and theSICK LMS200 laser range scanner was establishedthrough a CAN (Controller Area Network) bus.This bus permits the accommodation of the 500Kbaud sensor data transfer rate of the LMS200.

Since the communication with the laser is per-formed with a RS422 serial channel, a microcon-troller-based interface node was implemented forbridging the laser sensor and PC through the CANbus (Figure 9). The microcontroller (µC) interfaceimplements an algorithm to convert CAN formatto serial format and vice versa. A protocol was de-veloped to communicate the laser telegrams (mes-sages) between the µC and the PC through theCAN. For protocol management, a header is an-nexed to the telegram sent from the PC to thelaser. The header and telegram form what we calla CAN packet (Figure 10). Clearly, this header willnot be sent to laser. This header consist in fourfields (1 byte each): (1) number of CAN messagesinto which the packet was subdivided for sendingfrom the PC to the µC (the CAN protocol definesa maximum message length of 8 data bytes) (Figure10); (2) operation code, which permits the µC toidentify the type of the telegram received, so it canproceed with the appropriate actions in the currentoperation context. (3) communication velocity of theinterface serial port, that permits µC to know whento change velocity of its serial port; (4) telegram sizeto permit the µC to extract the annexed telegramfrom the received message, so it can be sent to thelaser. To understand the importance of this proto-

col notice that the laser can function in continuousmode (measurements sending) or in request mode,making the required µC processing to be complete-ly different. On the other hand, if the sent telegramchanges the laser communication velocity, the µChas to reconfigure its own serial port speed, forcommunication to be continued.

In the laser to PC way, the µC only divides thereceived telegrams from laser in CAN messages (upto 8 bytes) and puts it on the CAN bus for PC re-assembly. The overall effect of this real-time com-munication interface is to permit a totally transpar-ent communication between the PC and the laser,i.e. the communication is performed as if the laserwas directly connected to the PC.

6 EXPERIMENTAL RESULTS

All experiments were performed in an indoor en-vironment where the scenery could be changedfrom test to test. The laser was placed at a heightof 80 cm. The tracking algorithm was implementedwith a frequency of 5 iterations per second. Theused PC has a Pentium III processor at 1 GHz and512 Mbytes of RAM. The linux operating (kernel2.24.25) and the Real-Time Application Interface(RTAI 3.0) were used.

A. Tracking Multiple Objects

In this test several people are tracked in orderto demonstrate the tracking algorithm.

The sequence of Figure 11 starts with a singleperson being tracked. At the third displayed timesample (row) a new object enters the laser PR, a

AUTOMATIKA 46(2005) 1–2, 39–48 45


Fig. 8 Mobile features determination

Fig. 9 Communication between PC and laser by the microcontroller

Fig. 10 Packet communication from PC to µC through CAN mes-

sages

corresponding feature was detected, and a new fil-ter has been started and is already tracking thatperson. However its particles do not appear in theimage according to the count variable explained inSec. 4. In the last row the new filter is already rep-resented. From row four to row five we can seethat the filters interchange their features. This is aresult of SJPDAF utilization and by random filtersevolution. Figure 12 presents a situation, observedlater on the same experiment, where there arethree people in the PR. It can be observed in thisrow that the particles of the three active filtershave lower diversity. One fact that could justify thisoccurrence is the used Systematic Resampling thatlowers the particles diversity and can provoke thedegeneration of the filters. When the tracked mo-bile object has abrupt motion and a few number ofparticles are predicted inside the feature, the re-sampling step makes many equal copies. Althoughthe number of filter particles is always the same,

low samples diversity is due to particles being su-perimposed and have the same direction and velo-city (x, y, θ, ϑ). Anyway, in subsequent iterations,the randomness introduced in the filter predictionphase, mitigates this problem, and samples diversi-ty is restored. However, there are rare situationsin which the particles of a filter have irrelevantweights and the filter gets a high degenerate level.In such situations the filter has difficulties to trackthe objects and it is necessary to reinitialize the fil-ter.

A second fact that affects the operation of thefilters is that the tracked mobile objects are not al-ways detected, as in the case that a person invertshis/her trajectory, thus staying immobile for someinstants. In such situations the new occupation griddoes not have a corresponding feature present, andthe probability is affected, which in turncauses an incorrect evolution of particles. Figure 13corresponds to this test and specifies the numberof features, active filters, and combinations of validassociations made in assignment probabilities com-putation referenced in Eq. (14). At the iterationscorresponding to time instant of Figure 12 (insidethe [140, 200] time interval) an instability in thenumber of mobile features can be observed. How-ever, these inverted peaks are quick and do not in-fluence the number of active filters due the inertiacreated by the filters closing algorithm (Sec. 4) andin this way each feature always has a filter trackingit. Tracking three people the number of combina-tions of valid associations is 64. This number growsto 7776 when five people are tracked.

,( )j i nk kp |zz xx

46 AUTOMATIKA 46(2005) 1–2, 39–48


Fig. 11 Tracking two people

Fig. 12 Tracking three people

Fig. 13 Number of features detected, number of active filters andnumber of associations during one experiment of tracking five

people

Figure 14 shows another set of data related tothe same experiment. It represents the laser scanvector, and the middle point of each particle filter.In the left hand image of each row, all values fromthe laser vector are represented by vertical lines.The minimum values in this vector, which arecaused by the presence of three mobile objects, areperfectly visible. Notice that there are 361 values

B. Occluded Objects

In order to demonstrate the advantage of han-dling the occlusion of mobile objects, Figure 15presents a sequence where a person moves behinda static object. In this situation, the particles of thefilter disperse in a random way consistently withthe temporary lack of sensory data and correspon-ding increase of filter state uncertainty. In the firstrow the filter tracks the mobile object as usual. Inthe second and third samples, since the feature isnot detected in the sensory data, the particles ofthe filter are assigned to the occlusion feature, andpredicted over the corresponding area. Once thefeature reappears in the range measurements, thefilter rearranges its particles in order to track thefeature again with lower uncertainty, as show in row(time-stamp) four.

7 CONCLUSIONS

This paper has presented probabilistic methodsfor tracking multiple moving objects, using sensorydata from a laser range scanner. The developed sys-tem uses SJPDAF to handle the data associationproblem, and a particle filter to individually trackeach object. Particle filters have the advantage thatthey can be applied to nonlinear and non-Gaussiansystems. A method was also presented for percep-tion of moving objects, and separate moving ob-jects from all the static objects existing in the envi-ronment, based in probability occupancy grids andobstacle segmentation. The paper also presented arealtime architecture that was developed to link allsystem components and implement the tracking al-gorithms. The architecture permits a completelytransparent communication between the laser sen-sor and the host computer. Future work includesimproving particle filter behavior regarding thetracking of highly abrupt motions.

REFERENCES

[1] R. Araújo, G. Gouveia, N. Santos, Learning Self-organizingMaps for Navigation in Dynamic Worlds. in Proc. IEEEInt. Conf. Robotics and Automation (ICRA), 2003, pp.1312–1317.

[2] A. Fod, A. Howard, M. J. Mataric, Laser-based People Tra-king. in Proc. IEEE Int. Conf. Robotics and Automation(ICRA), 2002, pp. 3024–3029.

[3] M. Pitt, N. Shephart, Filtering via Simulation: AuxiliaryParticle Filters. In J. Amer. Statist. Assoc., vol. 94, no.446, 1999, pp. 590–599.

[4] N. J. Gordon, D. J. Salmond, A. F. M. Smith, A Novel Ap-proach to Nonlinear/non-gaussian State Estimation. In IEEProcedings F, vol. 2, 1993, pp. 107–113.

[5] Y. Bar-Shalom, T. E. Fortmann, Traking and Data Associa-tion, Ser. Mathematics in Science and Engeneering. Aca-demic Press, 1988, vol. 179.

[6] F. Gustafsson, F. Gunnarsson, N. Bergman, U. Forssell, J.Jansson, R. Karlsson, P.-J. Nordlund, Particle Filters for

AUTOMATIKA 46(2005) 1–2, 39–48 47


Fig. 14 Scan measurements and particle filter middle point

Fig. 15 Sequence of a occlusion situation

in each laser vector. Three local minima sets ofpoints (clusters) can be seen in each row, except inthe second row where temporarily there are onlytwo local minima but the filter management opera-tions (Sec. 4) retain three filters.

48 AUTOMATIKA 46(2005) 1–2, 39–48


Positioning, Navigation and Tracking. IEEE Trans. SignalProcessing, vol. 50, no. 2, pp. 425–437, 2002.

[7] M. S. Arulampalam, S. Maskell, N. Gordon, T. Clapp, A Tu-torial on Particle Filters for Online Nonlinear/non-gaussi-an Bayesian Tracking. IEEE Transactions on Signal Proces-sing, vol. 50, no. 2, pp. 174–188, Feb. 2002.

[8] A. Doucet, On Sequencial Monte Carlo Methods for Baye-sian Filtering. Dept. Eng., Univ. Cambridge, UK, Tech.Rep., 1998.

[9] R. C. J. S. Liu, Sequencial Monte Carlo Methods for Dyna-mical Systems. In J. Amer. Statist. Assoc., vol. 93, 1998,pp. 1032–1044.

[10] N. Bergmam, Recursive Bayesian Estimation: Navigationand Tracking Aplications. Ph.D. dissertation, LinköpingUniv., 1999.

[11] J. D. Hol, Resampling in particle filters. Institutionen förSystemteknik, Linköpings Univ., Tech. Rep., May 2004.

[12] D. Schulz, D. Fox, W. Burgard, A. B. Cremers, People Track-ing with a Mobile Robot Using Sample-based Joint Pro-babilistic Data Association Filters. In International Journalof Robotics Research (IJRR), vol. 22, no. 2, 2003, pp. 99–116.

Pra}enje vi{e gibaju}ih objekata u stvarnome vremenu primjenom ~esti~nih filtara i vjerojatnosnogpridruìvanja podataka. Mobilni roboti i mobilna vozila sve se vi{e koriste u dinami~kim okruènjima popunje-nim ljudima i drugim gibaju}im objektima. U tom je smislu va`no pra}enje gibaju}ih objekata u neposrednomokruènju kako bi se izbjegavale prepreke i planiralo gibanje. U ovome je radu predloèna metoda detekcije ipra}enja vi{e gibaju}ih objekata primjenom ~esti~nih filtara za estimaciju stanja objekata i filtara za zdruènovjerojatnosno pridruìvanje uzorkovanih podataka kojima se povezuju zna~ajke detektirane u mjernim podacima sodgovaraju}im filtrima. Izvedena je nadzorna ljuska filtara za odgovaraju}u integraciju o~itanih zna~ajki. Ukratkoje opisana arhitektura implementiranog sustava pra}enja objekata u stvarnom vremenu. Prikazani eksperimentalnirezultati dobiveni primjenom laserskog senzora udaljenosti potvr|uju izvedivost i u~inkovitost predloènog sustava.

Klju~ne rije~i: mobilni roboti, ~esti~ni filtri, pra}enje u stvarnome vremenu, vjerojatnosno pridruìvanje podataka

AUTHORS’ ADDRESSES

António AlmeidaJorge AlmeidaRui Araújo

ISR – Institute for Systems and RoboticsDepartment of Electrical and Computer EngineeringUniversity of CoimbraP-3030-290 Coimbra, Portugal

Received: 2005-12-01

real-time tracking of multiple moving objects using...

Documents