Adaptive Kalman filter method for accurate estimation of forward path geometry of an automobile

• US 6,718,259 B1
• Filed: 10/02/2002
• Issued: 04/06/2004
• Est. Priority Date: 10/02/2002
• Status: Expired due to Fees

• Current Assignee: HRL Laboratories LLC (The Boeing Co.)
• Original Assignee: HRL Laboratories LLC (The Boeing Co.)
• Inventors: Khosla, Deepak
• Primary Examiner(s): NGUYEN, TAN QUANG
• Application Number:
  US10/263,924
• Time in Patent Office: 552 Days
• Field of Search: 701/300, 701/301, 701/200, 701/205, 701/207, 701/210, 701/211, 701/216, 701/96, 340/435, 340/903, 342/457
• US Class Current: 701/200
• CPC Class Codes: B60W 2050/0052 Filtering, filters
• CPC Class Codes: B60W 2552/00 Input parameters relating to infrastructure
• CPC Class Codes: B60W 2552/30 Road curve radius
• CPC Class Codes: B60W 2552/53 Road markings, e.g. lane marker or crosswalk
• CPC Class Codes: G01C 21/26 specially adapted for navigation in a road network
• CPC Class Codes: G05D 1/0212 with means for defining a desired trajectory involving a plurality of land vehicles G05D1/0287
• CPC Class Codes: G06V 20/588 Recognition of the road, e.g. of lane markings; Recognition of the vehicle driving pattern in relation to the road
• CPC Class Codes: G08G 1/166 for active traffic, e.g. moving vehicles, pedestrians, bikes
• CPC Class Codes: G08G 1/167 Driving aids for lane monitoring, lane changing, e.g. blind spot detection

Abstract

The present invention provides a method and apparatus for estimation of vehicle forward path geometry utilizing an adaptive Kalman filter bank and a two-clothoid road model. The invention provides that each of a plurality of Kalman filters, utilizing the latest available measurement vector Y_k at time k, estimates the state vector X_k and error covariance matrix P_k. The outputs of filter 504a, 504b, and 504c denoted as as X_k^j and P_k^j, are provided to a plurality of weighting elements, which calculate weight factors, W_k^j 506a, 506b, and 506c for each filter. The weight factor of each filter is the probability that the upcoming road geometry matches the road model hypothesized in the filter. After being assigned a weighted value, the weighted value road models are fused in a fusion element 508, and a weighted output road geometry model is provided.

Claims

1. An apparatus for dynamically predicting road geometry comprising:

i. a bank of filters configured to receive current environmental data from at least one sensor and to perform an estimation using the current environmental data, and to provide a filter output from each filter;

ii. a weighting element configured to receive the filter output from the bank of filters and to ascribe a weighted value to each filter output based on a relevance of the filter, thereby providing weighted outputs;

iii. a fusing element configured to fuse the weighted outputs into a single output;

whereby the output describes a vehicle forward path estimate.

2. An apparatus for dynamically predicting road geometry as set forth in claim 1, wherein the bank of filters is further configured to receive a previous vehicle path estimate and to recursively perform the estimation based on the current environmental data and the previous vehicle path estimate.

3. An apparatus for dynamically predicting road geometry as set forth in claim 2, wherein each filter in the bank of filters is a Kalman filter.

4. An apparatus for dynamically predicting road geometry as set forth in claim 3, wherein the bank of filters includes three Kalman filters.

5. An apparatus for dynamically predicting road geometry as set forth in claim 4, wherein the configuration of the bank of filters is based on a polynomial road model.

6. An apparatus for dynamically predicting road geometry as set forth in claim 5, wherein the polynomial road model is a two-clothoid road model.

7. An apparatus for dynamically predicting road geometry as set forth in claim 6, wherein each filter is tuned to a different class of geometry within the two-clothoid road model.

8. An apparatus for dynamically predicting road geometry as set forth in claim 7, wherein the different classes of geometry include a constant geometry road section, an upcoming transition in the far range, and an upcoming transition in the near range.

9. An apparatus for dynamically predicting road geometry as set forth in claim 8, wherein the weighting element ascribes weighted values based on the posterior probabilities of a fit between the filter and the forward vehicle path estimate.

