Vehicle tracking

Abstract

The present invention relates to a method and system for accurately predicting future trajectories of observed objects in dense and ever-changing city environments. More particularly, the present invention relates to the use of prior trajectories extracted from mapping data to estimate the future movement of an observed object. As an example, an observed object may be a moving vehicle. Aspects and/or embodiments seek to provide a method and system for predicting future movements of a newly observed object, such as a vehicle, using motion prior data extracted from map data.

Claims

1. A method for estimating movements of an object, the method comprising; determining, by a system, initial state data of the object at a first time; determining, by the system, sequential trajectory data for one or more prior moving objects that intersected vicinity of the position of the object; estimating, by the system, future positions of the object, at a second time, based on the sequential trajectory data for the one or more prior moving objects; and constraining, by the system, the future positions of the object based on a comparison between the object and the one or more prior moving objects for which the sequential trajectory data intersects the vicinity of the position of the object, wherein the constrained future positions of the object are indicative of the estimated movement of the object at the second time.

2. The method of claim 1 wherein the initial state data of the object comprises a position, rotation and velocity in a 3D space.

3. The method of claim 1 wherein the sequential trajectory data is extracted from data used to construct 3D maps of an environment.

4. The method of claim 1 wherein determining the sequential trajectory data comprises using at least one visual data sensor in the one or more prior moving objects.

5. The method of claim 4 wherein said at least one visual data sensor comprises any or a combination of: an image camera; a video camera; a monocular camera; a depth camera; a stereo image camera; a high dynamic range camera, a light detection and ranging sensor; a radio detection and ranging sensor; an inertial measurement unit.

6. The method of claim 1 wherein determining the sequential trajectory data comprises performing structure from motion.

7. The method of claim 1 wherein estimating future positions of the object further comprises hypothesising that the object is following a trajectory path of each of the one or more prior moving objects in the same location as the object.

8. The method of claim 1 wherein estimating future positions of the object further comprises using location data from the one or more prior moving objects.

9. The method of claim 1 wherein estimating future positions of the object further comprises estimating a future pose of the object.

10. The method of claim 9 wherein the future pose estimate comprises a random noise model inclusion so as to account for deviations in the trajectory.

11. The method of claim 9 wherein the future pose estimate is the observed pose of a prior moving object, having previously intersected the vicinity of the position of the object, after a time interval.

12. The method of claim 1 wherein constraining the future positions of the object further comprises determining state comparisons between the one or more prior moving objects and the object.

13. The method of claim 12, wherein the differences comprises any one of, or any combination of: a difference in a Euclidean distance in the 3D space; relative difference of heading angle; and difference in linear speed.

14. The method of claim 12 wherein constraining the future positions of the object are weighted in order to output either a wider or narrower set of samples.

15. A system for estimating movements of an object, the system comprising: at least one processor; and a memory storing instructions that, when executed by the at least one processor, cause the system to perform: determining initial state data of the object at a first time; determining sequential trajectory data for one or more prior moving objects that intersected vicinity of the position of the object; estimating future positions of the object, at a second time, based on the sequential trajectory data for the one or more prior moving objects; and constraining the future positions of the object based on a comparison between the object and the one or more prior moving objects for which the sequential trajectory data intersects the vicinity of the position of the object, wherein the constrained future positions of the object are indicative of the estimated movement of the object at the second time.

16. A computer program product comprising instructions which, when executed by a computer, cause the computer to perform a method comprising: determining initial state data of an object at a first time; determining sequential trajectory data for one or more prior moving objects that intersected vicinity of the position of the object; estimating future positions of the object, at a second time, based on the sequential trajectory data for the one or more prior moving objects; and constraining the future positions of the object based on a comparison between the object and the one or more prior moving objects for which the sequential trajectory data intersects the vicinity of the position of the object, wherein the constrained future positions of the object are indicative of the estimated movement of the object at the second time.

17. The system of claim 15 wherein estimating future positions of the object further comprises hypothesising that the object is following a trajectory path of each of the one or more prior moving objects in the same location as the object.

18. The system of claim 15 wherein constraining the future positions of the object further comprises determining state comparisons between the one or more prior moving objects and the object.

19. The computer program product of claim 16 wherein estimating future positions of the object further comprises hypothesising that the object is following a trajectory path of each of the one or more prior moving objects in the same location as the object.

20. The computer program product of claim 16 wherein constraining the future positions of the object further comprises determining state comparisons between the one or more prior moving objects and the object.

