Toro: tracking and observing robot

The present invention provides a method for tracking entities, such as people, in an environment over long time periods. A region-based model is generated to model beliefs about entity locations. Each region corresponds to a discrete area representing a location where an entity is likely to be found. Each region includes one or more positions which more precisely specify the location of an entity within the region so that the region defines a probability distribution of the entity residing at different positions within the region. A region-based particle filtering method is applied to entities within the regions so that the probability distribution of each region is updated to indicate the likelihood of the entity residing in a particular region as the entity moves.

FIELD OF THE INVENTION

This invention relates generally to entity tracking, and more particularly to a system and method for using a region-based hierarchical model to locate entities.

BACKGROUND OF THE INVENTION

Mobile robots have been deployed in multiple types of environments including one or more people, such as offices and hospitals. Such robots can provide assistance in home, office and medical environments, but need to verbally and physically interact with people in the environment. For example, a robot can verbally provide information or physically retrieve an object to aid one or more people in the environment.

However, to effectively interact with people and perform tasks, it is desirable for a robot to model the location of people and other task-related entities in its surrounding environment. Tracking entities, such as people, allows the robot to plan efficient sequences of actions for accomplishing given tasks. Conventional methods for entity tracking have concentrated on tracking movement of people in the immediate vicinity of the robot over short time periods using lasers and radio frequency identification (“RFID”) sensors.

These conventional methods, however, focus on short-term entity tracking and are unable to track an entity after the entity leaves the robot's field of view. Further, the use of lasers prevents these conventional methods from differentiating people from other objects or obstacles in the environment. Additionally, large-scale use of RFID sensors is impractical. Hence, conventional methods merely allow robots to track entities for short time intervals.

Thus, what is needed is a system and method for tracking entities over extended time intervals.

SUMMARY OF THE INVENTION

The present invention provides a system and method for tracking entities, such as people, in an environment over long time periods. A plurality of regions specifying discrete areas where entities can reside are generated. Each region defines one or more parametres, such as positions included within the region, which more precisely specify the location of an entity within the region. Thus, a region defines a probability distribution of the likelihood of an entity residing at different positions within the region. For example, the positions are Cartesian coordinates and the region specifies a Gaussian distribution with a two-dimensional mean vector and covariance matrix describing the likelihood of the entity residing at positions within the region. As an image capture device obtains image data associated with the entity, the probability distributions of each region are updated responsive to changes in the entity position. Thus, as the entity moves within or between regions, the probability distribution of each region is modified to represent the current likelihood of the entity being located within a region.

DETAILED DESCRIPTION OF THE INVENTION

A preferred embodiment of the present invention is now described with reference to the Figures where like reference numbers indicate identical or functionally similar elements. Also in the Figures, the left most digits of each reference number correspond to the Figure in which the reference number is first used.

Certain aspects of the present invention include process steps and instructions described herein in the form of an algorithm. It should be noted that the process steps and instructions of the present invention could be embodied in software, firmware or hardware, and when embodied in software, could be downloaded to reside on and be operated from different platforms used by a variety of operating systems.

FIG. 1is an illustration of a computing system102in which one embodiment of the present invention may operate. The computing system102includes a computing device100and an image capture device105. The computing device100comprises a processor110, an input device120, an output device130and a memory140. In an embodiment, the computing device100further comprises a communication module150including transceivers or connectors.

An image capture device105, such as a video camera, video capture device or another device capable of electronically capturing movement, captures data, such as image or other positional data, describing the movement of an entity, such as a person, and transmits the captured data to the computing device100. In an embodiment, the image capture device105is a combined depth/vision camera apparatus, such as the CSEM depth camera, where the depth apparatus captures the presence and coordinates of entities in the visual field of the image capture module105and the vision apparatus distinguishes between different entities observed by the image capture device105. In one embodiment, the image capture device105distinguishes people from other types of detected entities by using depth information from the depth apparatus, or communicating the depth information to the processor110, to build horizontal and vertical contour lines of continuous depth. The horizontal and vertical contour lines are used as features for a person recognizer which is trained using logistic regression to return the maximum likelihood estimate of the coordinates of a person observed by the image capture device105. Although described above with reference to detecting a person, data from the depth apparatus may also be used to recognize other types of entities. In an embodiment, the vision apparatus is used to distinguish between individual people and objects, for example, by using clothing color to distinguish between different people. The captured image data is communicated from the image capture device105to the computing device110, which processes the captured image data to track one or more entities.

