Road curvature estimation and automotive target state estimation system

• US 7,034,742 B2
• Filed: 07/15/2003
• Issued: 04/25/2006
• Est. Priority Date: 07/15/2002
• Status: Expired due to Term

• Current Assignee: Joyson Safety Systems Acquisition, LLC (Ningbo Junsheng Investment Group Co., Ltd.)
• Original Assignee: Automotive Systems Laboratory, Incorporated (Ningbo Junsheng Investment Group Co., Ltd.)
• Inventors: Hong, Lang, Shen, Shi, Cong, Shan
• Primary Examiner(s): Tarcza, Thomas H.
• Assistant Examiner(s): ALSOMIRI, ISAM A
• Application Number:
  US10/620,749
• Publication Number:
  US 20050179580A1
• Time in Patent Office: 1,015 Days
• Field of Search: 342 70- 73, 342/90, 342/107, 342/133, 342113-115, 342/159, 342160-162, 342/189, 342/195, 342/196, 701/300, 701/301
• 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/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: 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

Abstract

A first Kalman filter estimates true measures of yaw rate and vehicle speed from associated noisy measures thereof generated by respective sensors in a host vehicle, and a second Kalman filter estimates therefrom parameters of a clothoid model of road curvature. Measures of range, range rate, and azimuth angle from a target state estimation subsystem, e.g. a radar system, are processed by an extended Kalman filter to provide an unconstrained estimate of the state of a target vehicle. Associated road constrained target state estimates are generated for one or more roadway lanes, and are compared—either individually or in combination—with the unconstrained estimate. If a constrained target state estimate corresponds to the unconstrained estimate, then the state of the target vehicle is generated by fusing the unconstrained and constrained estimates; and otherwise is given by the unconstrained estimate alone.

Claims

1. A method of estimating a state of a target vehicle on a roadway, comprising:

a. generating an estimate of a curvature of the roadway;

b. estimating an unconstrained state and associated covariance thereof of the target vehicle;

c. establishing at least one prospective constraint of the target vehicle, wherein at least one prospective constraint is responsive to said estimate of the curvature of the roadway;

d. estimating at least one constrained state and associated covariance thereof of the target vehicle corresponding to said at least one prospective constraint of the target vehicle;

e. determining a most likely state of the target vehicle, wherein said most likely state of the target vehicle is selected from said unconstrained state of the target vehicle and said at least one constrained state of the target vehicle; and

f. if said at least one constrained state of the target vehicle is the most likely state, then fusing the unconstrained state and covariance thereof of the target vehicle with the associated constrained state and covariance of said most likely state and outputting at least one of the fused state and the associated fused covariance thereof of the target as the estimated state or covariance of the target;

otherwise outputting at least one of the unconstrained state and the associated unconstrained covariance thereof of the target as the estimated state or covariance of the target.

2. A method of estimating a state of a target vehicle on a roadway as recited in claim 1, wherein the operation of estimating the curvature of the roadway comprises:

a. generating a speed measurement by measuring a longitudinal speed of a host vehicle traveling on the roadway;

b. generating a yaw rate measurement by measuring or determining a yaw rate of said host vehicle; and

c. estimating at least one curvature parameter, wherein said at least one curvature parameter is representative of a curvature of the roadway, and the operation of estimating at least one curvature parameter is responsive to said speed measurement and is responsive to said yaw rate measurement.

3. A method of estimating a state of a target vehicle on a roadway as recited in claim 2, wherein said at least one curvature parameter comprises at least one parameter of a clothoid model of curvature, and said Clothoid model of curvature is referenced to a coordinate system of said host vehicle.

4. A method of estimating a state of a target vehicle on a roadway as recited in claim 3, wherein said at least one curvature parameter comprises first and second parameters of a clothoid model, wherein said first parameter is a constant, and said second parameter is a sensitivity of curvature to a distance along the roadway.

5. A method of estimating a state of a target vehicle on a roadway as recited in claim 2, wherein the operation of estimating at least one curvature parameter comprises processing said measurements of speed and yaw rate with at least one Kalman filter.

6. A method of estimating a state of a target vehicle on a roadway as recited in claim 5, wherein said at least one Kalman filter comprises first and second Kalman filters, said first Kalman filter is adapted to estimate a first set of state variables from said speed measurement and from said yaw rate measurement, and said second Kalman filter is adapted to estimate said at least one curvature parameter from said estimate of said first set of state variables.