10. An apparatus for dynamically predicting road geometry as set forth in claim 9, wherein the at least one sensor is a device selected from a group consisting of imaging devices, GPS/map systems, laser sensors, and radars.

11. An apparatus for dynamically predicting road geometry as set forth in claim 10, wherein the output of the apparatus is configured for use by other automotive systems selected from a group consisting of driver assistance systems and vehicle safety systems.

12. An apparatus for dynamically predicting road geometry as set forth in claim 1 wherein each filter in the bank of filters is a Kalman filter.

13. An apparatus for dynamically predicting road geometry as set forth in claim 1, wherein the configuration of the bank of filters is based on a polynomial road model.

14. An apparatus for dynamically predicting road geometry as set forth in claim 13, wherein the polynomial road model is a two-clothoid road model.

15. An apparatus for dynamically predicting road geometry as set forth in claim 1, wherein each filter is tuned to a different class of geometry within the two-clothoid road model.

16. An apparatus for dynamically predicting road geometry as set forth in claim 15, wherein the different classes of geometry include a constant geometry road section, an upcoming transition in the far range, and an upcoming transition in the near range.

17. An apparatus for dynamically predicting road geometry as set forth in claim 1, wherein the weighting element ascribes weighted values based on the posterior probabilities of a fit between the filter and the forward vehicle path estimate.

18. An apparatus for dynamically predicting road geometry as set forth in claim 1, wherein the at least one sensor is a device selected from a group consisting of imaging devices, GPS/map systems, laser sensors, and radars.

19. An apparatus for dynamically predicting road geometry as set forth in claim 1, wherein the output of the apparatus is configured for use by other automotive systems selected from a group consisting of driver assistance systems and vehicle safety systems.

20. A computer program product for dynamically predicting road geometry, the computer program product comprising means, stored on computer readable medium, for:

i. filtering current environmental data from at least one sensor and performing an estimation using the current environmental data, and outputting a filter output from each filter;

ii. weighing the filter output from the bank of filters and ascribing a weighted value to each filter output based on a relevance of the filter, thereby providing weighted outputs;

iii. fusing the weighted outputs into a single output;

whereby the output describes a vehicle forward path estimate.

21. A computer program product for dynamically predicting road geometry as set forth in claim 20, wherein means for filtering is further configured to receive a previous vehicle path estimate and to recursively perform the estimation based on the current environmental data and the previous vehicle path estimate.

22. A computer program product for dynamically predicting road geometry as set forth in claim 21, wherein filters used in the filtering step include a bank of Kalman filters.

23. A computer program product for dynamically predicting road geometry as set forth in claim 22, wherein the bank of filters includes three Kalman filters.

24. A computer program product for dynamically predicting road geometry as set forth in claim 23, wherein the bank of filters is configured based on a polynomial road model.

25. A computer program product for dynamically predicting road geometry as set forth in claim 24, wherein the polynomial road model is a two-clothoid road model.

26. A computer program product for dynamically predicting road geometry as set forth in claim 25, wherein each filter in the bank of filters is tuned to a different class of geometry within the two-clothoid road model.

27. A computer program product for dynamically predicting road geometry as set forth in claim 26, wherein the different classes of geometry include a constant geometry road section, an upcoming transition in the far range, and an upcoming transition in the near range.

28. A computer program product for dynamically predicting road geometry as set forth in claim 27, wherein the means for weighing ascribes weighted values based on the posterior probabilities of a fit between the filter and the forward vehicle path estimate.

29. A computer program product for dynamically predicting road geometry as set forth in claim 28, wherein the environmental data is received from at least one sensor is a device selected from a group consisting of imaging devices, GPS/map systems, laser sensors, and radars.

30. A computer program product for dynamically predicting road geometry as set forth in claim 29, wherein the output is configured for use by automotive systems selected from a group consisting of driver assistance systems and vehicle safety systems.

31. A computer program product for dynamically predicting road geometry as set forth in claim 20 wherein filters used in the filtering step include a bank of Kalman filters.

32. A computer program product for dynamically predicting road geometry as set forth in claim 20, wherein the configuration of the bank of filters is based on a polynomial road model.

33. A computer program product for dynamically predicting road geometry as set forth in claim 32, wherein the polynomial road model is a two-clothoid road model.

