System and method for road angle estimation

A system and method for estimating road angles of a road on which a moving body is traveling are disclosed. The road angle estimation system includes: a sensing module adapted to be mounted on the moving body to detect a plurality of pieces of measurement information associated with the moving body; and a calculating module coupled to the sensing module to receive the pieces of measurement information therefrom. The calculating module simultaneously calculates an estimated bank angle and an estimated grade angle on the basis of the pieces of measurement information, a plurality of support parameters, and a plurality of user control parameters.

CROSS-REFERENCE TO RELATED APPLICATION

This application claims priority of Taiwanese Application No. 097139687, filed on Oct. 16, 2008.

BACKGROUND OF THE INVENTION

1. Field of the Invention

The present invention relates to a system and method for road angle estimation.

2. Description of the Related Art

Information related to road angle may be used by vehicular control systems to enhance safety and performance.

Examples of patents and published patent applications that disclose conventional road angle estimation techniques include the following: U.S. Pat. No. 5,446,658, entitled “Method and Apparatus For Estimating Incline and Bank Angles of a Road Surface”; U.S. Pat. No. 6,714,851, entitled “Method For Road Grade/Vehicle Pitch Estimation”; and US Patent Application Publication No. 2006/0276939, entitled “Real-time Determination of Grade and Superelevation Angles of Road.” Examples of non-patent publications that disclose conventional road angle estimation techniques include the following: “Integrating Inertial Sensors With GPS For Vehicle Dynamics Control” by Ryu and Gerdes J. C. and published in the June 2004 issue of “Dynamic Systems, Measurement, and Control”; “Dynamic Estimation of Road Bank Angle” by Tseng H. E. and published in 2001 in “Vehicle System Dynamics”; and “Real-time Identification of Road-bank Angle Using Differential GPS” by Hahn J. O., Rajamani R., You S. H., and Lee K. I. and published in 2004 in “IEEE Transactions on Control Systems Technology.”

However, conventional techniques for estimating road angle, such as those listed above, have some or all of the following disadvantages:

1. Many existing techniques utilize empirical formulas to estimate road angle. However, such methods are imprecise, and are not applicable to a road surface having road angles that vary rapidly and frequently.

2. Existing techniques estimate two road angles separately, and are unable to estimate two road angles simultaneously.

3. Current techniques require the use of four or more sensors for road angle estimation.

4. Road angles defined by conventional techniques are relative to the front of the vehicle, and such a manner of defining road angles is different from traditional Euler angles. Hence, even if two angle values are obtained, road conditions on a global (or terrestrial) reference frame cannot be described sufficiently rapidly.

SUMMARY OF THE INVENTION

Therefore, the object of the present invention is to provide a system and method for road angle estimation that can overcome the aforesaid drawbacks associated with the prior art.

In one aspect of the present invention, the road angle estimation system estimates road angles of a road on which a moving body is traveling. The road angle estimation system comprises: a sensing module adapted to be mounted on the moving body to detect a plurality of pieces of measurement information associated with the moving body; and a calculating module coupled to the sensing module to receive the pieces of measurement information therefrom. The calculating module simultaneously calculates an estimated bank angle and an estimated grade angle on the basis of the pieces of measurement information, a plurality of support parameters, and a plurality of user control parameters.

In another aspect of the present invention, the road angle estimation method comprises: a) detecting a plurality of pieces of measurement information associated with the moving body through a sensing module mounted on the moving body; and b) through a calculating module mounted on the moving body and coupled to the sensing module, simultaneously calculating an estimated bank angle and an estimated grade angle on the basis of the pieces of measurement information, a plurality of support parameters, and a plurality of user control parameters.

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENT

Referring toFIG. 1, a system for road angle estimation according to a preferred embodiment of the present invention is used to estimate road angles of a road20(seeFIG. 3) on which a moving body30is traveling. The system includes a sensing module31and a calculating module32.

