Road curvature estimation system Number:7,522,091 from the United States Patent and Trademark Office (PTO) owispatent

Title: Road curvature estimation system

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.

Patent Number: 7,522,091 Issued on 04/21/2009 to Cong,   et al.

---

Inventors:	Cong; Shan (Ann Arbor, MI), Shen; Shi (Farmington Hills, MI), Hong; Lang (Beavercreek, OH)
Assignee:	Automotive Systems Laboratory, Inc. (Farmington Hills, MI)
Appl. No.:	11/022,265
Filed:	December 24, 2004

---

Related U.S. Patent Documents

---

	Application Number	Filing Date	Patent Number	Issue Date
	10620749	Jul., 2003	7034742
	60532344	Dec., 2003
	60396211	Jul., 2002

---

Current U.S. Class:	342/70 ; 342/107; 342/115; 342/161; 342/90; 701/300; 701/301
Current International Class:	G01S 13/93 (20060101); G08G 1/16 (20060101)
Field of Search:	342/70-74,90,107,113-115,133,159,160-162,189,195,196 701/300,301

---

References Cited [Referenced By]

---

U.S. Patent Documents

2709804	May 1955	Chance et al.
3177485	April 1965	Taylor, Jr.
3603994	September 1971	Williams et al.
3699573	October 1972	Andrews et al.
3725918	April 1973	Fleischer et al.
3869601	March 1975	Metoalf
3971018	July 1976	Isbister et al.
4623966	November 1986	O'Sullivan
5051751	September 1991	Gray
5138321	August 1992	Hammer
5170440	December 1992	Cox
5202691	April 1993	Hicks
5307289	April 1994	Harris
5314037	May 1994	Shaw et al.
5343206	August 1994	Ansaldi et al.
5402129	March 1995	Gellner et al.
5406289	April 1995	Barker et al.
5471214	November 1995	Faibish et al.
5530651	June 1996	Uemura et al.
5537119	July 1996	Poore, Jr.
5539397	July 1996	Asanuma et al.
5587929	December 1996	League et al.
5594414	January 1997	Namngani
5598164	January 1997	Reppas et al.
5627768	May 1997	Uhlmann et al.
5631639	May 1997	Hibino et al.
5633642	May 1997	Hoss et al.
5657251	August 1997	Fiala
5668739	September 1997	League et al.
5684473	November 1997	Hibino et al.
5689264	November 1997	Ishikawa et al.
5703593	December 1997	Campbell et al.
5710565	January 1998	Shirai et al.
5751211	May 1998	Shirai et al.
5926126	July 1999	Engelman
5948043	September 1999	Mathis
5955967	September 1999	Yamada
5959552	September 1999	Cho
5959574	September 1999	Poore, Jr.
5983161	November 1999	Lemelson et al.
6070121	May 2000	Matsuda
6085151	July 2000	Farmer et al.
6134509	October 2000	Furusho et al.
6141617	October 2000	Matsuda et al.
6161071	December 2000	Shuman et al.
6198426	March 2001	Tamatsu et al.
6226389	May 2001	Lemelson et al.
6265991	July 2001	Nishiwaki et al.
6268825	July 2001	Okada
6275231	August 2001	Obradovich
6275773	August 2001	Lemelson et al.
6282478	August 2001	Akita
6292752	September 2001	Franke et al.
6300865	October 2001	Fechner et al.
6343253	January 2002	Matsuura et al.
6370475	April 2002	Breed et al.
6374184	April 2002	Zahm et al.
6393376	May 2002	Andreas
6405132	June 2002	Breed et al.
6420997	July 2002	Cong
6424904	July 2002	Takahashi et al.
6470272	October 2002	Cong et al.
6542111	April 2003	Wilson
6567039	May 2003	Shirai et al.
6643588	November 2003	Ibrahim
6670910	December 2003	Delcheccolo et al.
6675090	January 2004	Matsuura
6675094	January 2004	Russell et al.
6718259	April 2004	Khosla
6751547	June 2004	Khosla
6763318	July 2004	Winter et al.
6823241	November 2004	Shirato et al.
2002/0022927	February 2002	Lemelson et al.
2002/0042668	April 2002	Shirato et al.
2002/0044080	April 2002	Shirai et al.
2002/0049539	April 2002	Russell et al.
2003/0100992	May 2003	Khosla
2003/0195703	October 2003	Ibrahim
2003/0218563	November 2003	Miyahara
2004/0143416	July 2004	Hattori et al.
2004/0183663	September 2004	Shimakage
2005/0179580	August 2005	Cong et al.

Other References

