Road curvature estimation system

• US 20050225477A1
• Filed: 12/24/2004
• Published: 10/13/2005
• Est. Priority Date: 07/15/2002
• Status: Active Grant

• Current Assignee: TK Holdings Incorporated (Ningbo Junsheng Investment Group Co., Ltd.)
• Original Assignee: TK Holdings Incorporated (Ningbo Junsheng Investment Group Co., Ltd.)
• Inventors: Hong, Lang, Shen, Shi, Cong, Shan
• Granted Patent: US 7,522,091 B2
• Time in Patent Office: Days
• US Class Current: 342/70
• CPC Class Codes: B60K 31/0008 including means for detecting potential obstacles in vehicle path
• CPC Class Codes: B60K 31/0066 responsive to vehicle path curvature
• CPC Class Codes: B60K 31/0083 responsive to centrifugal force acting on vehicle due to the path it is following
• CPC Class Codes: B60T 2210/24 Curve radius
• CPC Class Codes: B60T 2260/08 Coordination of integrated systems
• CPC Class Codes: B60W 2050/0052 Filtering, filters
• CPC Class Codes: B60W 2520/14 Yaw
• CPC Class Codes: B60W 2552/20 Road profile, i.e. the change in elevation or curvature of a plurality of continuous road segments
• CPC Class Codes: B60W 2552/30 Road curve radius
• CPC Class Codes: B60W 2554/4041 Position
• CPC Class Codes: B60W 2554/4042 Longitudinal speed
• CPC Class Codes: B60W 2554/4043 Lateral speed
• CPC Class Codes: B60W 2554/803 Relative lateral speed
• CPC Class Codes: B60W 2554/804 Relative longitudinal speed
• CPC Class Codes: G01C 21/26 specially adapted for navigation in a road network
• CPC Class Codes: G01S 13/58 Velocity or trajectory determination systems; Sense-of-movement determination systems
• CPC Class Codes: G01S 13/723 by using numerical data
• CPC Class Codes: G01S 13/86 Combinations of radar systems with non-radar systems, e.g. sonar, direction finder
• CPC Class Codes: G01S 13/931 of land vehicles
• CPC Class Codes: G01S 2013/932 using own vehicle data, e.g. ground speed, steering wheel direction
• CPC Class Codes: G01S 2013/9325 : for inter-vehicle distance regulation, e.g. navigating in platoons
• CPC Class Codes: G01S 2013/93271 : in the front of the vehicles
• CPC Class Codes: G01S 7/411 : Identification of targets based on measurements of radar reflectivity G01S7/415 takes precedence

Abstract

A processor using a first Kalman filter estimates a host vehicle state from speed and yaw rate, the latter of which may be from a yaw rate sensor if speed is greater than a threshold, and, if less, from a steer angle sensor and speed. Road curvature parameters are estimated from a curve fit of a host vehicle trajectory or from a second Kalman filter for which a state variable may be responsive to a plurality of host state variables. Kalman filters may incorporate adaptive sliding windows. Curvature of a most likely road type is estimated with an interacting multiple model (IMM) algorithm using models of different road types. A road curvature fusion subsystem provides for fusing road curvature estimates from a plurality of curvature estimators using either host vehicle state, a map database responsive to vehicle location, or measurements of a target vehicle with a radar system.

Claims

1. A road curvature estimation system, comprising:

a. a speed sensor adapted to measure a longitudinal speed of a vehicle on a roadway and to generate a measure of longitudinal speed responsive thereto;

b. a yaw rate sensor adapted to measure a yaw rate of said vehicle and to generate a first measure of yaw rate responsive thereto;

c. a steer angle sensor adapted to measure a steer angle associated with at least one steered wheel of said vehicle and to generate a first measure of steer angle responsive thereto; and

d. a processor operatively coupled to said speed sensor, to said angular rate sensor and to said steer angle sensor, wherein if said longitudinal speed of said vehicle is greater than a threshold, said processor is adapted to estimate a curvature of said roadway responsive to said first measure of yaw rate and to said measure of longitudinal speed, and if said longitudinal speed of said vehicle is less than a threshold, said processor is adapted to generate and estimate of at least one curvature parameter of said roadway responsive to said measure of steer angle and to said measure of longitudinal speed, regardless of the curvature of said roadway.

2. A road curvature estimation system as recited in claim 1, wherein said processor comprises at least one Kalman filter, and said at least one Kalman filter is adapted to generate an estimate of at least one curvature parameter responsive to said measure of longitudinal speed and at least one of said first measure of yaw rate and a second measure of yaw rate responsive to said measure of steer angle and said measure of longitudinal speed, wherein said at least one curvature parameter is representative of a curvature of said roadway.

3. A road curvature estimation system as recited in claim 1, wherein said at least one Kalman filter comprises first and second Kalman filters, said first Kalman filter is adapted generate an output comprising an estimate of a host state responsive to said measure of longitudinal speed and to said first or said second measure of said yaw rate, said host state comprises a yaw rate, a yaw acceleration, a longitudinal speed and a longitudinal acceleration and said second Kalman filter is adapted to generate said estimate of said at least one curvature parameter responsive to said output from said first Kalman filter.

4. A road curvature estimation system as recited in claim 1, wherein said processor comprises at least one Kalman filter, at least one said Kalman filter is adapted generate an output comprising an estimate of a trajectory of said vehicle responsive to said measure of longitudinal speed and to said first or said second measure of said yaw rate, and said processor is adapted to determine said at least one said estimate of said at least one curvature parameter from a curve fit of said host trajectory.

5. A road curvature estimation system as recited in claim 4, wherein said curve fit spans a sliding window.