7. A method of estimating a state of a target vehicle on a roadway as recited in claim 6, wherein said first set of state variables comprises vehicle velocity, vehicle acceleration, vehicle yaw rate and vehicle yaw acceleration.

8. A method of estimating a state of a target vehicle on a roadway as recited in claim 1, wherein the operation of estimating an unconstrained state and associated covariance of the target vehicle comprise:

a. measuring a range, a range rate and an azimuth of the target vehicle relative to a host vehicle; and

b. estimating said unconstrained state of the target vehicle from said measurements of the range, range rate and azimuth of the target vehicle relative to the host vehicle.

9. A method of estimating a state of a target vehicle on a roadway as recited in claim 1, further comprising the operation of transforming said unconstrained state of the target vehicle to a coordinate system corresponding to said estimate of the curvature of the roadway.

10. A method of estimating a state of a target vehicle on a roadway as recited in claim 1, wherein said at least one prospective constraint of the target vehicle is applied to a lateral position of the target vehicle, and a longitudinal position of the target vehicle is unconstrained.

11. A method of estimating a state of a target vehicle on a roadway as recited in claim 1, wherein said at least one prospective constraint of the target vehicle comprises a plurality of prospective constraints, and at least two different prospective constraints correspond to different lanes of the roadway.

12. A method of estimating a state of a target vehicle on a roadway as recited in claim 1, wherein the operation of determining the most likely state of the target vehicle comprises:

a. determining at least one likelihood value, wherein said a least one likelihood value corresponds to said at least one prospective constraint of the target vehicle being active and said at least one likelihood value is responsive to a corresponding at least one distribution function evaluated at a value corresponding to said unconstrained state of the target vehicle;

b. determining a probability of a first constrained state of the target vehicle, wherein said probability is responsive to said at least one likelihood value;

c. testing at least one hypothesis that said unconstrained state of the target vehicle corresponds to said first constrain state, and

d. identifying a most likely state of the target vehicle responsive to the operation of testing said at least one hypothesis.

13. A method of estimating a state of a target vehicle on a roadway as recited in claim 12, wherein the operation of determining the most likely state of the target vehicle comprises establishing at least one a priori probability of a transition from a first state to a second state of the target vehicle, wherein said first and second states can be either the same or different states, and, said probability of said first constrained state of the target vehicle is responsive to said at least one a priori probability.

14. A method of estimating a state of a target vehicle on a roadway as recited in claim 12, wherein said at least one distribution function is responsive to said covariance of the particular constrained state of the target vehicle.

15. A method of estimating a state of a target vehicle on a roadway as recited in claim 12, wherein said at least one constrained state comprises a plurality of constrained states, and said first constrained state comprises a combination of said plurality of constrained states.

16. A method of estimating a state of a target vehicle on a roadway as recited in claim 12, wherein said first constrained state comprises a most likely of said at least one constrained state of the target vehicle.

17. A method of estimating a state of a target vehicle on a roadway as recited in claim 12, wherein said first constrained state comprises one of said at least one constrained state of the target vehicle.

18. A method of estimating a state of a target vehicle on a roadway as recited in claim 12, wherein the operation of testing at least one hypothesis is responsive to the most recent estimates of said state of the target vehicle.

19. A method of estimating a state of a target vehicle on a roadway as recited in claim 12, wherein the operation of testing at least one hypothesis comprises identifying a particular constrained state of the target vehicle corresponding to a most likely of said at least one constrained state of the target vehicle, calculating a value of a function responsive to a difference between said strained state and said particular constrained state, and comparing said value of said function with a threshold, whereby said hypothesis is satisfied if said value of said function is less than said threshold.

20. A method of estimating a state of a target vehicle on a roadway as recited in claim 19, wherein said threshold is responsive to the particular constraint associated with said particular constrained state of the target vehicle.

21. A method of estimating a state of a target vehicle on a roadway as recited in claim 20, wherein said threshold is relatively lower for said particular constrained state corresponding to an increased threat by the target vehicle to host vehicle.

22. A method of estimating a state of a target vehicle on a roadway as recited in claim 21, wherein said particular constrained state corresponding to an increased threat by the target vehicle to the host vehicle is selected from the target vehicle moving in the same lane as the host vehicle, and the target vehicle changing lanes to said same lane as the host vehicle.