Dickmanns, E.D.; Mysliwetz, B.D.; "Recursive 3-D Road and Relative Ego-State Recognitiion," IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 14, No. 2, Feb. 1992, pp. 199-213. cited by other . Hsu, J.C.; Chen, W. L.; Lin, R. H; Yeh, E. C.; "Estimations of previewed road curvatures and vehicular motion by a vision-based data fusion scheme," Machine Vision and Applications, vol. 9, 1997, p. 179-192. cited by other . Barber, P.A.; King, P.; Richardson, M.; "Road lane trajectory estimation using yaw rate gryoscopes for intelligent vehicle control," Transactions on the Institute of Measurement and Control, vol. 20, No. 2, 1998, pp. 59-66. cited by other . Schiffmann, J.K.; Widmann, G.R.; "Model-Based Scene Tracking Using Radar Sensors for Intelligent Automotive Vehicle Systems," IEEE International Radar Conference, 1998, pp. 421-426. cited by other . Jilkov, V.P.; Angelova, D.S.; Semerdjiev, Tz.A.; "Design and Comparison of Mode-Set Adaptive IMM Algorithms for Maneuvering Target Tracking," IEEE Transactions on Aerospace and Electronic Systems, vol. 35, No. 1, Jan. 1999, pp. 343-350. cited by other . Tsang, S. H.;. Hoare, E. G; Hall, P. S.; Clarke, N. J.; "Automotive Radar Image Processing To Predict Vehicle Trajectory," IEEE International Radar Conference, 1999, p. 867-870. cited by other . Jula, H.; Kosmatopoulos, E.; Ioannou, P.; "Collision Avoidance Analysis for Lane Changing and Merging," Technical Report, University of Southern California, 1999. cited by other . Lin, C-F.; Ulsoy, A.G.; LeBlanc, D.J.; "Vehicle Dynamics and External Disturbance Estimation for Vehicle Path Prediction," IEEE Transactions on Control Systems Technology, vol. 8, No. 3, May 2000, pp. 508-518. cited by other . Ke, C-C.; Herrero, J.G.; Llinas, J.; "Comparative Analysis of Alternative Ground Target Tracking Techniques," in International Conference on Information Fusion, vol. 2, 2000, pp. WeB5:3-10. cited by other . Miyahara, S.; "A Method for Radar-Based Target Tracking in Non-Uniform Road Condition," SAE 2003 World Congress, 2003-01-0013, 2003. cited by other . Karlsson, R.; Jansson, J.; Gustafsson, F.; "Model-based statistical tracking and decision making for collision avoidance application," Technical Report, Linkoping University, LiTH-ISY-R-2492, Feb. 2003. cited by other . Coue, C.; Fraichard, Th.; Bessiere, P.; Mazer, E.; "Using Bayesian Programming for Multi-Sensor Multi-Target Tracking in Automotive Applications," Proceedings of the IEEE Int. Conf. on Robotics and Automation, Taipei, Taiwan, May 2003. cited by other . E. D. Dickmanns, A. Zapp. A curvature-based scheme for improving road vehicle guidance by computer vision. Mobile Robots. Cambridge, MA: SPIE, 1986; vol. 727, p. 161-168. cited by other . J. Goldbeck, B. Huertgen, S. Ernst, L. Kelch. Lane following combining vision and DGPS. Image and Vision Computing, vol. 18, 2000, p. 425-433. cited by other . S. Shen, A Multisensor-Based Automotive Collision Prediction System, Ph.D. Dissertation, Wright State University, Oct. 2002. cited by other . International Search Report in PCT/US03/22182, Jul. 1, 2004. cited by other.

Primary Examiner: Alsomiri; Isam
Attorney, Agent or Firm: Raggio & Dinnin, P.C.

---

Parent Case Text

---

CROSS-REFERENCE TO RELATED APPLICATIONS

The instant application is a continuation-in-part of U.S. application Ser. No. 10/620,749 filed on 15 Jul. 2003, now U.S. Pat. No. 7,034,742 which claims the benefit of prior U.S. Provisional Application Ser. No. 60/396,211 filed on Jul. 15, 2002. The instant application also claims the benefit of prior U.S. Provisional Application Ser. No. 60/532,344 filed on Dec. 24, 2003. The above-identified applications are incorporated herein by reference in their entirety.

---

Claims

---

What is claimed is:

1. 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 roadway 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.

2. A road curvature estimation system as recited in claim 1, 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.

3. A road curvature estimation system as recited in claim 1, 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.

4. A road curvature estimation system as recited in claim 1, 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.

5. A road curvature estimation system as recited in claim 1, 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.

---

Description

---

BRIEF DESCRIPTION OF THE DRAWINGS

In the accompanying drawings:

FIG. 1 illustrates a block diagram of hardware associated with a predictive collision sensing system;

FIG. 2 illustrates a coverage pattern of a radar beam used by the predictive collision sensing system;

FIG. 3 depicts a driving scenario for purposes of illustrating the operation of the predictive collision sensing system;

FIG. 4 illustrates a block diagram of the hardware and an associated signal processing algorithm of the predictive collision sensing system;