The processor110processes data signals and may comprise various computing architectures including a complex instruction set computer (CISC) architecture, a reduced instruction set computer (RISC) architecture, or an architecture implementing a combination of instruction sets. Although only a single processor is shown inFIG. 1, multiple processors may be included. The processor110comprises an arithmetic logic unit, a microprocessor, a general purpose computer, or some other information appliance equipped to transmit, receive and process electronic data signals from the memory140, the input device120, the output device130or the communication module150.

The input device120is any device configured to provide user input to the computing device100such as, a cursor controller or a keyboard. In one embodiment, the input device120can include an alphanumeric input device, such as a QWERTY keyboard, a key pad or representations of such created on a touch screen, adapted to communicate information and/or command selections to processor110or memory140. In another embodiment, the input device120is a user input device equipped to communicate positional data as well as command selections to processor110such as a joystick, a mouse, a trackball, a stylus, a pen, a touch screen, cursor direction keys or other mechanisms to cause movement adjustment of an image.

The output device130represents any device equipped to display electronic images and data as described herein. Output device130may be, for example, an organic light emitting diode display (OLED), liquid crystal display (LCD), cathode ray tube (CRT) display, or any other similarly equipped display device, screen or monitor. In one embodiment, output device120is equipped with a touch screen in which a touch-sensitive, transparent panel covers the screen of output device130.

The memory140stores instructions and/or data that may be executed by processor110. The instructions and/or data may comprise code for performing any and/or all of the techniques described herein. Memory140may be a dynamic random access memory (DRAM) device, a static random access memory (SRAM) device, Flash RAM or other non-volatile storage device, combinations of the above, or some other memory device known in the art. The memory140comprises a region generation module142, an observation module144and a tracking module146, and is adapted to communicate with the processor110, the input device120, the output device130and/or the communication module150.

The region generation module142includes information describing generation and modification of a region-based model of entity location and computing system100position. In one embodiment, the region generation model describes a Dynamic Bayesian Network (DBN) which initially selects a region, and then selects a position, such as a Cartesian coordinate, within the region associated with an entity. For example, the region is a discrete variable and the position is a linear Gaussian conditional on the discrete variable. In an embodiment, the generated regions correspond to locations in an environment where people typically stop and stay for an extended period of time, such as the area in front of a desk, the area in front of a printer, the area in front of a water cooler or similar locations. In addition to describing regions and positions, the region generation module142also includes a transition model describing motion of an entity between regions and between positions within a region. In an embodiment, the transition model included in the region generation module142models the behavior of position variables within a region using Brownian motion assuming a Gaussian kernel density with a small base relative to the scale of the region around the current position. Similarly, when an entity transitions from a first region to a second region, the transition model determines the entity position in the second region using parameters associated with the second region.

The observation module144includes instructions for determining the probability of observing an entity and localizing the position of the computing system102. To determine the probability of entity observation, the observation module144includes data for calculating observation probability for different cases: when an entity is observed in the visual field of the image capture device105and when an entity is not observed in the visual field of the image capture device105. Calculation of entity observation probability performed by the observation module144is further described below in conjunction withFIG. 4.

Additionally, the observation module144includes data for determining the position of the computing system102. In one embodiment, the observation module144includes a localization module145which includes the instructions for determining the position of the computing system102. For example, the observation module144describes how to localize the position of a robot including the computing system102. In one embodiment, the observation module144describes a modified form of Monte Carlo localization where particle filters maintaining a guess of the path taken by the computing system102through an environment, such as the FastSLAM method described in “A Factored Solution to the Simultaneous Localization and Mapping Problem” by M. Montemerlo et al. in Proceedings of the AAAI National Conference on Artificial Intelligence, which is incorporated by reference herein its entirety. A localization method included in the observation module is further described below in conjunction withFIG. 2.