The sensing module31is adapted to be mounted on the moving body30to detect a plurality of pieces of measurement information associated with the moving body30. In the preferred embodiment, the sensing module31includes an accelerometer311, a plurality of suspension displacement sensors312, and a plurality of receiving antennas313of a differential global positioning system (DGPS). The moving body30may be a vehicle, as shown inFIG. 1, and the accelerometer311, the suspension displacement sensors312, and the receiving antennas313of the sensing module31may be mounted on the vehicle in the manner shown inFIG. 1. In the preferred embodiment, the pieces of measurement information detected by the sensing module31include a suspension displacement quantity, a lateral acceleration, a longitudinal speed, and a yaw angle of the moving body30. It is noted that different or additional sensing devices may be included in the sensing module31to generate the above and additional pieces of measurement information, and the sensing module31and the pieces of measurement information detected thereby are not limited to what has been disclosed herein.

The calculating module32is coupled to the sensing module31to receive the pieces of measurement information therefrom. The calculating module32simultaneously calculates estimated road angles on the basis of the pieces of measurement information, a plurality of support parameters, and a plurality of user control parameters. In the preferred embodiment, the estimated road angles include an estimated bank angle and an estimated grade angle.

The road angles determine the magnitudes of the components of the earth's gravity exerted on the moving body30, and these gravity components directly affect the dynamics of the moving body30. Therefore, it may be stated that the gravity components are the media through which the road angles affect the dynamics of the moving body30. In the preferred embodiment, the support parameters include a set of gravity components comprised of a first gravity component, a second gravity component, and a third gravity component. In other embodiments, the support parameters include a set of road angles comprised of a road bank angle and a road grade angle.

Moreover, in the preferred embodiment, the user control parameters include a steering angle, a throttle opening value, and a braking force value.

ReferringFIGS. 1 to 4, a method for road angle estimation according to a preferred embodiment of the present invention will now be described. The method is performed by the system of the preferred embodiment described above.

First, in step11, the sensing module31is mounted on the moving body30.

Next, in step12, preparations are made for use of the support parameters, and a plurality of auxiliary parameters.

In the preferred embodiment, the support parameters include a first gravity component, a second gravity component, and a third gravity component, which are represented by Gxa, Gya, Gza, respectively.

In the preferred embodiment, the auxiliary parameters include four reference frames, and first and second sets of Euler angles. Referring toFIG. 3, the four reference frames are a global reference frame (Eg)21, a road reference frame (Er)22, a moving body reference frame (Eb)23, and an auxiliary reference frame (Ea)24.

The first set of Euler angles is used for describing the angular relation of the rotational orientation of the moving body30relative to the road reference frame22, and the second set of Euler angles is used for describing the angular relation of topological variations of the road20relative to the global reference frame21. The first set of Euler angles in this embodiment indicates a vehicle yaw angle, a vehicle pitch angle, and a vehicle roll angle, which are represented respectively as (ψ,θ,φ). The second set of Euler angles in this embodiment indicates a road curve angle, a road grade angle, and a road bank angle, which are represented respectively as (ψr,θr,φr). Moreover, since the road curve angle does not affect the dynamics of the moving body30, it is assumed that the road curve angle is zero in the preferred embodiment, that is, ψr=0.

It is to be noted that the relationship of the gravity components with the road angles and the vehicle yaw angle is shown in relation formula (1) below:

In addition, since there is no hardware equipment mounted in a vehicle capable of obtaining road angular rates, it is not possible to use mathematical equations to describe real-time changes in road angle. Nevertheless, since the road angular rate in practice is slower than the dynamics of the moving body30, the support parameters added in step12may be assumed to be constants within a predetermined sampling period, that is, the variation of Gxa, Gya, and Gzamay be assumed to be 0.

Next, in step13, the sensing module31detects the pieces of measurement information associated with the moving body30. In the preferred embodiment, the sensing module31detects the pieces of measurement information.

Subsequently, in step14, the support parameters, the pieces of measurement information, and the user control parameters are input into the calculating module32. In the preferred embodiment, the calculating module32includes an extended Kalman filter321, and performs calculations in cooperation with a fading memory technique and a dynamic formula for describing the moving body30, so as to compensate for errors caused by setting the support parameters to constants in step12. It is to be noted that the dynamic formula for the moving body30is as follows:
{dot over (x)}=fc(xother,Gxa,Gya,Gza,u)

