System and method of lane path estimation using sensor fusion

1. Method for estimating a projected path of travel for a vehicle on a road, the method comprising:

monitoring a plurality of sensor inputs;

determining in a processor a road geometry in front of the vehicle based upon the monitored sensor inputs, the road geometry in front of the vehicle comprising

a road curvature determined by fusing radar based curvature, map based curvature, camera based curvature and yaw rate based curvature, the radar based curvature determined by

tracking one or more target vehicles in front of the vehicle to obtain a tracked trail for each target vehicle,

comparing each tracked trail to a curvature threshold, and

generating a sample mean curvature based on each tracked trail that exceeds the curvature threshold, and

a tangent at the vehicle position;

determining a vehicle position in relation to the road geometry based upon the monitored sensor inputs comprising

determining a vehicle orientation in relation to the road geometry based upon the monitored sensor inputs and the road curvature, and

determining a vehicle lateral position in relation to the road geometry based upon the monitored sensor inputs and the road curvature;

determining a plurality of particle points in front of the vehicle representing a potential path of travel from the road geometry and the vehicle position; and

utilizing iteratively determined ones of the plurality of particle points to navigate the vehicle including omitting ones of the plurality of particle points passed by the vehicle.

2. The method of claim 1 , wherein determining the plurality of particle points in front of the vehicle comprises:

determining a plurality of particle points for each of a plurality of the sensor inputs; and

fusing the plurality of particle points for each of the plurality of the sensor inputs to determine the plurality of particle points in front of the vehicle.

3. The method of claim 1 , wherein utilizing iteratively determined ones of the plurality of particle points to navigate the vehicle comprises:

determining the projected path of travel for navigation of the vehicle based upon the iteratively determined ones of the plurality of particle points.

4. The method of claim 1 , wherein determining a vehicle lateral position in relation to the road geometry comprises:

determining the vehicle lateral position in relation to a current lane center.

5. The method of claim 1 , wherein monitoring the plurality of sensor inputs comprises:

monitoring data from a mapping device; and

monitoring data from at least one additional sensor input selected from the group consisting of a vision subsystem, a vehicle kinematic sensor, radar, LIDAR, a vehicle-to-vehicle communication system, and a vehicle-to-infrastructure communication system.

6. The method of claim 5 , further comprising:

iteratively updating the vehicle position when new data from at least one of the plurality of sensor inputs is monitored.

7. The method of claim 1 , wherein the map based curvature is determined by:

generating a spline describing road curvature in proximity of the vehicle; and

determining the vehicle position upon the spline based upon the plurality of sensor inputs.

8. The method of claim 7 , wherein generating the spline comprises:

collecting map shape points from a map database; and

fitting the map shape points with the spline.

9. The method of claim 8 , wherein the map shape points are associated with a local coordinate system.

10. The method of claim 7 , wherein monitoring the plurality of sensor inputs comprises monitoring a global positioning coordinate;

wherein determining the vehicle position upon the spline comprises:

determining an initial estimate of the vehicle position upon the spline based upon the plurality of sensor inputs;

correcting the initial estimate of the vehicle position upon the spline based upon the global positioning coordinate.

11. The method of claim 1 , wherein the plurality of particle points in front of the vehicle is based upon:

a first set of particle points determined based upon vehicle kinematic data;

a second set of particle points determined based upon camera data; and

a third set of particle points determined based upon map data.

12. The method of claim 11 , further comprising:

fusing the first, second, and third sets of particle points into a single set of particles points representing the path of travel using a least squares estimation.

13. The method of claim 12 , wherein fusing is executed based upon a position variance associated with each of the plurality of particle points.

14. The method of claim 1 , wherein determining the vehicle position is executed using a Kalman filter.

15. Method for estimating a projected path of travel for a vehicle on a road, the method comprising:

monitoring a plurality of sensor inputs describing a road geometry in front of the vehicle;

determining in a processor a set of particle points for each of the sensor inputs, comprising:

determining the road geometry in front of the vehicle based upon the respective sensor input, the road geometry in front of the vehicle comprising

a road curvature determined by fusing radar based curvature, map based curvature, camera based curvature and yaw rate based curvature, the radar based curvature determined by

tracking one or more target vehicles in front of the vehicle to obtain a tracked trail for each target vehicle,

comparing each tracked trail to a curvature threshold, and

generating a sample mean curvature based on each tracked trail that exceeds the curvature threshold, and

a tangent at the vehicle position;

determining a vehicle position in relation to the road geometry based upon the respective sensor input comprising

determining a vehicle orientation in relation to the road geometry based upon the monitored sensor input and the road curvature, and

determining a vehicle lateral position in relation to the road geometry based upon the monitored sensor inputs and the road curvature; and

utilizing the road geometry and the vehicle position to determine the set of particle points for the respective sensor input representing a potential path of travel;

fusing the sets of particle points for each of the sensor inputs into a set of fused particle points; and

utilizing iterative determinations of the set of fused particle points to operate the vehicle, wherein utilizing iterative determinations comprises omitting individual particle points of the sets of fused particle points passed by the vehicle.

16. The method of claim 15 , wherein fusing the sets of particle points comprises utilizing a least squares estimation.

17. The method of claim 15 , wherein monitoring the plurality of sensor inputs comprises:

monitoring data from a global positioning device;

monitoring vehicle kinematics data from vehicle kinematic sensors; and

monitoring camera data from a vision subsystem.

18. The method of claim 15 , wherein fusing the sets of particle points is based upon a variance associated with each of the particle points for each of the sensor inputs.