The tracking module146includes data describing how to track the posterior distribution of entity locations. In one embodiment, the tracking module146describes a region based particle filter where each particle includes a region variable and a position variable. The tracking module146also computes a weight for each particle indicating the probability of the current observation based on the hypothesis that the entity is at a position and region specified by a particle. Each region is then resampled by the tracking module146so that the total weight of a specific region equals the total weight of the particles included in the specific region to keep the probability mass of each region tightly controlled. By using this particle-specific weighting, the transition model described by the region generation module142regulates modeling of transitions of entities between regions so that the discrete probabilities of an entity being located in a specific region are accurate, even if there are errors in the distribution of positions within the specific region. A method of tracking entities by the tracking module146is further described in conjunction withFIG. 5.

In an embodiment, the computing device100further comprises a communication module150which links the computing device100to a network (not shown), or to other computing devices100. The network may comprise a local area network (LAN), a wide area network (WAN) (e.g. the Internet), and/or any other interconnected data path across which multiple devices man communicate. In one embodiment, the communication module150is a conventional connection, such as USB, IEEE 1394 or Ethernet, to other computing devices100for distribution of files and information. In another embodiment, the communication module150is a conventional type of transceiver, such as for infrared communication, IEEE 802.11a/b/g/n (or WiFi) communication, Bluetooth® communication, 3G communication, IEEE 802.16 (or WiMax) communication, or radio frequency communication.

It should be apparent to one skilled in the art that computing device100may include more or less components than those shown inFIG. 1without departing from the spirit and scope of the present invention. For example, computing device100may include additional memory, such as, for example, a first or second level cache, or one or more application specific integrated circuits (ASICs). Similarly, computing device100may include additional input or output devices. In some embodiments of the present invention one or more of the components (110,120,130,140,142,144,146) can be positioned in close proximity to each other while in other embodiments these components can be positioned in geographically distant locations. For example the units in memory140can be programs capable of being executed by one or more processors110located in separate computing devices100.

FIG. 2is a flowchart illustrating a method200for region-based entity tracking according to one embodiment of the present invention. In an embodiment, the steps of the method200are implemented by the microprocessor110executing software or firmware instructions that cause the described actions. Those of skill in the art will recognize that one or more steps of the method may be implemented in embodiments of hardware and/or software or combinations thereof. For example, instructions for performing the described actions are embodied or stored within a computer readable medium. Furthermore, those of skill in the art will recognize that other embodiments can perform the steps ofFIG. 2in different orders. Moreover, other embodiments can include different and/or additional steps than the ones described here.

Initially, a plurality of regions are generated210by the region generation module142. In an embodiment, the plurality of regions form a Dynamic Bayesian Network (DBN) having a layer of discrete region variables representing common locations where entities, such as people, reside, such as a location proximate to a desk or a location proximate to a water cooler. Each region specifies a probability distribution over the precise position of an entity within a region. In an embodiment, a two-dimensional mean vector, ptr and a 2×2 covariance matrix Σrare associated with each region. Each region includes a plurality of position variables which more precisely specify the location of an entity. For example, the position variables comprise Cartesian coordinates, such as (xt,yt) specifying the location of an entity within a region. Hence, the position, Xt=(xt,yt) for each entity in a region, R, is drawn from the Gaussian distribution having parameters (μr, Σr), so that Xt˜N(μrt, Σrt). Additionally, in the generated DBN, the position of the computing system102is defined by an (x,y,θ) tuple, where θ indicates the direction the image capture device105is facing, which allows translation of observed data into absolute coordinates. For example, if the computing system102is included in a robot, the robot pose is specified by the (x,y,θ) tuple. For example, the regions