FIG. 5 illustrates a flow chart of an associated signal processing algorithm of the predictive collision sensing system;

FIG. 6 illustrates a geometry used for determining curvature parameters of a roadway;

FIG. 7 illustrates the geometry of an arc;

FIGS. 8a-d illustrates an example of the estimation of target position, lateral velocity, and road curvature parameters for a straight roadway;

FIGS. 9a-b illustrate an example of the target state RMS errors from unconstrained and constrained filtering on the straight roadway, corresponding to FIGS. 8a-d;

FIGS. 10a-d illustrate an example of the estimation of target position, lateral velocity, and road curvature parameters for a curved roadway;

FIGS. 11a-b illustrate an example of the target state RMS errors from unconstrained and constrained filtering for the curved roadway, corresponding to FIGS. 10a-d;

FIGS. 12a-d illustrate an example of the estimation of target position, lateral velocity, and associated RMS errors for a straight roadway involving a lane change;

FIGS. 13a-d illustrates an example of the estimation of target position, lateral velocity, and their RMS errors for a curved roadway involving a lane change;

FIG. 14 illustrates a block diagram of hardware associated with another embodiment of a predictive collision sensing system;

FIG. 15 illustrates a free-body diagram of a steered wheel;

FIG. 16a illustrates a geometry of a bicycle model of a vehicle undergoing a turn;

FIG. 16b illustrates a geometry of the steered wheel illustrated in FIG. 16a.;

FIG. 17 illustrates a switching curve;

FIG. 18 illustrates a flow chart of a process associated with the switching curve illustrated in FIG. 17;

FIG. 19 illustrates a block diagram of a road curvature estimation subsystem for estimating road curvature from host vehicle state estimates;

FIG. 20 illustrates a curvature filter associated with a first embodiment of a curvature estimator;

FIG. 21 illustrates a curvature filter associated with a fourth embodiment of a curvature estimator;

FIG. 22 illustrates various types of roads and associated road models;

FIG. 23 illustrates a block diagram of a tenth embodiment of a road curvature estimation subsystem;

FIG. 24 illustrates a flow chart of an interacting multiple model algorithm;

FIG. 25 illustrates a block diagram of a curvature estimation subsystem responsive to vehicle location and associated road curvature data from an associated map system;

FIG. 26 illustrates a block diagram of a curvature estimation subsystem responsive to radar measurements of a target vehicle on the roadway; and

FIG. 27 illustrates a block diagram of a predictive collision sensing system comprising a plurality of road curvature estimation subsystems and an associated road curvature fusion subsystem.

DESCRIPTION OF EMBODIMENT(S)

Referring to FIG. 1, a predictive collision sensing system 10 incorporated in a host vehicle 12, comprises a radar system 14 for sensing objects external to the host vehicle 12, and a set of sensors, including a yaw rate sensor 16, e.g. a gyroscopic sensor, and a speed sensor 18, for sensing motion of the host vehicle 12. The yaw rate sensor 16 and speed sensor 18 respectively provide measurements of the yaw rate and speed of the host vehicle 12. The radar system 14, e.g. a Doppler radar system, comprises an antenna 20 and a radar processor 22, wherein the radar processor 22 generates the RF signal which is transmitted by the antenna 20 and which is reflected by objects in view thereof. The radar processor 22 demodulates the associated reflected RF signal that is received by the antenna 20, and detects a signal that is responsive to one or more objects that are irradiated by the RF signal transmitted by the antenna 20. For example, the radar system 14 provides target range, range rate and azimuth angle measurements in host vehicle 12 fixed coordinates. Referring to FIG. 2, the antenna 20 is adapted to generate a radar beam 23 of RF energy that is, for example, either electronically or mechanically scanned across an azimuth range, e.g. +/-.gamma., e.g. +/-50 degrees, responsive to a beam control element 24, and which has a distance range, e.g. about 100 meters, from the host vehicle 12 that is sufficiently far to enable a target to be detected sufficiently far in advance of a prospective collision with the host vehicle 12 so as to enable a potentially mitigating action to be taken by the host vehicle 12 so as to either avoid the prospective collision or mitigate damage or injury as a result thereof. The radar processor 22, yaw rate sensor 16, and speed sensor 18 are operatively connected to a signal processor 26 that operates in accordance with an associated predictive collision sensing algorithm to determine whether or not a collision with an object, e.g. a target vehicle 36 (illustrated in FIG. 3), is likely, and if so, to also determine an action to be taken responsive thereto, for example, one or more of activating an associated warning system 28 or safety system 30 (e.g. frontal air bag system), or using a vehicle control system 32 (e.g. an associated braking or steering system) to take evasive action so as to either avoid the prospective collision or to reduce the consequences thereof.