Description

BRIEF DESCRIPTION OF DRAWINGS

(1) FIG. 1 illustrates an example of a known linear extrapolation model to predict the future position of a detected vehicle at an intersection;

(2) FIG. 2 illustrates a flowchart according to an embodiment;

(3) FIGS. 3a and 3b illustrate map datasets used by an embodiment;

(4) FIGS. 4a, 4b, 4c and 4d illustrate prior trajectory information used by an embodiment;

(5) FIGS. 5a and 5b illustrate samples of estimated positions produced in accordance with an embodiment;

(6) FIG. 6 depicts the logic flow of an embodiment;

(7) FIG. 7 illustrates samples of estimated positions produced in accordance with an embodiment; and

(8) FIG. 8 illustrates an example embodiment of the present invention to predict the future position of a detected vehicle at an intersection.

DETAILED DESCRIPTION

(9) FIG. 1 depicts one of the problems seen by current methods of predicting future motion. More specifically, the illustration relates to motion models that rely on linear extrapolation of motion data.

(10) The figure shows a bird's eye view of a four-way road intersection 100. A first vehicle 101 is depicted approaching the intersection. The position of the first vehicle at a first time, t, is shown as 101a and the position of the first vehicle ata second time, t+1, is shown as 101b. The trajectory of the first vehicle is indicated as a straight path 103.

(11) A second vehicle 102 is also depicted in the figure. The second vehicle is seen mid-way through the intersection at the first time, t, shown as 102a and the second time, t+1, shown as 102b. Although in real-world scenarios, the position on the second vehicle is likely to be in the area indicated by 106, using the linear motion model, the system assumes the second vehicle is traversing along a second straight path 104. According to this interpretation, the linear model expects the two vehicles to collide at point 105 which is the point the first 103 and second 104 straight paths intersect.

(12) However, anyone with an appreciation of traffic rules and/or a highway code will at a first glance disagree with the expected collision predicted by the linear motion model. Since linear motion models do not incorporate curved motions of real world scenarios the true nature of where the second vehicle is actually likely to be after passing through the intersection 106 is not accounted for. The use of these models therefore results in inaccurate and unreliable estimations of future positions of moving vehicles.

(13) In a similar way, various methods have been proposed over years to understand and model vehicle motion dynamics, driver intent and vehicle interactions with the environment and neighboring agents. In most cases, motion prediction involves relying fully or partly on a vehicle dynamics model. For example, some methods compare and evaluate several motion models for tracking vehicles. These methods conclude that constant turn rate and acceleration model (CTRA) perform the best. Other methods include constant turn rate and velocity (CTRV), constant steering angle and velocity (CSAV), constant curvature and acceleration (CCA) and purely linear motion models such as constant velocity (CV) or constant acceleration (CA), as previously described.

(14) These models are usually combined with Kalman filtering or Bayesian filtering for path prediction. However, these approaches are only able to perform predictions for a very short window into the future. In order to address this, some models combine a constant yaw rate and acceleration model with a manoeuvre classifier to predict vehicle trajectories. But these methods are restricted to limited scenarios and are constrained by the number of manoeuvres.

(15) As opposed to explicitly crafting vehicle dynamics, Dynamic Bayesian networks, Gaussian mixture models, Hidden Markov models, Neural networks or a combination of these techniques are used to provide data-driven approaches to vehicle dynamics. Although these approaches achieve better performance than pure vehicle dynamics based approaches, they are either trained for specific limited scenarios (e.g., highways) or tend to learn a general model that does not utilise environment specific cues such as traffic pattern in the area, changes in the environment structure, etc.

(16) An example embodiment will now be described with reference to FIGS. 2 to 6.

(17) As illustrated in FIG. 2, the first step of the method implemented by the invention is to detect a moving object, such as a vehicle, and capture data relating to the observed state of the moving vehicle 201. There is no restriction to the methods used to initially detect a moving vehicle. As an example, the use of neural networks may be incorporated to identify an object and place a bounding box around the pixels that represent the object.

(18) The initial state (s.sub.0) of the car includes position data (x.sub.0 R.sup.3), rotation data (r.sub.0 SO(3)) and velocity data (v.sub.0 R). Mathematically this can be represented as:
s.sub.0=(x.sub.0,r.sub.0,v.sub.0)

