Methods and systems for scan matching approaches for vehicle heading estimation

• US 9,043,069 B1
• Filed: 11/07/2012
• Issued: 05/26/2015
• Est. Priority Date: 11/07/2012
• Status: Active Grant

• Current Assignee: Google LLC (Alphabet Inc.)
• Original Assignee: Google Inc. (Alphabet Inc.)
• Inventors: Ferguson, David I., Burnette, Donald Jason, Zhu, Jiajun
• Primary Examiner(s): Trammell, James
• Assistant Examiner(s): SMITH-STEWART, DEMETRA R
• Application Number:
  US13/670,917
• Time in Patent Office: 930 Days
• US Class Current: 701/23
• CPC Class Codes: B60W 30/00 Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units
• CPC Class Codes: B60W 40/103 Side slip angle of vehicle body
• CPC Class Codes: G01S 17/58 Velocity or trajectory determination systems; Sense-of-movement determination systems
• CPC Class Codes: G01S 17/89 for mapping or imaging
• CPC Class Codes: G05D 1/00 Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots drive control systems specially adapted for autonomous road vehicles B60W60/00
• CPC Class Codes: G06T 2207/10028 Range image; Depth image; 3D point clouds
• CPC Class Codes: G06T 2207/30261 Obstacle
• CPC Class Codes: G06T 7/215 Motion-based segmentation

Abstract

Methods and devices for estimating a heading of a target vehicle are disclosed. An example method may include determining a first point cloud representative of a location of a target vehicle at a first time period and a second point cloud representative of the location of the target vehicle at a second time period. Using a computing device, an initial comparison between the first point cloud and the second point cloud may be determined based on an estimate of a speed for the target vehicle and a time difference between the first time period and the second time period. Additionally, the initial comparison may be revised based on a minimization of a distance between points of the first point cloud and corresponding points of the second point cloud. An estimate of a heading of the target vehicle may then be determined based on the revised comparison.

Claims

1. A method comprising:

determining a first point cloud representative of a location of a target vehicle at a first time period;

determining a second point cloud representative of the location of the target vehicle at a second time period;

determining, using a computing device, an initial comparison between the first point cloud and the second point cloud based on an estimate of a speed for the target vehicle and a time difference between the first time period and the second time period;

revising, using the computing device, the initial comparison based on a minimization of a distance between points of the first point cloud and corresponding points of the second point cloud;

determining, using the computing device, an estimate of a heading of the target vehicle based on the revised comparison; and

controlling another vehicle based on the determined estimate of the heading of the target vehicle.

2. The method of claim 1, further comprising determining the initial comparison between the first point cloud and the second point cloud based on the estimate of the speed for the target vehicle, the time difference, and a prior estimate of the heading of the target vehicle.

3. The method of claim 1, wherein the other vehicle is configured to operate in an autonomous mode.

4. The method of claim 1, wherein determining the first point cloud and determining the second point cloud comprises performing a laser scan at a fixed time interval, wherein the time difference is determined based on the fixed time interval.

5. The method of claim 1, further comprising revising the initial comparison using iterative closest point (ICP).

6. The method of claim 1, further comprising reducing a size of the first point cloud and the second point cloud by removing points of the first point cloud and the second point cloud that are outside of a predetermined height range.

7. The method of claim 1, further comprising reducing a density of the first point cloud and the second point cloud by randomly sampling points of the first point cloud and the second point cloud and selecting the randomly sampled points as representations of the first point cloud and the second point cloud.

8. The method of claim 1, wherein determining the estimate of the heading of the target vehicle based on the revised comparison comprises:

determining an offset between the first point cloud and the second point cloud, wherein the offset comprises a three-dimensional (3D) vector; and

determining the estimate of the heading of the target vehicle based on an inverse tangent of the offset along a ground plane, wherein the ground plane is an approximation of a two-dimensional plane that the target vehicle travels on.

9. A non-transitory computer-readable medium having stored therein instructions executable by a computing device to cause the computing device to perform functions comprising:

determining a first point cloud representative of a location of a target vehicle at a first time period;

determining a second point cloud representative of the location of the target vehicle at a second time period;

determining an initial comparison between the first point cloud and the second point cloud based on an estimate of a speed for the target vehicle and a time difference between the first time period and the second time period;

revising the initial comparison based on a minimization of a distance between points of the first point cloud and corresponding points of the second point cloud;

determining an estimate of a heading of the target vehicle based on the revised comparison; and

controlling another vehicle based on the determined estimate of the heading of the target vehicle.

10. The non-transitory computer-readable medium of claim 9, wherein the functions further comprise determining the initial comparison between the first point cloud and the second point cloud based on the estimate of the speed for the target vehicle, the time difference, and a prior estimate of the heading of the target vehicle.

11. The non-transitory computer-readable medium of claim 9, wherein determining the first point cloud and determining the second point cloud comprises performing a laser scan at a fixed time interval, wherein the time difference is determined based on the fixed time interval.

12. The non-transitory computer-readable medium of claim 9, wherein the functions further comprise reducing a size of the first point cloud and the second point cloud by removing points of the first point cloud and the second point cloud that are outside of a predetermined height range.

13. The non-transitory computer-readable medium of claim 9, wherein the functions further comprise reducing a density of the first point cloud and the second point cloud by randomly sampling points of the first point cloud and the second point cloud and selecting the randomly sampled points as representations of the first point cloud and the second point cloud.

14. The non-transitory computer-readable medium of claim 9, wherein determining the estimate of the heading of the target vehicle based on the revised comparison comprises:

determining an offset between the first point cloud and the second point cloud, wherein the offset comprises a three-dimensional (3D) vector; and

determining the estimate of the heading of the target vehicle based on an inverse tangent of the offset along a ground plane, wherein the ground plane is an approximation of a two-dimensional plane that the target vehicle travels on.

15. A controller comprising:

at least one processor;

a memory; and

instructions stored in the memory and executable by the at least one processor to cause the controller to perform functions comprising;

determining a first point cloud representative of a location of a target vehicle at a first time period;

determining a second point cloud representative of the location of the target vehicle at a second time period;

determining an initial comparison between the first point cloud and the second point cloud based on an estimate of a speed for the target vehicle and a time difference between the first time period and the second time period;

revising the initial comparison based on a minimization of a distance between points of the first point cloud and corresponding points of the second point cloud;

determining an estimate of a heading of the target vehicle based on the revised comparison; and

controlling another vehicle based on the determined estimate of the heading of the target vehicle.

16. The controller of claim 15, wherein the functions further comprise determining the initial comparison between the first point cloud and the second point cloud based on the estimate of the speed for the target vehicle, the time difference, and a prior estimate of the heading of the target vehicle.

17. The controller of claim 15, wherein determining the first point cloud and determining the second point cloud comprises performing a laser scan at a fixed time interval, wherein the time difference is determined based on the fixed time interval.

18. The controller of claim 15, wherein the functions further comprise reducing a size of the first point cloud and the second point cloud by removing points of the first point cloud and the second point cloud that are outside of a predetermined height range.

19. The controller of claim 15, wherein the functions further comprise reducing a density of the first point cloud and the second point cloud by randomly sampling points of the first point cloud and the second point cloud and selecting the randomly sampled points as representations of the first point cloud and the second point cloud.

20. The controller of claim 15, wherein the other vehicle is configured to operate in an autonomous mode.