34. A computer program product for dynamically predicting road geometry as set forth in claim 20, wherein each filter is tuned to a different class of geometry within the two-clothoid road model.

35. A computer program product for dynamically predicting road geometry as set forth in claim 34, wherein the different classes of geometry include a constant geometry road section, an upcoming transition in the far range, and an upcoming transition in the near range.

36. A computer program product for dynamically predicting road geometry as set forth in claim 20, wherein the means for weighing ascribes weighted values based on the posterior probabilities of a fit between the filter and the forward vehicle path estimate.

37. A computer program product for dynamically predicting road geometry as set forth in claim 20, wherein the environmental data is received from at least one sensor is a device selected from a group consisting of imaging devices, GPS/map systems, laser sensors, and radars.

38. A computer program product for dynamically predicting road geometry as set forth in claim 20, wherein the output is configured for use by automotive systems selected from a group consisting of driver assistance systems and vehicle safety systems.

39. A method for dynamically predicting road geometry, the method comprising steps of:

i. filtering current environmental data from at least one sensor and performing an estimation using the current environmental data, and outputting a filter output from each filter;

ii. weighing the filter output from the bank of filters and ascribing a weighted value to each filter output based on a relevance of the filter, thereby providing weighted outputs;

iii. fusing the weighted outputs into a single output;

whereby the output describes a vehicle forward path estimate.

40. A method for dynamically predicting road geometry as set forth in claim 39, wherein step of filtering is tailored to receive a previous vehicle path estimate and to recursively perform the estimation based on the current environmental data and the previous vehicle path estimate.

41. A method for dynamically predicting road geometry as set forth in claim 40, wherein filters used in the filtering step include a bank of Kalman filters.

42. A method for dynamically predicting road geometry as set forth in claim 41, wherein the bank of filters includes three Kalman filters.

43. A method for dynamically predicting road geometry as set forth in claim 42, wherein the bank of filters is configured based on a polynomial road model.

44. A method for dynamically predicting road geometry as set forth in claim 43, wherein the polynomial road model is a two-clothoid road model.

45. A method for dynamically predicting road geometry as set forth in claim 44, wherein each filter in the bank of filters is tuned to a different class of geometry within the two-clothoid road model.

46. A method for dynamically predicting road geometry as set forth in claim 45, wherein the different classes of geometry include a constant geometry road section, an upcoming transition in the far range, and an upcoming transition in the near range.

47. A method for dynamically predicting road geometry as set forth in claim 46, wherein the step of weighing ascribes weighted values based on the posterior probabilities of a fit between the filter and the forward vehicle path estimate.

48. A method for dynamically predicting road geometry as set forth in claim 47, wherein the environmental data is received from at least one sensor is a device selected from a group consisting of imaging devices, GPS/map systems, laser sensors, and radars.

49. A method for dynamically predicting road geometry as set forth in claim 48, wherein the output is configured for use by automotive systems selected from a group consisting of driver assistance systems and vehicle safety systems.

50. A method for dynamically predicting road geometry as set forth in claim 39 wherein filters used in the filtering step include a bank of Kalman filters.

51. A method for dynamically predicting road geometry as set forth in claim 39, wherein the configuration of the bank of filters is based on a polynomial road model.

52. A method for dynamically predicting road geometry as set forth in claim 51, wherein the polynomial road model is a two-clothoid road model.

53. A method for dynamically predicting road geometry as set forth in claim 39, wherein each filter is tuned to a different class of geometry within the two-clothoid road model.

54. A method for dynamically predicting road geometry as set forth in claim 53, wherein the different classes of geometry include a constant geometry road section, an upcoming transition in the far range, and an upcoming transition in the near range.

55. A method for dynamically predicting road geometry as set forth in claim 39, wherein the step of weighing ascribes weighted values based on the posterior probabilities of a fit between the filter and the forward vehicle path estimate.

56. A method for dynamically predicting road geometry as set forth in claim 39, wherein the environmental data is received from at least one sensor is a device selected from a group consisting of imaging devices, GPS/map systems, laser sensors, and radars.

57. A method for dynamically predicting road geometry as set forth in claim 39, wherein the output is configured for use by automotive systems selected from a group consisting of driver assistance systems and vehicle safety systems.