(19) As illustrated in step 202, the method then brings together trajectory data of vehicles that have previously traversed the area in which the new moving vehicle was detected. Although any traditional method may be implemented to obtain this data the preferred option is to extract data from map data that was constructed using structure-from-motion techniques. This advantageously enables a large amount of crowd-sourced high-quality motion data to drive the motion prediction of this invention. As an example, this type of data can be collected by equipping a large fleet of vehicles with cameras and performing structure-from-motion at a city scale to accurately reconstruct their trajectories. As will be further elaborated below, this data can be used a sample for the underlying motion distribution in the area and be used for future motion prediction of newly observed cars.

(20) Structure from motion methods have the benefits of needing zero human annotation as it implicitly captures modelled and unmodelled aspects of the vehicle motion, scales to large city-scale scenarios and improves with time as the amount of data increases. This data is usually built up of sequential images over a period of time. Additionally, each image also includes pose information which can be used to vehicles position, rotation and velocity along its path.

(21) Example city scale map datasets are depicted in FIGS. 3a and 3b. The datasets shown in these figures were compiled using over ten million images captured in San Francisco and New York using dash-cam mounted mobile phones. The images were used to perform large-scale structure-from-motion to reconstruct accurate vehicle trajectories in the cities over a period of several weeks. Although a monocular camera of a mobile phone was executed to derive the datasets shown in this figure, any type of visual sensor may be used to compile the initial sequential image data. As a result, prior trajectory data can be automatically extracted as a by-product of building a large-scale crowd-sourced 3D map of the environment.

(22) FIG. 4a illustrates the trajectories 400 extracted from the San Francisco data set, as generated by a randomised fleet of vehicles, which is used by this invention as prior trajectory data. FIGS. 4b, 4c and 4d correspond to points 410, 420 and 430, respectively, in FIG. 4a. These figures illustrate a few paths taken by the fleet of vehicles (401, 402, 403, 404) and their respective orientations. These figures illustrate the vehicles' motion along a curved road (FIG. 4b), an intersection (FIG. 4c) and a straight road (FIG. 4d).

(23) In this way, the invention utilises location specific information for accurate future predictions. Instead of learning a global generic model or relying on limited variable models, the invention relies on historical vehicle trajectories in the locality of a newly detected vehicle to perform on-the-fly future position prediction, in substantially real time.

(24) As aforementioned, the motion prior data comprises of a large set of individual trajectory samples that contain accurate 3D positions and rotations of vehicles driven through the area in the past. Mathematically, this is represented as G={G.sup.1, G.sup.2, . . . , G.sup.N}, where each trajectory G.sup.i={s.sub.1.sup.i, s.sub.2.sup.i, . . . , s.sub.m.sup.i} is a sequence of observed positions, rotations, and velocities of the car at regular time intervals t=1, 2, 3 . . . as the car had been driven around the city. Using this method, there is no requirement to use manual or semantic annotations of the environment or any knowledge of traffic rules. Instead it is assumed that each trajectory or path implicitly captures all relevant local and road information in the behaviour of the vehicle's motion.

(25) Referring back to FIG. 2, once prior trajectory information has been obtained, a number of future positions of the newly observed vehicle are estimated 203. In order to predict the future position of a vehicle at a time t, it is hypothesized that the newly observed vehicle is following the same path and trajectory pattern as one of the previous vehicles at the same location. Specifically, for each prior state s.sub.j.sup.i of a prior trajectory, it is assumed that the newly observed vehicle is going to follow the same motion pattern as the previous vehicle that generated the prior trajectory continuing from that state. Given this assumption, the pose of the vehicle in the future is likely to be:
s.sub.t=s.sub.j+t.sup.i+

(26) where s.sub.j+t.sup.i is the observed pose of the vehicle previously driven through the area t seconds after the queried state (when the new vehicle was first observed) and is random noise taking into account that the trajectory can slightly differ. Examples of estimated future positions or samples can be seen in FIGS. 5a and 5b, where 501 illustrates a newly observed vehicle at a first time, t, and 502 illustrates the estimated future positions of the vehicle and a second time, t+1.

(27) After having estimated the likely future position for the newly observed vehicle based on prior positions and trajectories of each or any of the previous vehicles, in order to improve the estimation, the samples are constrained by assessing the likelihood of the observed vehicle following the path of the one or more samples 204.

(28) Mathematically, the distribution of the future pose is a weighted sum of individual factors:

(29) $p\left(s_t.Math.s_0,G\right)=\frac{1}{Z}.Math.K\left(s_j^i,s_0\right)p\left(s_t.Math.s_{j+t}^i,\right)$
where Z is a normalisation factor:
Z=K(s.sub.j.sup.i,s.sub.0),
and K(s.sub.j.sup.i, s.sub.0) measures the similarity of a prior state to the current state of a newly observed vehicle, capturing the likelihood that it can indeed follow the exhibited prior motion pattern. This similarity is modelled as the sum of a number of individual factors:

(30) $K\left(s_j^i,s_0\right)=\exp \left\{-\frac{{.Math.x_j^i-x_0.Math.}^2}{{}_x^2}-\frac{{.Math.r_j^i-r_0.Math.}^2}{{}_r^2}-\frac{{.Math.v_j^i-v_0.Math.}^2}{{}_v^2}\right\}$
||x.sub.j.sup.ix.sub.0||.sup.2 is the Euclidean distance between the sample position and the observed position of the vehicle in the 3D space, ||r.sub.j.sup.ir.sub.0||.sup.2 is the relative difference of heading angles between the sample and the observed vehicle and ||v.sub.j.sup.iv.sub.0||.sup.2 is the difference in linear speed. The parameters .sub.x, .sub.r, and .sub.v model the relevance of the individual factors.

(31) By constraining the samples in this way, the most likely estimates for the future positions of the observed vehicles based on the prior vehicle data are produced.

(32) Thus, the probability density function p(s.sub.t|s.sub.0, G) can be evaluated explicitly in a closed form. Moreover, a sampling procedure can be implemented efficiently by first sampling the corresponding prior state s.sub.j.sup.i according to relevance factor K, performing table look-up for s.sub.j+t.sup.i and adding noise. This is depicted in FIG. 6.

(33) An example of future vehicle motion prediction is illustrated in FIG. 7. 701 represents an observed vehicle at a query position and a velocity at time t. The groupings of 702 and 703 represent the distribution of predicted samples of the vehicle at a time of t+5. Notably, the road ahead of the vehicle is a one-way road in the opposite direction of the vehicle's motion. Without needing any manual input of road traffic signage, the method implicitly captures this information by using the paths of previous vehicles in the area. Thus, the only two potential options for the vehicle is taking a left or right at the intersection.

(34) FIG. 5 also illustrates samples drawn from prior data. As depicted, sampling follows the previously observed trajectories of prior motion in the area while parameters a model the relevance of the individual components to the state of the observed vehicle. For example, a small value of .sub.v (FIG. 5a) results in predictions matching the current velocity of the newly observed vehicle while a larger .sub.v (FIG. 5b) results in future predictions sampled using a wider variety of the previously observed initial velocities.

(35) In FIG. 1, motion prediction using linear extrapolation was illustrated. In contrast, FIG. 8 depicts how the method of this invention predicts the future movements of a vehicle in the same scenario. As opposed to relying on linear projections of the trajectories, 801 depicts a cluster of estimated future positions of the vehicle 102 using prior trajectory data.

(36) Additionally, this invention can be used universally as a motion-prediction step in various vehicle-tracking systems for the purpose of vehicle safety and autonomy. The system may be used to drive motion prediction on a large scale in a variety of environmental and traffic conditions. Specifically, by creating large-scale accurate dataset of vehicle motion priors as a by-product of building a crowd-sourced city-scale 3D map of the environment and predicting a new vehicle's future position using the extracted prior data from the area.

(37) The method vastly improves the precision over traditional methods and also demonstrates continuously improving performance as the amount of prior data grows.

(38) Any system feature as described herein may also be provided as a method feature, and vice versa. The invention can be implemented by or as a system comprising: at least one processor; and a memory storing instructions that, when executed by the at least one processor, cause the system to perform the invention. The invention can be implemented by or as a computer program product comprising instructions which, when executed by a computer, cause the computer to perform a method comprising the invention. As used herein, means plus function features may be expressed alternatively in terms of their corresponding structure.

(39) Any feature in one aspect may be applied to other aspects, in any appropriate combination. In particular, method aspects may be applied to system aspects, and vice versa. Furthermore, any, some and/or all features in one aspect can be applied to any, some and/or all features in any other aspect, in any appropriate combination.

(40) It should also be appreciated that particular combinations of the various features described and defined in any aspects of the invention can be implemented and/or supplied and/or used independently.