Method and apparatus for estimation of forward path geometry of a vehicle based on a two-clothoid road model

[0001] The present invention is generally related to forward path geometry estimation, and more specifically to a method and apparatus for accurate estimation of forward path geometry of a vehicle based on a two-clothoid road model.

[0002] Vehicular land and road detection systems have been the subject of significant research. Applications such as adaptive cruise control, collision avoidance systems, and vehicle guidance systems require knowledge of the geometry of the road to be most effective. It is difficult for a collision avoidance system to accurately characterize a stationary or slower moving object as non-hazardous unless there is some certainty that the stationary object is not in the vehicle's path. Previous approaches to estimation of forward road geometry, model the road in the forward view as either a constant curvature or a linearly varying curvature. These road geometry estimation approaches then fit parameters of these models via a least-squares fitting technique or other mathematical technique. The linearly varying curvature model is usually referred to as a single-clothoid model and is completely described by two coefficients, c₀, the local curvature at host vehicle position and c₁ the rate of change of curvature with distance. Both the constant curvature and single-clothoid road models do not adequately represent road geometry, especially when there are abrupt changes in curvature in the look-ahead range. Such sharp changes in curvature are common on many roads. This is also seen on many freeways, where a straightaway section transitions to a curved section abruptly and vice-versa. Thus, methods based on these simple models are generally inaccurate in adequately describing forward road geometry.

[0003] To overcome the problem of inaccurate road geometry estimation with these simple models, an averaged curvature road model was proposed. This approach works well on roads with low curvature and very smooth curvature changes, but still fails on roads with significant curvature changes or discontinuities in c₁. To reduce these errors further, more complex road models were proposed that split the road into multiple clothoid segments. The transition between segments is estimated using ad-hoc methods such as the generalized likelihood ratio test. In this approach, the geometry of the segments is not dynamic and is updated using measurements that fall in the corresponding segment. A dynamic model is required for vehicle dynamics only, as the road model remains spatially fixed, while the vehicle is moving through the road segments. One of the fundamental limitations of this approach is that road geometry estimation accuracy is strongly dependent on reliable detection of transition points between segments making it very sensitive to noise in the measurements. Thus they are often inaccurate in moderate and high noise measurement conditions. In addition, the segmentation process is computationally expensive because the likelihood ratio test has to be performed for each new measurement.

[0004] A nave approach of using a road model based on a higher-order polynomial to solve these problems with previous methods will lead to over-fitting road geometry and a statistically insignificant estimate of the coefficient of this higher order term. This will also make this approach more sensitive to noise. Thus, it would be desirable to develop a model that overcomes the problems associated with previous approaches. Ideally such a model would be simple to implement and would not as computationally intensive or ad-hoc as the previous multi-clothoid models.

[0005] The present invention provides a method and apparatus for accurate estimation of the forward path geometry of a vehicle based on a two-clothoid road model. The primary components of the invention include a data collection element configured to provide collected data to a first processing element. The data collection element could be a camera sensitive to signals in the visible or infrared region or a combination thereof. Alternatively the data collection element might be a radar transceiver used alone or in conjunction with a camera. The first processing element computes a full measurement transfer function of a two-clothoid road model and conveys the full measurement transfer function to a second processing element. In one embodiment the processing elements in this invention are preprogrammed computers. The processing elements could also include application specific integrated circuits, or devices configured to respond to internal or external instructions. The second processing element is configured to utilize the full measurement transfer function to simultaneously estimate the near-range clothoid coefficients and far-range clothoid coefficients, a third processing element then combines the near-range clothoid coefficients and far-range clothoid coefficients to provide a forward path description.

[0006] The objects, features, and advantages of the present invention will be apparent from the following detailed description of the preferred embodiment of the invention with references to the following drawings.

[0007] Figure is defined as: an illustration, a diagram, a picture, a drawing

[0008] FIG. 1 is an illustration depicting a host vehicle and the near-range and far-range forward path;

[0009] FIG. 2 is a block diagram flowchart showing the operation of the principle embodiment;