As the plurality of regions are generated210, a corresponding transition model is also generated. The transition model represents movement by an entity within a region by moving from position to position and movement by an entity between regions. In an embodiment, the dynamic behavior of movement between positions in a region is modeled by Brownian motion. For example, a Gaussian kernel density with a small base centered at the current position of an entity is generated. When the DBN is updated, a new position of the entity is chosen based on the Gaussian kernel density and accepted according to the Gaussian distribution associated with the current region (e.g., the Gaussian distribution having parameters (μr, Σr)). Hence, the equilibrium distribution of the position data converges on a slow Markov chain back to the region's prior distribution. This distribution models the expected behavior of an unobserved entity, namely that the entity remains in the current region, but if unobserved can occupy one of many positions in the region as described by the region's prior probability distribution.

The transition model also describes transitions of an entity between different regions. At a given time, there is a small probability associated with an entity transitioning from a first region to a second region. When an entity transitions from a first region to a second region, the position of the entity in the second region is drawn from the Gaussian prior distribution for the second region. For example, an entity has position Xtin region r at time t, and transitions into region r′ at time t+1. The entity position in region r′ at time t+1 is modeled using the Gaussian distribution associated with region r′ at time t.

After the regions and transition model have been generated210, the region associated with a particle identifying an entity is determined220and the position of the particle within the determined region is subsequently determined230. For example, the transition model generated210in addition to the plurality of regions is applied to one or more particles each representing an entity. If application of the transition model indicates that a particle has moved from a first region to a second region, the probability distribution associated with the second region is used to determine230the particle's new position in the second region.

An observation model is then applied240to the particle associated with an entity to determine the probability of observing the particle in different regions and different positions within a region. In applying240the observation model, the observation module144determines whether the entity has been observed by the image capture device105and calculates the probability of observing the entity in each of the generated regions. As the observation model is applied, the observation module144also determines the position of the computing system102, as this position affects the subsequent entity tracking by the tracking module146to update250each region. An example of this observation and probability calculation is further described in conjunction withFIG. 4.

Responsive to application of the observation model, the tracking module146updates250weights associated with one or more particles within each of the generated regions. The tracking module146modifies the probability distribution associated with each region responsive to application of the observation module so that the probability distribution of each region reflects the likelihood of an entity currently being in a particular region. Each region is updated250so that the total weight of the particles in a region equals the total weight of a region to tightly regulate the probability mass of each region. Hence, the transition model of the generated regions regulates modification of each region's probability mass so that the probability mass of each discrete region accurately indicates the likelihood of finding the entity in each region.

FIG. 3is an example of a region-based representation of an environment according to one embodiment of the present invention. In an embodiment, the computing system102is initially configured with the floor plan of the environment surrounding the computing system102. The floor plan describes the location of different objects, such as walls, doors, desks, chairs and other objects. For example, the floor plan of an office describing the relative location of different rooms and objects in each room building is initially stored in the region generation module142of the computing system102. In addition to initially receiving a description of the surrounding environment, the computing system102also receives data describing the possible locations of entities in the environment. For example, the computing system102receives data indicating approximations of the likelihood of entities being proximate to different objects in the environment. In one embodiment, these initial approximations assign an equal likelihood of an entity being proximate to various objects in the environment. Alternatively, the initial approximations assign different likelihoods of an entity being proximate to different objects based on prior data describing similar environments or prior data describing the current environment.

In the example shown inFIG. 3, a first object305A and a second object305B from the initial description of the computing system102environment are shown for clarity. In other embodiments, more or fewer objects are included in the description of the environment surrounding the computing system102. After the region generation module142identifies the first object305A and the second object305B from the received environment data, a region is generated associated with each object. Hence, a first region310A is generated and associated with the first object305A and a second region310B is generated and associated with the second object305B. Each of the regions defines a distribution over the position of an entity proximate to the object. Hence, the first region310A describes a probability distribution of an entity being proximate to the first object305A and the second region310B indicates a probability distribution of an entity being proximate to the second object305A. A two-dimensional mean vector, μrand a 2×2 covariance matrix Σrare associated with each region and specify the characteristics of a Gaussian distribution associated with each region. The region-specific Gaussian distribution is used to determine the position of an entity with the region.

