Object and vehicle detection and tracking using 3-D laser rangefinder

1. A method for detecting and tracking objects near a vehicle, said method comprising:

providing a plurality of scan points from a laser rangefinder, said scan points representing locations in a space surrounding the vehicle where an object has been detected by the laser rangefinder;

using the scan points and vehicle dynamics data to establish a location of a ground plane;

using the scan points and the location of the ground plane to construct an object occupancy map and an object elevation map; and

using the scan points and the object occupancy map to create dynamic object data, where the dynamic object data identifies and tracks objects which are moving.

2. The method of claim 1 wherein using the scan points and vehicle dynamics data to establish a location of a ground plane includes:

using vehicle pitch and roll angle data to calculate a predicted ground plane location from a previous ground plane location;

performing a gating operation on the scan points, where the gating operation uses the predicted ground plane location to identify a set of points which are to be included in a ground plane computation;

creating a matrix containing the set of points which are to be included in the ground plane computation;

performing an eigenvalue decomposition on the matrix to identify a smallest eigenvalue and an eigenvector associated with the smallest eigenvalue; and

using the eigenvector associated with the smallest eigenvalue to determine an equation for the ground plane.

3. The method of claim 2 further comprising providing an updated ground plane location using a Kalman filter.

4. The method of claim 1 wherein using the scan points and the location of the ground plane to construct an object occupancy map and an object elevation map includes:

projecting each of the scan points onto the ground plane to determine both a ground plane intersection point and a normal vector for each of the scan points;

using the normal vectors for all of the scan points to build an object elevation map, where the object elevation map shows height information for all objects detected in the space surrounding the vehicle; and

using the ground plane intersection points for all of the scan points to build the object occupancy map, where the object occupancy map shows location information for all objects detected in the space surrounding the vehicle.

5. The method of claim 1 wherein using the scan points and the object occupancy map to create dynamic object data includes:

removing ground points from the scan points to create a set of object points;

using the object occupancy map most recently computed;

constructing a similarity graph to determine which of the object points are moving together;

clustering groups of the object points which are moving together into dynamic objects;

classifying the dynamic objects into object types, where the object types include vehicle, pedestrian, bicycle, and animal;

computing centroids of the dynamic objects; and

tracking the dynamic objects to create the dynamic object data.

6. The method of claim 5 wherein clustering groups of the object points which are moving together into dynamic objects uses a Depth First Search strategy.

7. The method of claim 5 wherein tracking the dynamic objects uses a Kalman filter.

8. The method of claim 5 wherein the dynamic object data includes a longitudinal and lateral position and a longitudinal and lateral velocity relative to ground of each of the dynamic objects.

9. The method of claim 1 further comprising using the object occupancy map and the dynamic object data in an application in the vehicle.

10. A method for detecting and tracking objects near a vehicle, said method comprising:

providing a plurality of scan points from a laser rangefinder, said scan points representing locations in a space surrounding the vehicle where an object has been detected by the laser rangefinder;

using the scan points and vehicle dynamics data to establish a definition of a ground surface;

using the scan points and the definition of the ground surface to construct an object occupancy map and an object elevation map;

using the scan points and the object occupancy map to create dynamic object data, where the dynamic object data identifies and tracks objects which are moving; and

using the object occupancy map and the dynamic object data in an application in the vehicle.

11. The method of claim 10 wherein using the scan points and vehicle dynamics data to establish a definition of a ground surface includes using a Voronoi diagram method to construct a non-flat ground surface segmentation.

12. The method of claim 10 wherein using the scan points and the definition of the ground surface to construct an object occupancy map and an object elevation map includes:

projecting each of the scan points onto the ground surface to determine both a ground surface intersection point and a normal vector for each of the scan points;

using the normal vectors for all of the scan points to build an object elevation map, where the object elevation map shows height information for all objects detected in the space surrounding the vehicle; and

using the ground surface intersection points for all of the scan points to build the object occupancy map, where the object occupancy map shows location information for all objects detected in the space surrounding the vehicle.

13. The method of claim 10 wherein using the scan points and the object occupancy map to create dynamic object data includes:

removing ground points from the scan points to create a set of object points;

using the object occupancy map most recently computed;

constructing a similarity graph to determine which of the object points are moving together;

clustering groups of the object points which are moving together into dynamic objects;

classifying the dynamic objects into object types, where the object types include vehicle, pedestrian, bicycle, and animal;

computing centroids of the dynamic objects; and

tracking the dynamic objects to create the dynamic object data.

14. The method of claim 13 wherein clustering groups of the object points which are moving together into dynamic objects includes constructing a similarity graph, using a depth-first search strategy, and using a grid data structure with a hash table.

15. A system for detecting and tracking objects near a vehicle, said system comprising:

a laser rangefinder in the vehicle for providing a plurality of scan points, where the scan points represent locations in a space surrounding the vehicle where an object has been detected by the laser rangefinder;

a first processor configured to compute a ground surface and create an object occupancy map using the scan points;

a second processor configured to identify and track dynamic objects using the object occupancy map and the scan points; and

a shared memory module for sharing data between the first processor and the second processor.

16. The system of claim 15 wherein the first processor includes a ground surface tracking module, said ground surface tracking module including a gating algorithm for identifying a set of points to be included in a ground plane computation matrix, and an eigenvalue decomposition algorithm for computing an equation of a ground plane from the ground plane computation matrix.

17. The system of claim 16 wherein the ground surface tracking module in the first processor also includes a ground surface segmentation algorithm which uses a Voronoi diagram method to construct a non-flat ground surface segmentation.

18. The system of claim 15 wherein the first processor includes an object occupancy module, said object occupancy module including a ground surface projection algorithm for projecting all scan points orthogonally onto the ground surface, an elevation map construction algorithm for constructing an elevation map which shows height information for all of the objects detected in the space surrounding the vehicle, and an occupancy map construction algorithm for constructing the occupancy map showing location information for all of the objects detected in the space surrounding the vehicle.

19. The system of claim 15 wherein the second processor includes an object clustering module for clustering object points which are moving together into dynamic objects, an object classification module for classifying the dynamic objects into object types, and an object tracking module for providing position and velocity information about each of the dynamic objects.

20. The system of claim 15 further comprising a second laser rangefinder in the vehicle.