Methods and systems for scan matching approaches for vehicle heading estimation
View Patent ↗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.
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.