While the regions define regions where an entity is likely to reside, a position within the region more precisely indicates the location of an object within a region. Parameters of the position are dependent on characteristics of the region, such as the two-dimensional mean vector, μrand the 2×2 covariance matrix Σrassociated with each region.FIG. 3shows the position of an entity within the first region310A and the second region310B. The first position320A indicates the location of an entity within the first region310A. The first position320A comprises a variable drawn from the Gaussian distribution associated with the first region31OA, so the two-dimensional mean vector, μrand a 2×2 covariance matrix Σrassociated with the first region310A. Similarly, the second position320B comprises a variable drawn from the Gaussian distribution associated with the second region310B, so the two-dimensional mean vector, μrand a 2×2 covariance matrix Σrassociated with the second region310B.

In one embodiment, the region-based representation of entity location is implemented using a Dynamic Bayesian Network (DBN), which allows division of entity location information into two parts—one for modeling and another for decision-making. By using both a region and a position within a region to model entity position, entity transitions occurring at different time scales are separated into different layers of the entity position model, improving entity tracking over extended time intervals. For example, movement within a specific region can be modeled using the Gaussian distribution associated with the specific region, simplifying entity tracking within a region. Additionally, region based information may simplify decision-making using information about the larger region which includes an entity rather than exact information about the precise location of an entity. Further, the use of discrete regions allows entity tracking to be performed on a per-region basis, allowing for more efficient and accurate tracking than a conventional particle filter.

FIG. 4is a flowchart illustrating a method for determining the probability of observing an entity in one or more regions according to one embodiment of the present invention. Hence,FIG. 4shows one embodiment of applying240an observation model to one or more regions responsive to data from the image capture device105.

Initially, the observation module144localizes405the position of the image capture device105. For example, if the image capture device105is included in a mobile robot, localization405determines the position of the mobile robot. Localizing405the position of the image capture module105improves the entity tracking further described in conjunction withFIG. 5by using conditional interdependencies from the localization process.

In one embodiment, a modified form of Monte Carlo Localization, such as the FastSLAM method described in “A Factored Solution to the Simultaneous Localization and Mapping Problem” by M. Montemerlo et al. in Proceedings of the AAAI National Conference on Artificial Intelligence, which is incorporated by reference herein in its entirety, allows particle filters to be applied to localization. In an embodiment where the FastSLAM localization method is applied, a particle associated with an entity maintains an approximation of a path taken by the image capture device105through the environment. Hence, the set of particles at a particular time t is represented by:
St={st,[m]}m={(s1[m],s2[m], . . . ,st[m])}m
Where the superscript [m] identifies the mthparticle and the superscript t identifies the set of variables from time1to time t. St is computed recursively from the set of particles at the prior time interval, St−1using a Bootstrap algorithm, such as the algorighm described in “A Factored Solution to the Simultaneous Localization and Mapping Problem” by M. Montemerlo et al. in Proceedings of the AAAI National Conference on Artificial Intelligence, which is incorporated by reference herein in its entirety. In an embodiment, a candidate pose at time t for each particle st−1,mis generated from the probabilistic motion model:
qt[m]˜p(·|ut,st−1[m])
Where utis the control at time t and the new pose qt[m]is appended to the set of poses included in st−1,mand the resulting particles are stored in a temporary set T. Each particle in the temporary set T is weighed by an importance factor calculated using:

wi[m]=p⁡(qt,[m]❘zt,ut)p⁡(qt,[m]❘zt-1,ut)
Where ztdenotes the observations made from time1to the current time, t. Stis then computed by sampling from the temporary set T weighted by the importance factors. Thus, if St−1is distributed according to p(st−1,[m]|zt−1,ut−1) then Stis drawn from p(st,[m]|zt,ut). The set of particles at the current time can be calculated using the pose estimate from the prior time (st−1) without additional information, conserving storage resources by allowing pose estimates from earlier times to be discarded.