[0010] FIG. 3a is a diagram of the geometry of a road;

[0011] FIG. 3b is a depiction of the of the corresponding road curvature parameters;

[0012] FIG. 4a is a comparison of the mean error between the single-clothoid road model and the two-clothoid road model; and

[0013] FIG. 4b is a comparison of the maximum error between the single-clothoid road model and the two-clothoid road model.

[0014] The present invention is generally related to forward path geometry estimation and more specifically to a method and apparatus for accurate estimation of forward path geometry of a vehicle, based on a two-clothoid road model. The following description, taken in conjunction with the referenced drawings, is presented to enable one of ordinary skill in the art to make and use the invention and to incorporate it in the context of particular applications. Various modifications, as well as a variety of uses in different applications, will be readily apparent to those skilled in the art, and the general principles defined herein may be applied to a wide range of embodiments. Thus, the present invention is not intended to be limited to the embodiments presented, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein. Furthermore it should be noted that unless explicitly stated otherwise, the figures included herein are illustrated diagrammatically and without any specific scale, as they are provided as qualitative illustrations of the concept of the present invention.

[0015] The present invention provides a method and apparatus for accurate estimation of forward path geometry of a vehicle. One embodiment of the present invention discloses a road model that describes the forward geometry as two contiguous clothoid segments with different geometries but with a continuous curvature across the transition. This proposed model describes road geometry in front of the host vehicle with a closed-form parametric expression of the same polynomial order as previous models but with higher accuracy. This higher accuracy comes without a concomitant increase in computational cost or increased sensitivity to noise.

[0016] A commonly used model of the horizontal course of a road is a clothoid, which is a curve whose curvature changes linearly with distance along the road. With a curvature co, the curvature at a distance x along the curve is given by equation 1:

[0017] The coefficients c₀ and c₁ represent the rate of change of curvature with distance and are called the clothoid coefficients. Typical highway geometries are approximated by a set of successive clothoids with different coefficients. For example, two segments of constant curvature, where c₁0, could be joined by a transition section where the curvature changes linearly with distance, where c₁0. Thus transition between segments can have jumps in the clothoid coefficient c₁, even when the curvature c(x) itself is steady across the segment transition. A clothoid can be approximated by a third order polynomial that relates the lateral offset of the curve with longitudinal distance along it when the change in road heading angle over the distance along the curve is small. If x is denoted as longitudinal distance and y as lateral offset within a segment then: $\begin{array}{cc}y=\frac{c_0{x}^2}{2}+\frac{c_1{x}^3}{6}& \mathrm{Eqn}.2\end{array}$

[0018] Since the road geometry changes as the host vehicle moves along the road, the clothoid coefficients are time varying and dependent on the speed of the vehicle. One approach to estimating model parameters at each time instant is to perform non-recursive least-squares fitting on the data points. The main problem with non-recursive least-squares estimation is that it can be very sensitive to stochastic errors, including sensor errors, resulting in widely oscillating estimates from one time step to the next. It is possible to substantially overcome this problem by utilizing a recursive estimation method. Since vehicle motion relative to the road makes forward road geometry a quantity that varies dynamically with time, it is appropriate to use a dynamic recursive estimation approach such as a Kalman filter. The dynamic recursive estimation approach uses both a model-based update of state variables, clothoid coefficients in this example, and uses updates of the state estimates incorporating a weighted version of new measurements. Recursive filtering can provide more accuracy and improved robustness to stochastic errors, as it acts as a type of low-pass filter whereby errors are minimized.