6. A road curvature estimation system as recited in claim 5, wherein a length of said sliding window is adapted responsive to at least one said estimate of said at least one curvature parameter.

7. A road curvature estimation system, comprising:

a. a speed sensor adapted to measure a longitudinal speed of a vehicle on a roadway and to generate a measure of longitudinal speed responsive thereto;

b. a source of a measure of yaw rate of said vehicle; and

c. a processor operatively coupled to said speed sensor and to said source of said measure of yaw rate, wherein said processor comprises at least one Kalman filter, and said at least one Kalman filter is adapted to generate an estimate associated with at least one curvature parameter responsive to said measure of longitudinal speed and to said measure of yaw rate, wherein said at least one curvature parameter is representative of a curvature of said roadway and at least one said Kalman filter comprises a sliding window.

8. A road curvature estimation system as recited in claim 7, wherein a length of said sliding window is adapted responsive to at least one said estimate of said at least one curvature parameter.

9. A road curvature estimation system, comprising:

a. a speed sensor adapted to measure a longitudinal speed of a vehicle on a roadway and to generate a measure of longitudinal speed responsive thereto;

b. a source of a measure of yaw rate of said vehicle; and

c. a processor operatively coupled to said speed sensor and to said source of said measure of yaw rate, wherein said processor comprises first and second Kalman filters, said first Kalman filter is adapted generate an output comprising estimates of a plurality of host state variables responsive to said measure of longitudinal speed and said measure of said yaw rate, said plurality of host state variables comprise yaw rate, yaw acceleration, longitudinal speed and longitudinal acceleration;

said second Kalman filter is adapted to generate an estimate of at least one state variable responsive to a measurement calculated from a plurality of said host state variables, and said at least one curvature parameter is calculated from at least one state variable of said second Kalman filter.

10. A road curvature estimation system as recited in claim 9, wherein at least one said state variable of said second Kalman filter is related to a time derivative of curvature.

11. A road curvature estimation system, comprising:

a. a speed sensor adapted to measure a longitudinal speed of a vehicle on a roadway and to generate a measure of longitudinal speed responsive thereto;

b. a source of a measure of yaw rate of said vehicle; and

c. a processor operatively coupled to said speed sensor and to said source of said measure of yaw rate, wherein said processor comprises a plurality of curvature estimators associated with a corresponding plurality of different roadway models, each said curvature estimator is adapted to estimate at least one associated curvature parameter of a corresponding said different roadway model, said processor provides for selecting a most likely of said different roadway models as a most likely roadway model, and said processor provides for outputting said at least one curvature parameter of said most likely roadway model as an estimate of curvature of said roadway.

12. A road curvature estimation system as recited in claim 11, wherein said processor comprises an interacting multiple model algorithm incorporating a mode jump process characterized by a Markov chain.

13. A road curvature estimation system as recited in claim 12, wherein said interacting multiple model algorithm comprises:

a. a mode-conditioned filter initialization process;

b. a mode-conditioned state propagation process;

c. a combination of mode-conditioned state and covariance propagations;

d. a mode-conditioned update process;

e. a probability evaluation and update process; and

f. process to output state and covariance estimates.

14. A road curvature estimation system as recited in claim 11, wherein said plurality of different roadway models comprise at least two roadway models selected form a straight road model, a quadratic road model, and a clothoid road model.

15. A road curvature estimation system, comprising:

a. a plurality of road curvature estimation subsystems selected from a first road curvature estimation subsystem adapted to estimate a first set of at least one first curvature parameter responsive to a measure of longitudinal speed of a host vehicle on a rodway and a measure of yaw rate of said host vehicle, a second road curvature estimation subsystem adapted to estimate a second set of at least one second curvature parameter responsive to a measure of said second set of at least one second curvature parameter from a map database responsive to a measure of location of said host vehicle, and a third road curvature estimation subsystem adapted to estimate a third set of at least one third curvature parameter responsive to a radar measurement of a target vehicle traveling on said roadway; and

b. a processor adapted to fuse at least two of said first set of at least one first curvature parameter, said second set of at least one second curvature parameter, and said third set of at least one third curvature parameter so as to generate a fourth set of at least one fourth curvature parameter as an estimate of curvature of said roadway.

16. A road curvature estimation system as recited in claim 15, wherein said second road curvature estimation subsystem comprises:

a. a vehicle navigation system adapted to provide said measure of location of said host vehicle;

b. said map database adapted to provide said measure of said second set of at least one second curvature parameter responsive to said measure of location; and

c. a Kalman filter adapted to estimate said second set of at least one road curvature parameter.

17. A road curvature estimation system as recited in claim 15, wherein said third road curvature estimation subsystem comprises:

a. a radar sensor adapted to provide a measure of a trajectory of said target vehicle;

b. an extended Kalman filter adapted to provide a target state vector responsive to said measure of said trajectory of said target vehicle; and

c. a curvature filter adapted to generate said estimate of said third set of at least one third curvature parameter responsive to a measure of said third set of at least one third curvature parameter responsive to said target state vector.

18. A road curvature estimation system as recited in claim 15, wherein said processor generates said at least one fourth curvature parameter from a weight combination of at least two of said first set of at least one first curvature parameter, said second set of at least one second curvature parameter, and said third set of at least one third curvature parameter.

19. A road curvature estimation system as recited in claim 15, wherein said processor generates and error covariance associated with said at least one fourth curvature parameter from a weight combination of at least two of a first error covariance associated with said first set of at least one first curvature parameter, a second error covariance associated with said second set of at least one second curvature parameter, and a third error covariance associated with said third set of at least one third curvature parameter.