The observation module144then determines410whether an entity has been observed by the image capture device105. For example, depth information from the image capture device105is used to form horizontal and vertical contour lines which are used as features for an entity recognizer trained using logistic regression. The entity recognizer generates a maximum likelihood estimate for coordinates of an entity, the observation module144determines410that an entity has been observed in the field of view of the image capture device105. If no coordinate estimates are returned, the observation module determines410that an entity has not been observed in the field of view of the image capture device105. In one embodiment, the observation module144modifies an observation variable Othaving the form (OtI, OtX) when determining410whether an entity has been observed. OtIis an indicator variable which has the value of 1 when an entity is observed by the image capture device105and a value of 0 when an entity is not observed by the image capture device105. If OtIindicates an entity is observed, then OtXis configured to the coordinates of the observed entity provided by the image capture device105.

To further clarify application240of the observation module when an entity is detected and when an entity is not detected, reference toFIGS. 6A and 6Bis made in addition toFIG. 4.FIG. 6Ais an example where an entity622is not within the field of view of the image capture device105whileFIG. 6Bis an example where an entity is within the field of view of the image capture device105. Thus,FIG. 6Aillustrates an example scenario where the observation module144determines410an entity is not observed by the image capture device105andFIG. 6Billustrates an example scenario where the observation module144determines410an entity is observed by the image capture device105.

If the observation module144determines410that an entity is not observed by the image capture device105, the probability distribution of not observing an entity in the current visual field630of the image capture device105is specified420. In an embodiment, the error associated with observing an entity622at a position Xt=(xt,yt) is specified420as a Gaussian distribution centered at Xtwith a covariance matrix of Σobs. The covariance matrix is rotated so that the principal axes of the Gaussian distribution are oriented along the normal and perpendicular from the image capture device105to the position of the entity622. Hence, the probability of not observing the entity in the current visual field630of the image capture device is specified420by:

Where A denotes a portion635of the region620including the entity622within the visual field630that is not occluded and N(x; Xt,) represents the Gaussian distribution having a mean of Xtand a covariance of Σobs. The specified probability distribution is used to calculate430the probability of the entity622being observed in a region, or portion of a region, outside of the visual field630of the image capture device105.

As shown inFIG. 6A, the visual field630may include one or more obstructions640, such as walls or other opaque objects, which prevent the image capture device105from viewing regions within the visual field630. For example, the image capture device105is unable to observe the portion of the visual field630region behind obstructions640. Hence, the obstructions640may prevent the image capture device105from viewing a portion of region620, or other regions, so the probability of not observing the entity in the current visual field630accounts for the areas of the region620including the entity622that are occluded. In an embodiment, the depth of the obstructions640is obtained by a range-finder, such as a laser range-finder, that communicates with the image capture device105or is included in the image capture device105. In one embodiment, to simplify calculation of the unobscured portion635of the region620, one dimensional integration of the points of unobscured portion635along the major principal axis perpendicular to the position of the image capture device105is performed.

If the observation module144determines410that an entity is observed by the image capture device105, the coordinates of the observed entity are identified425from the image capture device105. In one embodiment, the observation module144modifies the observation variable Otresponsive to the image capture device105observing an entity. For example, the OtIindicator variable of the observation variable is set to the value of 1 and the OtXcomponent of the observation variable configured to the coordinates of the observed entity provided by the image capture device105. For example, the O component is specified as Cartesian coordinates associated with the entity observed by the image capture device105. In the example shown inFIG. 6B, as an object652is within a region650included in the visual field630of the image capture device105, so the observation variable is set to Ot=(1, (ox, oy)).