[0019] In another embodiment of the present invention, the forward road geometry is modeled as two contiguous single-clothoid segments with different c₁ parameters as shown in FIG. 1. The first segment is from 0-d₁ 100 in front of the host vehicle 102. This is the near-range clothoid segment. The second segment is from d₁-d₂ 104 in front of the host vehicle, this is the far-range clothoid segment. Here d₁ 106 is the transition point between the segments and d₂ 108 is the set range of the sensor system. Since the detection the transition between the segments is difficult and sensitive to noise, d₁ 106 is set to a fixed distance in front of the vehicle with a nominal value equal to approximately 50% of the utilized range of the sensor system. One example of an embodiment is where d₁ 10650 m for a d₂ 108100 m. If the actual transition point is closer, then near-range segment estimation errors will be similar to a single-clothoid model based method albeit for a much shorter range from host vehicle to end of near-range segment. Similarly, if actual transition is beyond this nominal value, far-range segment estimation errors will be similar to a single-clothoid model based method, albeit for a much shorter range from the start to the end of far-range segment. Thus, choosing a mid-range distance as the transition point offers the best compromise when the actual transition point deviates from the fixed chosen value. However, the transition point between the clothoid segments can be selected as a variable in the state space representation of the Kalman filter, but the transition point is set to a nominal value for practical reasons.

[0020] A block diagram flowchart showing the operation of the principle embodiment of the present invention is shown in FIG. 2. According to this flowchart, an apparatus is presented, wherein the apparatus is configured to estimate the forward path of a vehicle. Accordingly a data collection element collects road data 200. This data collection element could include a video camera affixed to a vehicle, a radar transceiver affixed to a vehicle, an image capture device affixed to a vehicle and substantially responsive to electromagnetic radiation in the visible region, the infrared region, or a combination of the infrared and visible regions. The data collection element is configured to collect data and to provide the collected data to a first processing element, wherein the first processing element 204 computes a full measurement transfer function of a two-clothoid road model 202. The first processing element then conveys the full measurement transfer function to a second processing element which is configured to utilize the full measurement transfer function to simultaneously estimate the near-range clothoid coefficients and far-range clothoid coefficients and provide the near-range clothoid coefficients and far-range clothoid coefficients to a third processing element. The third processing element combines the near-range clothoid coefficients and far-range clothoid coefficients to provide a forward path description 206. The processing elements could be any number of different devices that are configured to perform the necessary computations. For example, a computer configured to perform pre-specified functions could be utilized, or a device configured to respond to operator instructions, or a device configured to respond to external instructions, a device configured to respond to internal system instructions, an application-specific integrated circuit, or a preprogrammed logic circuit. Additionally, a combination of the foregoing could be used.

[0021] The present invention provides a method for estimating the forward path of a vehicle comprising the steps shown in FIG. 2. According to this diagram, a method is presented, wherein the method provides a sequence of steps, which in the aggregate estimate the forward path of a vehicle. According to this method there is a data collection step where road data is collected 200 utilizing a data collection element. The data collection element could include a video camera affixed to a vehicle, a radar transceiver affixed to a vehicle, an image capture device affixed to a vehicle and substantially responsive to electromagnetic radiation in the visible region, the infrared region, or a combination of the infrared and visible regions. The data collection element is configured to collect data and to provide the collected data to a first processing element, wherein the first processing element computes a full measurement transfer function of a two-clothoid road model 202. The first processing element then conveys the full measurement transfer function to a second processing element which is configured to utilize the full measurement transfer function to simultaneously estimate the near-range clothoid coefficients and far-range clothoid coefficients and provide the near-range clothoid coefficients and far-range clothoid coefficients to a third processing element 204. The third processing element combines the near-range clothoid coefficients and far-range clothoid coefficients to provide a forward path description 206. The processing elements could be any number of different devices that are configured perform the necessary computations. For example, a computer configured to perform pre-specified functions could be utilized, or a device configured to respond to operator instructions, or a device configured to respond to external instructions, a device configured to respond to internal system instructions, an application-specific integrated circuit, or a preprogrammed logic circuit. Additionally, a combination of the foregoing could be used.

[0022] The curvature of the road at the host vehicle is defined to be co. The rate of change of curvature in the near and far range segments are defined as c₁ and c₂, respectively. For small angle approximations, the lateral offset y of the road edge as a function of look-ahead distance x can be expressed as: $\begin{array}{cc}y=c_0\frac{x^2}{2}+c_1\frac{x^3}{6},0<x{d}_{1}y=c_0\frac{x^2}{2}+c_1\left(\frac{d_1^3}{6}+\frac{x\left(x-d_1\right)d_1}{2}\right)+c_2\frac{{\left(x-d_1\right)}^3}{6},d_1<x{d}_2& \mathrm{Eqn}.3\end{array}$