where (x) indicates the dynamics of the moving body30and the previously assumed three gravity components of the support parameters, (xother) indicates dynamic values other than the gravity components, (u) indicates the steering angle, the throttle opening value, and the braking force value, and fc(·) is a system dynamic formula of the moving body30which is expressed in a continuous time manner.

In practice, the calculating module32may also employ other conventional techniques, or techniques that have yet to be developed, and the present invention is not limited in this regard.

It is further noted that, in the preferred embodiment, in addition to the suspension displacement quantity, the lateral acceleration, the longitudinal speed, and the yaw angle of the pieces of measurement information detected in step13, the sensing module31may be configured to accommodate a plurality of sensors to obtain the steering angle, the throttle opening value, and the braking force value of the user control parameters of step14. The relationship between the measurement values and the dynamics of the moving body30is expressed by relation formulas (2)˜(4) below:
ÿmg=({umlaut over (x)}a−{dot over (y)}a{dot over (ψ)}+Gxa)sin φ sin θ+(ÿa+{dot over (x)}a{dot over (ψ)}+Gya)cos φ+({umlaut over (z)}a+Gza)sin φ cos θ  Relation formula (2)

Relation formula (2) expresses the lateral acceleration (ÿmg) measured by the accelerometer311which is mounted at the center of mass of the moving body30. In relation formula (2), xa, ya, zarepresent linear displacement magnitudes of the center of mass of the moving body30observed from the auxiliary reference frame24, and are linear displacement magnitudes respectively in the longitudinal direction, the lateral direction, and the vertical direction.
H1,ma=−za+lfsin θ−tfcos θ sin φ
H2,ma=−za+lfsin θ+tfcos θ sin φ
H3,ma=−za−lrsin θ+trcos θ sin φ
H4,ma=−za−lrsin θ−trcos θ sin φ  Relation formula (3)

Relation formula (3) expresses spring displacement magnitudes (H1,ma, i=1˜4) of a suspension system (not shown) of the moving body30measured by the suspension displacement sensors312, where (tf) and (tr) represent respectively the half-length between the two front wheels and the half-length between the two rear wheels of the moving body30(assuming the moving body30is a vehicle), and (lf) and (lr) represent respectively the distance between the center of mass of the moving body30and the front axle and the distance between the center of mass of the moving body30and the rear axle.

Relation formula (4)

When there are two of the DGPS receiving antennas313, as indicated by (a) and (b) inFIG. 4, relation formula (4) is expressed as follows:
ψm=90°−α1and{dot over (x)}ma=0.5(Va,xy+Vb,xy)cos ψm

When there are three of the DGPS receiving antennas313, as indicated by (a), (b), and (c) inFIG. 4, relation formula (4) is expressed as follows:
ψm=α2and{dot over (x)}ma=0.5(Va,xy+Vb,xy)cos ψm

Relation formulas (4) utilize the DGPS receiving antennas313mounted on the moving body30and cooperate with a differential global positioning system (not shown) to calculate a relative position and a relative speed between each of the receiving antennas313and a measuring base station (not shown), i.e., the global reference frame21, and to further calculate the yaw angle (ψm) and the longitudinal speed ({dot over (x)}ma) of the moving body30(vehicle), where (α1) and (α2) represent the auxiliary angles of the yaw angle of the moving body30as shown inFIG. 4, and (Va,xy) and (Vb,xy) represent relative speeds measured by the (a) and (b) receiving antennas313, respectively, on the (x-y) plane.

The parameters mentioned in step13and step14can be obtained according to the aforementioned dynamic formulas and relation formulas of the moving body30. After the parameters needed for subsequent operation of the calculating module32are obtained, detailed processes related to the fading memory technique used in cooperation with the extended Kalman filter321of the preferred embodiment, i.e., sub-steps of step14, are performed, as outlined inFIG. 2.

First, in sub-step141, the dynamic formulas associated with relation formulas (1) to (4) are discretized to become:
xk+1=fd(xk,uk)+wkandyk+1=hd(xk)+vk