Referring to FIG. 3, the host vehicle 12 is shown moving along a multiple lane roadway 34, either straight or curved, and there is also shown a target vehicle 36 moving in an opposite direction, towards the host vehicle 12. Generally, there can be any number of target vehicles 36 that can fit on the roadway 34, each moving in the same or opposite direction as the host vehicle 12. These target vehicles 36 can either be in the host lane 38 or in a neighboring lane 40 either adjacent to or separated from the host lane 38, but generally parallel thereto. For purposes of analysis, it is assumed that the host vehicle 12 moves along the center line 41 of its lane 38 steadily without in-lane wandering, and the road curvatures of all the parallel lanes 38, 40 are the same. Road curvature is assumed small such that the differences between the heading angles of the host vehicle 12 and any detectable target vehicles 36 are smaller than 15 degrees.

Referring to FIG. 4, the predictive collision sensing system 10 uses the measurements of speed U.sup.h and yaw rate .omega..sup.h of the host vehicle 12 from the speed sensor 18 and the yaw rate sensor 16 respectively therein; and the measurements of target range r, range rate {dot over (r)} and azimuth angle .eta. for all target vehicles 36 from the radar system 14 mounted on the host vehicle 12; along with the corresponding error covariance matrices of all these measurements, to estimate each target's two dimensional position, velocity and acceleration [x, {dot over (x)}, {umlaut over (x)}, y, {dot over (y)}, ]' in the host fixed coordinate system at every sampling instance, preferably with an error as small as possible. The predictive collision sensing system 10 comprises 1) a road curvature estimation subsystem 42 for estimating the curvature of the roadway 34 using measurements from the host vehicle motion sensors, i.e. the yaw rate sensor 16 and speed sensor 18; 2) an unconstrained target state estimation subsystem 44 for estimating the state of a target illuminated by the radar beam 23 and detected by the radar processor 22; 3) a constrained target state estimation subsystem 46 for estimating the state of the constraint on the target, assuming that the target is constrained to be on the roadway 34, either in the host lane 38 or in a neighboring lane 40, for each possible lane 38, 40; 4) a target state decision subsystem 48 for determining whether the best estimate of the target state is either the unconstrained target state, or a target state constrained by one of the constraints; and 5) a target state fusion subsystem 50 for fusing the unconstrained target state estimate with the appropriate constraint identified by the target state decision subsystem 48 so as to generate a fused target state. The best estimate of target state--either the unconstrained target state or the fused target state--is then used by a decision or control subsystem for determining whether or not the host vehicle 12 is at risk of collision with the target, and if so, for determining and effecting what the best course of action is to mitigate the consequences thereof, e.g. by action of either the warning system 28, the safety system 30, or the vehicle control system 32, or some combination thereof. When possible, the use of the geometric structure of the roadway 34 as a constraint to the target kinematics provides for a more accurate estimate of the target state, which thereby improves the reliability of any actions taken responsive thereto.

Referring also to FIG. 5, illustrating a method 500 of detecting the state, i.e. kinematic state variables, of a target in view of the host vehicle 12, the steps of which are, for example, carried out by the signal processor 26, in steps (502) and (504), the speed U.sup.h and yaw rate .omega..sup.h of the host vehicle 12 relative to the roadway 34 are respectively read from the speed sensor 18 and the yaw rate sensor 16 respectively. Then, in step (506), the curvature parameters and associated covariance thereof of the roadway 34 are estimated using first 52 and second 54 Kalman filters that respectively estimate the state (i.e. kinematic state variables of the host vehicle 12) and associated covariance thereof of the host vehicle 12, and then the curvature parameters and associated covariance thereof of the roadway 34, as described hereinbelow, wherein the curvature parameters and associated covariance thereof of the roadway 34 are then subsequently used by the constrained target state estimation subsystem 46 to generate associated constraints on the possible location of a prospective target vehicle 36.

A well-designed and constructed roadway 34 can be described by a set of parameters, including curvature, wherein the curvature of a segment of the roadway 34 is defined as:

##EQU00001## where R is the radius of the segment. In general, for a piece of smooth roadway 34, the curvature variation can be described as a function of a distance l along the roadway 34 by a so-called clothoid model, i.e.:

dd.times..times. ##EQU00002## where C.sub.1=1/A.sup.2 and A is referred to as the clothoid parameter.

Referring to FIG. 6, the heading angle .theta. defining the heading direction is given by:

.theta..theta..intg..times..function..tau..times..times.d.tau. ##EQU00003##

Substituting equation (2) into equation (3) gives .DELTA..theta.=.theta.-.theta..sub.0=C.sub.0l+C.sub.1l.sup.2/2 (4)

Referring to FIG. 6, the equation of the roadway 34, i.e. the road equation, in x-y coordinates is given by:

.intg..times..times..times..theta..function..tau..times.d.tau..times..time- s..intg..times..times..times..theta..function..tau..times..times.d.tau. ##EQU00004##

Assuming the heading angle .theta. to be within 15 degrees, i.e. |.theta.|<15.degree., equations (5) and (6) can be approximated by: .DELTA.x=x-x.sub.0.apprxeq.l (7)