So that calculation of the probability of the entity being within the field of view of the image capture device105is comparable to calculation of the probability of the entity not being within the field of view of the image capture device105, the minimum observation error is set427by the observation module144. Because evaluating the Gaussian distribution of the region at the coordinates of the observation makes the probability asymmetrical with respect to the calculation when the entity is not within the visual field, the minimum observation error is set427to ∥ox−xt, oy−yt∥. Where (ox, oy) are the coordinates of the observation point and (xt, yt) are the coordinates of the entity. By setting427the minimum observation error, the probability of observing the entity in the visual field430is calculated435using:
Pr(Ot=(1,(ox,oy))|Xt=(xt,yt))=(erf(|x−xt|>|oz−xt|,|y−yt|>|ou−yt|;(xt,yt),Σobs)
Where “erf” denotes the error function, which is the cumulative distribution function of a Gaussian distribution. Hence, the probability that the entity is in a region650within the visual field630of the image capture device105is area of the Gaussian distribution having a mean of Xtand a covariance of Σobsbounded by the observed position. As calculating435the probability of the entity being observed by the image capture device105involves calculating the cumulative distribution function described by the region650including the entity652, obstructions640in the visual field do not complicate the probability calculation435.

FIG. 5a flowchart illustrating a method for updating250region probabilities based on entity location according to one embodiment of the invention. In an embodiment, one or more particle filters are used to track the posterior distribution of entity locations. However, because of latency induced by the image capture module105, which may take up to 0.5 seconds to return a result, the one tracking module146or more particle filters are updated responsive to the availability of a new observation. By updating on a per-result scale, the availability of results provides a natural time scale for the Dynamic Bayesian Network used for entity tracking.

The weight of each particle is retrieved510from the observation module144. When the observation module144applies the observation model as described inFIG. 4, the calculated probability of the entity being inside the visual field of the image capture device105or the calculated probability of the entity being outside the visual field of the image capture device105is used as the weight of the particle associated with the entity. Each region is then resampled520and the new data is used to update530the probabilities of the entity being in different regions sot that the total weight of a specific region equals the total weight of the probability of the particles included in the region. Hence, the probability mass of each region, which indicates the likelihood that the particle is in a region, is tightly controlled so that the probabilities of the particle being in a region are highly accurate.

In one embodiment, the regions are resampled520when the empirical variance of the particle weights within a region is below a fixed threshold, beneficially resampling520when the weights are concentrated on a small set of particles within the region. In one embodiment, the regions are resmapled520using a sequential stochastic process, such as the process described in “Probabilistic Robotics” by Sebastian Thrun et al., which is incorporated by reference herein in its entirety, rather than by individually generating each new particle. The sequential stochastic process initially generates a random number t in the range [0, W−1], where W is the total weight of a region. Hence, the set of samples correspond to the weights t, t+W−1, t+2W−1, etc. Resampling520using the sequential stochastic process reduces the complexity of the resampling520from O(M log M) to O(M), where M denotes the number of particles.

The method for updating250region probabilities based on entity location according to one embodiment of the invention can be used to track multiple entities using separate particle filters for each entity. As the correlations between entity locations are induced by the position of the image capture device105, the location of two entities are conditionally independent of each other given the history of the movement of the image capture device105. Because the localization performed by the observation module144, each particle associated with an entity includes the history of image capture device105movement, allowing separate particle filters to be used to track separate entities. By using separate particle filters, the dependence of particle filter size to number of entities is reduced from exponential to linear.

FIG. 7is an illustration of the average completion time of automated delivery tasks according to one embodiment of the present invention. Performance of the region-based particle filter is compared to a conventional bootstrap filter without region representation over100simulations of a multiple entity delivery task.FIG. 7illustrates bootstrap filter performance710and region-based particle filter performance720by depicting the mean time to complete delivery tasks for all entities versus the number of entities. As shown inFIG. 7, the region-based particle filter completes delivery tasks in a shorter amount of time than the conventional bootstrap filter and the performance improvements afforded by the region-based particle filter are more pronounced as more entities are involved.

While particular embodiments and applications of the present invention have been illustrated and described herein, it is to be understood that the invention is not limited to the precise construction and components disclosed herein and that various modifications, changes, and variations may be made in the arrangement, operation, and details of the methods and apparatuses of the present invention without departing from the spirit and scope of the invention as it is defined in the appended claims.