[0023] The above equation is derived by matching the lateral offset, heading angle, and curvature at the end of the near-range segment and start of the far-range segment as shown below.

[0024] At the transition point, or the end of the near range segment (xd₁)

[0025] Curvature at transition point: C_TC₀c₁d₁

[0026] Heading angle at transition point: ${}_T=c_0{d}_1+\frac{1}{2}{c}_1{d}_1^2$

[0027] Lateral offset at transition point: $y_T=\frac{1}{2}{c}_0{d}_1^2+\frac{1}{6}{c}_1{d}_1^3$

[0028] In far-range segment (d₁<xd₂):

[0029] Lateral offset of point P in coordinate system XY: $y_p^{}=\frac{1}{2}{c_T\left(x-d_1\right)}^2+\frac{1}{6}{c_2\left(x-d_1\right)}^3$

[0030] Lateral offset of point P in coordinate system XY: y_py_Ty_Pcos_T(xd₁)sin_T

[0031] Using small angle approximation: y_py_Ty_P(xd₁)_T

[0032] Substituting for y_p, c_T, _T and y_T gives Eqn. (3), above.

[0033] The estimation apparatus and method proposed here are based on the above two-clothoid road model and relies on a dynamic recursive estimation approach. A major difference between the invented apparatus and method and conventional single-clothoid based systems lies in the new process dynamics model, which accounts for the additional coefficient c₂ and the new measurement transfer function.

[0034] As the vehicle drives through the clothoid with speed v, the coefficients describing the curvature model of the clothoid change according to the motion of the vehicle. For a road with jumps in curvature rate in the look-ahead range, the time derivative of this rate of change at the transition is an impulse. However, since these transitions cannot be reliably determined in noisy measurements, the time derivative of c₁ and the time derivative c₂ are both approximated by white noise. This leads to the following dynamical model of the road in the horizontal plane: $\left[\begin{array}{c}{\stackrel{.}{c}}_0\\ {\stackrel{.}{c}}_1\\ {\stackrel{.}{c}}_2\end{array}\right]=\left[\begin{array}{ccc}0& v\left(t\right)& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}{c}_0\\ c_1\\ c_2\end{array}\right]+\left[\begin{array}{c}0\\ w_1\left(t\right)\\ w_2\left(t\right)\end{array}\right],$

[0035] where,

[0036] v(t) is vehicle speed;

[0037] w₁(t) is the continuous driving white noise term for c₁; and

[0038] w₂(t) is the continuous driving white noise term for c₂.

[0039] The vertical curvature in the above model has been ignored, but can be easily appended to the above state-space representation. Consider a sample time interval T, the predicted state vector estimate at time k1 using the estimate at time k is given by:

X_k1kW_k; $X_k=\text{state vector}=\left[\begin{array}{c}{c}_{0,k+1}\\ c_{1,k+1}\\ c_{2,k+1}\end{array}\right]$ ${}_k=\text{State transitio n matrix}=\left[\begin{array}{ccc}1& v\left(k\right)T& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$ $W_k=\text{Process noise vector}=\left[\begin{array}{c}v\left(k\right){\mathrm{Tw}}_1\left(k\right)\\ w_1\left(k\right)\\ w_2\left(k\right)\end{array}\right]$

[0040] where w₁(k) and w₂(k) are scalar, zero-mean, white-noise sequences with variances ₁² and ₂², respectively.

[0041] The process noise covariance Q(k) is thus defined as: $Q\left(k\right)=\left[\begin{array}{ccc}{\left(v\left(k\right)T\right)}^2{}_1^2& v\left(k\right)T{}_1^2& 0\\ v\left(k\right)T{}_1& {}_1^2& 0\\ 0& 0& {}_2^2\end{array}\right]$

[0042] The measurement vector Y_k comprises noisy lateral offsets of a number of road points available from some sensor system onboard the host vehicle (vision-based, GPS, etc.). The measurements are available for N road points: n points with lateral offsets y₁, y₂, . . . , y_n at distances x₁, x₂, . . ., x_n, respectively, in the near-range segment, and f points with lateral offsets y_n1, y_n2, . . . , y_nf at distances x_n1, x_n2, . . . , x_nf, respectively, in the far-range segment (Nnf). Using Equation (3), the measurement equation in a matrix notation can be denoted as:

Y_kH_kX_kN_k, $Y_k=\text{Measurement vector}=\left[\begin{array}{c}{y}_1\\ y_2\\ \\ \\ y_N\end{array}\right],H_k=\text{Measurement transfer function}=\left[\begin{array}{ccc}\frac{1}{2}{x}_1^2& \frac{1}{6}{x}_1^3& 0\\ \frac{1}{2}{x}_2^2& \frac{1}{6}{x}_2^3& 0\\ & & \\ & & \\ \frac{1}{2}{x}_n^2& \frac{1}{6}{x}_n^3& 0\\ \frac{1}{2}{x}_{n+1}^2& \frac{1}{6}{d}_1^3+\frac{1}{2}\left(x_{n+1}-d_1\right)\left(x_{n+1}\right)d_1& \frac{1}{6}{\left(x_{n+1}-d_1\right)}^3\\ \frac{1}{2}{x}_{n+1}^2& \frac{1}{6}{d}_1^3+\frac{1}{2}\left(x_{n+2}-d_1\right)\left(x_{n+2}\right)d_1& \frac{1}{6}{\left(x_{n+2}-d_1\right)}^3\\ \frac{1}{2}{x}_{n+f}^2& \frac{1}{6}{d}_1^3+\frac{1}{2}\left(x_{n+f}-d_1\right)\left(x_{n+f}\right)d_1& \frac{1}{6}{\left(x_{n+f}-d_1\right)}^3\end{array}\right]$ $N_k=\text{Measurement noise vector}=\left[\begin{array}{c}{n}_1\\ n_2\\ \\ \\ n_N\end{array}\right],$

[0043] The estimation approach set forth above, uses the above process and measurement model equations to recursively estimate the road model parameters. In this example a Kalman filter is used.

[0044] In FIGS. 3a and 3b, a simulated segment of road is introduced. The road segment has several different geometry segments with sharp discontinuities in the c₁ coefficient 300. The road segment is based on a clothoid model with continuous curvature across the clothoid transitions. This geometry is typical of many freeways. The simulated road geometry is shown in FIG. 3a, while the road curvature parameters c₀ 302 and c₁ 300 are shown in FIG. 3b. The actual transition points 304 are also shown in FIG. 3a. A host vehicle in this example traverses the road with a speed of 20 m/sec with a look-ahead distance of 100 m. Road geometry information is provided as offsets of 8 points along the road spaced 10 m apart starting in front of the host vehicle. For this example, the sampling rate was selected as 10 Hz, however a wide variety of other sampling rates are quite satisfactory. Kalman filter estimation methods, based on the two-clothoid road model, were used for the depictions set forth in FIG. 3a and FIG. 3b. The invention uses offsets of road points as measurements, and estimates corresponding model parameters. Further the two-clothoid model uses a fixed transition point at 50 m in front of the vehicle.

[0045] The estimated road geometry is compared to the road geometry and errors are computed. The mean estimation errors for both the single and two-clothoid models are shown in FIG. 4a. The maximum estimation errors for a single clothoid and a two-clothoid model are shown in FIG. 4b. The peaks 400 in the error plots correspond to the transition points 304. The absence of peaks, or presence of smaller peaks 402, reflects the improvement achieved with the two-clothoid road model, according to the present invention. For the purposes of clarity not every small peak 402 is illustrated. The present invention provides for smaller road geometry estimation errors, especially at and near transitions in road geometry 400.