.DELTA..times..times..apprxeq..times..times..apprxeq..times..DELTA..times.- .times..times..DELTA..times..times. ##EQU00005##

Accordingly, the roadway 34 is modeled by an incremental road equation in terms of curvature coefficients (or parameters): C.sub.0 and C.sub.1. This incremental road equation describes a broad range of road shapes as follows: 1) Straight roadway 34: C.sub.0=0 and C.sub.1=0; 2) circular roadway 34: C.sub.1=0; and 3) a general roadway 34 with an arbitrary shape for which the change in heading angle .theta. is less than 15 degrees: C.sub.0>0.

The road curvature parameters C.sub.0 and C.sub.1 are estimated using data from motion sensors (yaw rate sensor 16 and speed sensor 18) in the host vehicle 12, based upon the assumption that the host vehicle 12 moves along the center line 41 of the roadway 34 or associated host lane 38.

The road curvature parameters C.sub.0 and C.sub.1 can be calculated from data of .omega., {dot over (.omega.)}, U, {dot over (U)} responsive to measurements of yaw rate .omega..sup.h and speed U.sup.h of the host vehicle 12 from the available host vehicle 12 motion sensors. However, generally the measurements of yaw rate .omega..sup.h and speed U.sup.h, from the yaw rate sensor 16 and speed sensor 18 respectively, are noisy. A host state filter implemented by a first Kalman filter 52 is beneficial to generate estimates of .omega., {dot over (.omega.)}, U, {dot over (U)} from the associated noisy measurements of yaw rate .omega..sup.h and speed U.sup.h; after which a curvature filter implemented by a second Kalman filter 54 is used to generate smoothed estimates of the curvature parameters C.sub.0 and C.sub.1. The dynamics of the host vehicle 12 for the host state filter follows a predefined set of kinematic equations (constant velocity in this case) given by: x.sub.k+1.sup.h=F.sub.k.sup.hx.sub.k.sup.h+w.sub.k.sup.h, w.sub.k.sup.h.about.N(0, Q.sub.k.sup.h) (9) z.sub.k.sup.h=H.sub.k.sup.hx.sub.k.sup.h+v.sub.k.sup.h, v.sub.k.sup.h.about.N(0, R.sub.k.sup.h) (10) where

.omega..omega..times..times..times..times..omega. ##EQU00006## and where T is the sampling period, superscript (.cndot.).sup.h is used to indicate that the filter is the host filter, and U.sup.h and .omega..sup.h are host vehicle 12 speed and yaw rate measurements. The first Kalman filter 52 is implemented to estimate the host state {circumflex over (x)}.sub.k|k.sup.h and its error covariance as illustrated in FIG. 4.

The estimate of the host state from the first Kalman filter 52, i.e. the host state filter, is then used to generate a synthetic measurement that is input to the second Kalman filter 54, i.e. curvature coefficient (or parameter) filter, wherein the associated Kalman filters 52, 54 operate in accordance with the Kalman filtering process described more fully in the Appendix hereinbelow. The relationship between the road curvature parameters C.sub.0, C.sub.1 and the host state variables .omega., {dot over (.omega.)}, U, {dot over (U)} is derived as follows:

From equation (4), the radius R of road curvature is expressed generally as a function R(l) of the distance l along the roadway, as is illustrated in FIG. 7. Taking the time derivative on both sides of equation (4) yields: {dot over (.theta.)}=C.sub.0{dot over (l)}+C.sub.1l{dot over (l)}=(C.sub.0+C.sub.1l){dot over (l)}. (12)

Noting that {dot over (.theta.)}=.omega., the yaw rate of the host vehicle 12, and that {dot over (l)}=U, the speed of the host vehicle 12, and substituting the clothoid model of equation (2) in equation (12), yields: .omega.=CU (13) or

.omega. ##EQU00007##

Clothoid parameter C.sub.0 is given as the value of curvature C at l=0, or

.times..omega. ##EQU00008##

Taking the derivative on both sides of equation (14) yields

.omega..omega. ##EQU00009##

Using the definition of C.sub.1, from equation (2), C.sub.1 may be expressed in terms of the host state as follows:

dddddd.omega..omega. ##EQU00010##

The system equations for the second Kalman filter 54, i.e. the curvature filter, that generates curvature estimates C.sub.0.sub.k|k and C.sub.1.sub.k|k, are given by x.sub.k+1.sup.C=F.sub.k.sup.Cx.sub.k.sup.C+w.sub.k.sup.C, w.sub.k.sup.C.about.N(0, Q.sub.k.sup.C) (18) z.sub.k.sup.C=H.sup.Cx.sub.k.sup.C+v.sub.k.sup.C, v.sub.k.sup.C.about.N(0, R.sub.k.sup.C) (19) where