Next, in sub-step142, two linearized models are selected as follows:

Next, in sub-step144, the dynamics of the moving body30, an output of related detection, and a related covariance matrix are predicted as follows:
{circumflex over (x)}k+1=f({circumflex over (x)}k,uk),ŷk+1=h({circumflex over (x)}k),Pk+1|k=λk+1FkRk|kFkT+Qk

Subsequently, in sub-step145, an error covariance matrix of the actual detected output and the predicted detected output of the moving body30is calculated as follows:
Sk+1=Hk+1Pk+1|kHk+1T+Rk+1

Next, in sub-step146, again value of the calculating module32is calculated as follows:
Kk+1=Pk+1|kHk+1TSk+1−1

Next, in sub-step147, the dynamics of the moving body30and the covariance matrix are updated as follows:
{circumflex over (x)}k+1|k+1={circumflex over (x)}k+1|k+Kk+1(yk+1−ŷk+1),Pk+1|k+1=Pk+1|k−Kk+1Sk+1Kk+1−1

Lastly, in sub-step148, the original variable (k) is incremented by 1, and sub-steps141˜148are repeated until estimation is completed.

The parameters used in sub-steps141˜148are further detailed below.

(xk) represents a kth sampling value over a discrete time period of the system dynamics (x) of the moving body30. (uk) represents a kth sampling value over a discrete time period of the system input (u) of the moving body30. (yk) represents a kth sampling value over a discrete time period of the output of the sensing module31including the lateral acceleration of the moving body30, the suspension displacement quantity, the yaw angle of the moving body30, and the longitudinal speed of the moving body30. fd(·) represents a system dynamic formula of the moving body30over a discrete time period. hd(·) represents an output formula of the sensing module31over a discrete time period. (wk) and (vk) represent respectively an uncertain quantity of the system of the moving body30and a noise (assumed to be white noise herein) of the sensing module31. (Fk) represents a linearized dynamic formula of the moving body30calculated over a kth sampling time. (Hk) represents a linearized system output formula of the moving body30calculated over a kth sampling time. (Pk+1|k) represents a dynamic covariance matrix of the moving body30, where (k+1|k) represents information of a (k+1)th sampling time estimated during the (kth) sampling time. (Sk) represents an associative outputted error covariance matrix calculated during the kth sampling time. (Kk) represents a gain of the extended Kalman filter321in the calculating module32calculated during the (kth) sampling time.

Lastly, in step15, through the calculating module32mounted on the moving body30and coupled to the sensing module31, the estimated bank angle and the estimated grade angle are simultaneously calculated on the basis of the pieces of measurement information, the support parameters, and the user control parameters. In the preferred embodiment, the calculating module32simultaneously calculates the estimated bank angle and the estimated grade angle with reference to the auxiliary parameters.

FIG. 5shows the steps involved in a modified example of the method for road angle estimation according to the present invention.

The modified example of the method for road angle estimation is substantially the same as the method for road angle estimation of the preferred embodiment described above. However, in the modified example, step42, which corresponds to step12of the preferred embodiment, is different. In particular, the support parameters added in step12and originally assumed to be three gravity component values in the preferred embodiment are replaced in the modified example by a road bank angle and a road grade angle as indicated inFIG. 5. (A road curve angle is also included in this replacement of the gravity component values but is not considered since the road curve angle is set to zero due to the fact that this parameter does not affect the dynamics of the moving body30as described above.) Through such an alternative approach, there is no need to indirectly compute the related road bank angle and road grade angle using relation formula (1) in step12, and it is possible to simply assume these parameters to be constants before proceeding with the subsequent steps.

In the system and method for road angle estimation of this invention, through use of the support parameters, the pieces of measurement information, and the user control parameters, as well as the fading memory technique and the extended Kalman filter321by the calculating module32, more efficient and accurate estimation of road angle changes are simultaneously obtained using a smaller number of sensors (i.e., the accelerometer311, the suspension displacement sensors312, and the DGPS receiving antennas313).