.DELTA..times..times..DELTA..times..times..times..times. ##EQU00011## .DELTA.t is the update time period of the second Kalman filter 54, and the values of the elements of the measurement vector z.sub.k.sup.C are given by the corresponding values of the state variables--i.e. the clothoid parameters C.sub.0 and C.sub.1--of the curvature filter.

The measurement, z.sub.k.sup.C, is transformed from the estimated state [, {dot over ()}, {circumflex over (.omega.)}, {dot over ({circumflex over (.omega.)})}].sub.k.sup.T as follows:

.omega..omega..omega. ##EQU00012## and the associated covariance of the measurements is given by: R.sub.k.sup.C=J.sub.k.sup.CP.sub.k|k.sup.h(J.sub.k.sup.C).sup.T (22) where

.differential..differential..times..omega..omega..omega..omega. ##EQU00013##

It should be understood that other systems and methods for estimating the curvature parameters of the roadway 34 may be substituted in the road curvature estimation subsystem 42 for that described above. For example, the curvature parameters of the roadway may also be estimated from images of the roadway 34 by a vision system, either instead of or in conjunction with the above described system based upon measurements of speed U.sup.h and yaw rate .omega..sup.h from associated motion sensors. Furthermore, it should be understood that yaw rate can be either measured or determined in a variety of ways, or using a variety of means, for example, but not limited to, using a yaw gyro sensor, a steering angle sensor, a differential wheel speed sensor, or a GPS-based sensor; a combination thereof; or functions of measurements therefrom (e.g. a function of, inter alia, steering angle rate).

Referring again to FIG. 5, in step (508), the measurements of target range r, range rate {dot over (r)}, and azimuth angle .eta. are read from the radar processor 22, and are used as inputs to an extended Kalman filter 56, i.e. the main filter, which, in step (510), generates estimates of the unconstrained target state--i.e. the kinematic state variables of the target--which estimates are relative values in the local coordinate system of the host vehicle 12 (i.e. the host-fixed coordinate system) which moves with therewith. In step (512), the unconstrained target state, i.e. the target velocity and acceleration, is transformed to absolute coordinates of the absolute coordinate system fixed on the host vehicle 12 at the current instant of time as illustrated in FIG. 3, so as to be consistent with the absolute coordinate system in which the road constraint equations are derived and for which the associated curvature parameters are assumed to be constant, when used in the associated constraint equations described hereinbelow in order to generate estimates of the constrained target state. The absolute coordinate system superimposes the moving coordinate system in space at the current instant of time, so that the transformation in step (512) is realized by adding velocity and acceleration related correction terms--accounting for the motion of the host vehicle 12--to the corresponding target estimates, in both x and y directions.

The result from the coordinate transformation in step (512) of the output from the extended Kalman filter 56 is then partitioned into the following parts, corresponding respectively to the x and y position of the target vehicle 36 relative to the host vehicle 12, wherein the superscript 1 refers to the unconstrained target state of the target vehicle 36:

.times..times..times..times. ##EQU00014##

Referring again to FIG. 5, following steps (506) and (512), in steps (514) through (524) described more fully hereinbelow, various constraints on the possible trajectory of the target vehicle 36 are applied and tested to determine if the target vehicle 36 is likely traveling in accordance with one of the possible constraints. For example, the constraints are assumed to be from a set of lanes that includes the host lane 38 and possible neighboring lanes 40, and a target vehicle 36 that is likely traveling in accordance with one of the possible constraints would likely be traveling on either the host lane 38 or one of the possible neighboring lanes 40. In step (524), the hypothesis that the target vehicle 36 is traveling on either the host lane 38 or one of the possible neighboring lanes 40 is tested for each possible lane. If the hypothesis is not satisfied for one of the possible lanes, then, in step (526), the state of the target is assumed to be the unconstrained target state, which is then used for subsequent predictive crash sensing analysis and control responsive thereto. Otherwise, from step (524), in step (528), the target state is calculated by the target state fusion subsystem 50 as the fusion of the unconstrained target state with the associated state of the constraint that was identified in step (524) as being most likely.

Prior to discussing the process of steps (514) through (524) for determining whether the target is likely constrained by a constraint, and if so, what is the most likely constraint, the process of fusing the unconstrained target state with state of a constraint will first be described for the case of a target vehicle 36 moving in the same lane as the host vehicle 12. The constraints are applied in the y-direction and are derived from road equations where y-direction state variables are functions of x-direction state variables, consistent with the assumptions that the host vehicle 12 moves along the center line 41 of its lane 38 steadily without in-lane wandering and that the road curvatures of all the parallel lanes 38, 40 are the same, and given that the absolute coordinate system is fixed on the host vehicle 12 at the current instant of time. Assuming the target vehicle 36 is moving in the same lane 38 as the host vehicle 12, and using the road constraint equation with the estimated coefficients (or parameters), in step (514), the constraint state variables are then given in terms of the lateral kinematic variable as:

.function..function..times..times..function..times..function..times..times- ..times..function..function..times..times..times..times..function..times..- function..times..times..times..function..times..times..times..times..funct- ion..times..function..times..times..times..times..times..times..times..tim- es..function..times..times. ##EQU00015##

In step (528), the two y-coordinate estimates, one from the main filter and the other from the road constraint, are then fused as follows:

.times..times..times..times..times..times..times..times..times..times..tim- es..times..times. ##EQU00016##

Finally, the composed estimate of the target state is

.times..times.' ##EQU00017## where P.sub.xy.sub.t=P.sub.x.sub.t(A.sub.k.sup.1)' (33)

In step (530), this composed estimate would then be output as the estimate of the target state if the target vehicle 36 were to be determined from steps (514) through (524) to be traveling in the host lane 38.

Returning to the process of steps (514) through (524) for determining whether the target is likely constrained by a constraint, and if so, what is the most likely constraint; according to the assumption that targets follow the same roadway 34, if the target vehicle 36 were known to travel in a particular lane, it would desirable to use estimated road parameters for that lane as a constraint in the main filter of estimating target kinematics. However, the knowledge of which lane the target vehicle 36 is current in is generally not available, especially when the target is moving on a curved roadway 34. Since the road equation (8) is only for the host lane 38 in the host-centered coordinate system, constrained filtering would require knowing which lane the target is in, and different constraint equations would be needed for different lanes. Ignoring the difference of road curvature parameters among these parallel lanes, i.e. assuming the curvature of each lane to be the same, the road equation for an arbitrary lane can be written as:

.times..times..+-..+-. ##EQU00018## where B is the width of the lanes and m represents the lane to be described (m=0 corresponds the host lane 38, m=1 corresponds the right neighboring lane 40, m=-1 corresponds the left neighboring lane 40, and so on). Without the prior knowledge of the target lane position, each of the multiple constraints forming a multiple constraint system (analogous to the so-called multiple model system) is tested to determine which, if any, of the constraints are active. A multiple constraint (MC) system is subjected to one of a finite number N.sup.C of constraints. Only one constraint can be in effect at any given time. Such systems are referred to as hybrid--they have both continuous (noise) state variables as well as discrete number of constraints.

The following definitions and modeling assumptions are made to facilitate the solution of this problem:

Constraint equations: y.sub.t.sub.k=f.sub.t.sub.k(x.sub.t.sub.k) (35)

where f.sub.t.sub.k denotes the constraint at time t.sub.k in effect during the sampling period ending at t.sub.k.

Constraint: among the possible N.sup.C constraints f.sub.t.sub.k.epsilon.{f.sup.j}.sub.j=1.sup.N.sup.C (36)

y.sub.t.sub.k|kt.sup.j: state estimate at time t.sub.k using constraint f.sub.t.sub.k.sup.j

P.sub.yt.sub.k|k.sup.j, P.sub.xyt.sub.k|k.sup.j: covariance matrix at time t.sub.k under constraint f.sub.t.sub.k.sup.j

.mu..sub.t.sub.k-1.sup.j: probability that the target is following constraint j at time t.sub.k-1

Constraint jump process: is a Markov chain with known transition probabilities P{f.sub.t.sub.k=f.sup.j|f.sub.t.sub.k-1=f.sup.i}=p.sub.ij. (37)

To implement the Markov model--for systems with more than one possible constraint state--it is assumed that at each scan time there is a probability p.sub.ij that the target will make the transition from constraint state i to state j. These probabilities are assumed to be known a priori and can be expressed in the probability transition matrix as shown below.

.times..times..times..times..times..times..times..times..function. ##EQU00019##

The prior probability that f.sup.j is correct (f.sup.j is in effect) is P(f.sup.j|Z.sup.0)=.mu..sub.t.sub.0.sup.j j=1, . . . , N.sup.C (39) where Z.sup.0 is the prior information and

.times..mu. ##EQU00020## since the correct constraint is among the assumed N.sup.C possible constraints.

The constrained target state estimation subsystem 46 provides for determining whether the target state corresponds to a possible constrained state, and if so, then provides for determining the most likely constrained state.

A multiple constraint (MC) estimation algorithm mixes and updates N.sup.C constraint-conditioned state estimates using the unconstrained state estimate y.sub.t.sub.k|k.sup.1 as a measurement, along with the calculation of the likelihood function and probability associated with each constraint. In one embodiment of the multiple constraint (MC) estimation algorithm, the constrained state estimate output is a composite combination of all of the constraint-conditioned state estimates. If this constrained state estimate is valid, i.e. if the constrained state estimate corresponds to--e.g. matches--the unconstrained state estimate, then the target state is given by fusing the constrained and unconstrained state estimates; otherwise the target state is given by the unconstrained state estimate. This embodiment of the multiple constraint (MC) estimation algorithm comprises the following steps:

1. Estimation of state variables from multiple constraints: In step (514), using the multiple lane road equation (34) to replace the first row in equation (25), the multiple constraint state estimates are given by:

.times..times..times. ##EQU00021## where

.+-..times..+-..times. ##EQU00022## and B is the width of a lane. Stated in another way, the constraint state estimates corresponds to--e.g. matches--the y locations of the centerlines of each possible lane in which the target vehicle 36 could be located.

The associated covariance is given by: P.sub.yt.sub.k|k.sup.0j=A.sub.k.sup.1P.sub.xt.sub.k|k(A.sub.k.sup.1).sup.- T+A.sub.k.sup.2P.sub.k|k.sup.C(A.sub.k.sup.2).sup.T (42) where A.sub.k.sup.1 and A.sub.k.sup.2 are given by equation (27) and equation (28), P.sub.xt.sub.k|k is from equation (24) and P.sub.k|k.sup.C is from the curvature filter.

2. Constraint-conditioned updating: In step (516), the state estimates and covariance conditioned on a constraint being in effect are updated, as well as the constraint likelihood function, for each of the constraints j=1, . . . N.sup.C. The updated state estimate and covariances corresponding to constraint j are obtained using measurement y.sub.t.sub.k|k.sup.1, as follows: y.sub.t.sub.k|k.sup.j= y.sub.t.sub.k|k.sup.0j+ P.sub.yt.sub.k|k.sup.0j( P.sub.yt.sub.k|k.sup.0j+P.sub.yt.sub.k|k.sup.1).sup.-1(y.sub.t.sub.k|k.su- p.1- y.sub.t.sub.k|k.sup.0j) (43) P.sub.yt.sub.k|k.sup.j= P.sub.yt.sub.k|k.sup.0j- P.sub.yt.sub.k|k.sup.0j P.sub.yt.sub.k|k.sup.0j+P.sub.yt.sub.k|k.sup.1).sup.-1( P.sub.yt.sub.k|k.sup.0j. (44) P.sub.xyt.sub.k|k.sup.j= P.sub.xyt.sub.k|k.sup.0j- P.sub.yt.sub.k|k.sup.0j( P.sub.yt.sub.k|k.sup.0j+ P.sub.yt.sub.k|k.sup.1).sup.-1 P.sub.xyt.sub.k|k.sup.0j (45)

3. Likelihood calculation: In step (518), the likelihood function corresponding to constraint s is evaluated at the value y.sub.t.sub.k|k.sup.1 of the unconstrained target state estimate, assuming a Gaussian distribution of the measurement around the constraint-conditioned state estimate for each of the constraints j=1, . . . N.sup.C, as follows: .LAMBDA..sub.t.sub.k.sup.j=N(y.sub.t.sub.k|k.sup.1; y.sub.t.sub.k|k.sup.0j, P.sub.yt.sub.k|k.sup.0j+P.sub.yt.sub.k|k.sup.1) (46) wherein the Gaussian distribution N (; ,) has a mean value of y.sub.t.sub.k|k.sup.0j and an associated covariance of P.sub.yt.sub.k|k.sup.0j+P.sub.yt.sub.k|k.sup.1.

4. Constraint probability evaluations: In step (520), the updated constraint probabilities are calculated for each of the constraints j=1, . . . N.sup.C, as follows:

.mu..times..LAMBDA..times. ##EQU00023## where .sub.j, the probability after transition that constraint j is in effect, is given by

.times..mu. ##EQU00024## and the normalizing constant is

.times..LAMBDA..times. ##EQU00025##

5. Overall state estimate and covariance: In step (522), the combination of the latest constraint-conditioned state estimates and covariances is given by:

.times..mu..times..mu.'.times..mu. ##EQU00026##

The output of the estimator from step (522) in the above algorithm is then used as the constrained estimates in the fusion process described by equations (29) and (30), and the result of equation (52), instead of the result of equation (33), is used in equation (32).

When the target vehicle 36 is not following the roadway 34 or is changing lanes, imposing the road constraint on target kinematic state variables will result in incorrect estimates that would be worse than using the associated unconstrained estimates. However, noise related estimation errors might cause a correct road constraint to appear invalid. Accordingly, it is beneficial to incorporate a means that can keep the constraints in effect when they are valid, e.g. when the target vehicle 36 follows a particular lane; and lift them off promptly when they are invalid, e.g. when the target vehicle 36 departs from its lane. The unconstrained target state estimate plays a useful role in road constraint validation, since it provides independent target state estimates.

One approach is to test the hypothesis that the unconstrained target state estimate satisfies the road constraint equation, or equivalently, that the constrained estimate and the unconstrained estimate each correspond to the same target. The optimal test would require using all available target state estimates in history through time t.sub.k and is generally not practical. A practical approach is the sequential hypothesis testing in which the test is carried out based on the most recent state estim