Abstract:
In a method and system for estimating a trajectory of a vehicle having an Electronic Stability Program (ESP), sensor data pertaining to dynamics of the vehicle is obtained, and an estimate of the trajectory of the vehicle is calculated using the sensor data and vehicle dynamics parameters estimated by the ESP. The acquisition of the sensor data and the calculation of vehicle trajectory estimates occur independently of any external signal communication infrastructure-based trajectory estimation tools.

Description:
FIELD OF THE INVENTION  
         [0001]    The present invention relates to automobile electronic control systems, and relates more particularly to a method and system for providing real-time estimation of the trajectory of a vehicle that is equipped with an Electronic Stability Program (ESP).  
         BACKGROUND INFORMATION  
         [0002]    At present, automobile engineers and manufacturers are designing and implementing vehicle trajectory estimation systems that calculate and store data from which the trajectory a vehicle takes during travel can be estimated and reconstructed. Accurate vehicle trajectory estimation can be used in vehicle performance testing and accident reconstruction, among other applications. Typically deployed vehicle trajectory estimation systems utilize either an external signal communication infrastructure, such as the Global Positioning System (GPS), or specialized additional equipment such as gyroscopes, accelerometers and wheel tachometers that directly measure wheel dynamics. The systems which use an external signal communication infrastructure determine vehicle position in approximate real-time via a process of triangulation based upon varying reception times of radio waves broadcast by various GPS satellites and/or cellular antennas operating in the region of the vehicle. It has been found that the update rates of such systems can be too slow for precise continuous trajectory estimation. In addition, the position accuracy provided by such systems may be too low depending on the level of precision required, and signals may be corrupted, discontinuous or unavailable due to contingencies such as presence of tunnels and extreme weather conditions. Furthermore, systems that use additional measurement equipment often suffer from inaccuracies due to sensor drift and sensor output bias.  
           [0003]    Recently developed automobile safety systems such as Anti-Lock Braking Systems (ABS) and Vehicle Dynamics Control Systems (VDC, also known as Electronic Stability Program (ESP)) have been incorporated in many automobile models. As the article “ VDC, The Vehicle Dynamics Control System of Bosch”  by A. van Zanten et al. (1995) describes, Vehicle Dynamics Control systems are equipped with sensors that detect various primary dynamic parameters, such as the yaw rate ({dot over (ψ)}), the lateral acceleration (a y ), and wheel speed (v wh ), as well as driver-controlled parameters such as the steering wheel angle and the brake pressure. From the detected primary parameters, an Electronic Stability Program (ESP) estimates various quantities pertaining to the real-time dynamics of the vehicle, and from these quantities, determines whether to implement automatic controls. For example, during certain unsafe driving conditions, such as a sharp turn on a slippery surface, the ESP may determine from the estimated vehicle dynamics that special control measures are necessary for lateral and longitudinal stability of the vehicle. In such cases, the ESP automatically activates engine and/or brake elements which alter the dynamics of the vehicle so as to increase vehicle safety.  
           [0004]    In accomplishing the complex adjustments involved in adapting to various unsafe driving conditions, the ESP derives and estimates using the primary detected parameters, several dynamics parameters that are not readily measured by available sensors, such as the longitudinal acceleration of the vehicle, wheel-to-surface friction coefficients, wheel slip angles and slope angles. Since ESP controller sampling rates are on the order of 50 Hz, vehicle dynamics parameter estimates can be updated in near real-time. In addition, the ESP uses feedback from the derived estimates to correct for drift in the sensors and for additional bias due to sloped road conditions.  
           [0005]    It is known in the art that a vehicle&#39;s trajectory over time can be calculated as a function of the vehicle dynamics parameters if there is a full and accurate set of parameters for each near-instant of time. Given the high accuracy provided by the self-correcting mechanisms in ESP systems, and the relatively fast sampling rate at which ESP controllers make direct measurements and derive estimates of the vehicle dynamics parameters, it would be advantageous to selectively harness the capabilities of the ESP in order to generate a continuous vehicle trajectory estimation.  
         SUMMARY OF THE INVENTION  
         [0006]    It is therefore an object of the invention to estimate the trajectory of a vehicle equipped with an Electronic Stability Program (ESP) using the dynamics parameter detection and estimation functions provided by the ESP.  
           [0007]    In accordance with the desired object, the present invention provides a method for estimating a trajectory of a vehicle equipped with an ESP under both normal and abnormal driving conditions, such as wet or icy roads, sharp turns at high speed, uneven terrain, etc. According to the method, sensor data pertaining to the dynamics of the vehicle is obtained, and an estimate of the trajectory of the vehicle is calculated based upon the sensor data and estimates of vehicle dynamics parameters made by the ESP. The acquisition of the input data and the calculation of the estimates are performed independently of any external signal communication infrastructure-based trajectory estimation tools.  
           [0008]    In an exemplary embodiment, the sensor data is communicated by sensors that are readily available in commercial vehicles, such as a yaw rate sensor and a lateral acceleration sensor. The sensor data, comprising input data to the ESP, forms the basis for estimation of a wide range of vehicle dynamics parameters that are either not readily measurable in the vehicle (such as the longitudinal acceleration), or cannot currently be directly measured in the vehicle (such as wheel coefficients of friction, wheel slip angles, etc.). These estimated vehicle dynamics parameters are in turn used to compute the real-time position of the vehicle with respect to a stationary reference frame.  
           [0009]    The method according to the present invention also increases the accuracy of trajectory estimates by providing for high sampling and update rates, and by compensating for sensor drifts using observed dynamics parameter estimates computed by the ESP. The fast sampling rates allow real-time computation of the vehicle trajectory, up to the limit of the ESP controller cycle, thereby providing a sampling rate adequate for the bandwidth of vehicle motion even in extreme driving conditions.  
           [0010]    In addition, the present invention provides a procedure for obtaining the trajectory of a vehicle. According to a first embodiment, a vehicle equipped with an ESP obtains sensor data for an instant in time, computes related vehicle dynamics parameters, and from these data sources, estimates the position of the vehicle at that instant in time. The estimated position is then recorded either internally or externally. The trajectory is then assembled from the time series of recorded positions, which time series has a total duration T.  
           [0011]    According to a second embodiment, a vehicle equipped with sensors (with or without an ESP) obtains and records sensor data at a number of instants of time over a duration T. The recorded sensor data is thereafter input to an ESP which may be internal or external to the vehicle. The estimation of vehicle dynamics parameters and the calculation of positions may occur in near real time, while the vehicle is in operation, or it may occur subsequently, when the vehicle is no longer in operation.  
           [0012]    The method and system for vehicle trajectory estimation provided by the present invention can be advantageously used in numerous applications including, but not limited to: vehicle accident reconstruction, various testing of vehicle maneuverability and maneuver reconstruction, a stand-alone vehicle tracking system and/or an enhancement to future GPS systems that may have faster sampling rates and higher accuracy. When used in conjunction with GPS systems, the method and system of the present invention can be particularly advantageously applied when GPS updates are lost due to, for example, severe weather conditions, or driving in tunnels. Moreover the present invention can be usefully applied to pre-defined trajectory tracking for autonomous vehicles. By pre-defining the trajectory, the ESP can use the inverse of the equations used to compute the trajectory to calculate the dynamics parameters required to control the vehicle to follow the pre-defined trajectory. 
       
    
    
     BRIEF DESCRIPTION OF THE DRAWINGS  
       [0013]    [0013]FIG. 1 shows a schematic block diagram of a VDC control system in an automotive vehicle that executes an Electronic Stability Program (ESP) according to the present invention.  
         [0014]    [0014]FIG. 2 illustrates an exemplary path of a vehicle over time, and indicates how the starting reference point changes after each cycle period ends.  
         [0015]    [0015]FIG. 3 is a functional diagram of an exemplary embodiment of the method for vehicle trajectory estimation according to the present invention.  
         [0016]    [0016]FIG. 4A is a flow chart of one method for obtaining the trajectory of a vehicle according to the present invention, in which method the vehicle trajectory is computed in real-time in the vehicle.  
         [0017]    [0017]FIG. 4B is a flow chart of another method for obtaining the trajectory of a vehicle according to the present invention, in which method the vehicle trajectory is not calculated in real-time. 
     
    
     DETAILED DESCRIPTION  
       [0018]    The present invention provides a method and system for combining the signals measured by the VDC system, such as the yaw rate and lateral acceleration, with dynamic states estimated by the ESP, such as the longitudinal acceleration and velocity, to accurately compute the position of the center of mass of the vehicle as well as its heading in a defined coordinate system over time. In each measurement cycle, the vehicle position is computed over a time period T, which comprises sub-intervals beginning at a starting reference point t 0  and ending at t n .  
         [0019]    [0019]FIG. 1 schematically illustrates an exemplary VDC system in an automotive vehicle. It should be noted that the system shown is vastly simplified in comparison to actual VDC systems, and only relevant components are shown. As shown, a VDC system  5  includes an Electronic Controller Unit (“ECU”)  10  that may be implemented, for example, as a microprocessor. The ECU  10  further includes one or more Read Only Memory (ROM) units shown schematically as a single ROM unit  12 , and one or more Random Access Memory (RAM) units also shown as a single RAM unit  14 . The ROM units  10  store algorithms and data that collectively comprise the Electronic Stability Program (ESP). As noted above, and described in further detail below, the ESP receives primary input data and uses the input data to calculate various vehicle dynamics parameters, the values of which allow the ECU  10  to determine whether challenging driving conditions exist. If such conditions are present, the ECU  10  controls vehicle actuators to automatically adjust the vehicle to the driving conditions so as to promote safety. During a series of complex computations by which the vehicle dynamics parameters are determined, the ECU  10  may store the primary data, intermediate values and the determined dynamics parameters in the RAM units  14 . In addition, as will be discussed further below, any of the primary data, intermediate values and determined dynamics parameters may be stored in (and/or transferred to) a non-volatile memory  20 , which may implemented, for example, as EEPROM memory such as flash memory that retains data during and after power outages. The non-volatile memory  20  may be a singular or distributed entity, and may be co-located with the ECU  10  or it may be located at any node coupled to the VDC system  5  of the vehicle. In addition, if a remote communication unit is coupled to the VDC system  5 , data received and computed by the ECU  10  can be transmitted to a remote site in real-time, or when the vehicle is stationary (offline).  
         [0020]    Various active control sensors  16 ,  18  (of which there may be a plurality, but only two are shown) are directly coupled to the ECU  10  and communicate signals pertaining to the active (driver) control of the vehicle such as the steering wheel angle, brake pressure and gas pedal pressure (i.e., the intended vehicle velocity and acceleration). The information obtained from these sensors  16 ,  18  comprises part of the primary data received and used by the ESP. The ECU  10  is also coupled to a high-capacity data bus  25 , via which it also receives and transmits data. As shown, a yaw-rate sensor  30 , which may be of the metal-cylinder type, for example, is directly coupled to the data bus  25 . The yaw rate sensor  30  directly measures the rotational speed of the vehicle about the vertical axis of the sensor. A lateral acceleration sensor  32 , also directly coupled to the data bus  25  and which may include a magnet and a linear Hall element, measures the acceleration (at the sensor) in the lateral direction, perpendicular to the front-back axis of the vehicle. For purposes of illustration, a single wheel speed sensor (e.g., a tachometer)  34  is shown coupled to the data bus  25 , which should be taken to represent one sensor for each wheel of the vehicle. Each wheel speed sensor  34  measures the actual speed of the wheel, which may vary from the vehicle speed due to wheel slip. Each of the sensors  30 ,  32 ,  34  transmits detected signals through the data bus  25  to the ECU  10 .  
         [0021]    The data bus  25  also couples the ECU  10  to actuators to enable automatic control of vehicle operation. Coupled to the data bus  25  are a brake controller  40 , which may be implemented as a hydraulic system for converting signals generated from the ECU  10  into appropriate mechanical brake functions at various wheels, and an engine interface  45 . Through the engine interface  45  (and via the data bus  25 ), the ECU  10  receives data pertaining to engine conditions, and controls various engine parameters, such as throttle valve angles, in order to change the amount of torque delivered by the engine to the vehicle.  
         [0022]    The trajectory of the vehicle is estimated by the ECU  10  from calculation based on the signals it receives from the various sensors  30 ,  32 ,  34  of the vehicle. According to one embodiment of the present invention, the trajectory estimation occurs over a period of time T, beginning from a starting location. Once the time period T elapses, the end point of the first trajectory becomes the starting point of a new trajectory for a new time period of length T. In other words, each starting point is the origin of the reference coordinate system for the corresponding trajectory for a time period T. FIG. 2 illustrates an exemplary path of a vehicle over time, and indicates how the starting reference point changes after each cycle period ends. As shown, a vehicle starts at point A and travels in a northeasterly curve to a point B over time T. As the vehicle travels from point A to point B over time T, dynamics parameters are detected and trajectory estimations are calculated at time increments, t 1 , t 2  . . . t n−1 , t n , so that the trajectory over time T consists of a time series of n discrete points located in space with respect to a starting reference frame having an origin at A. The time interval between successive trajectory points depends upon ECU  10  update capability and may be as low as 20 milliseconds, for example.  
         [0023]    As shown in FIG. 2, when a vehicle has traveled for a time period T after starting from point A, the vehicle is located at point B. Point B then becomes the origin of an inertial reference frame for the next time period T. Over this subsequent period, the vehicle travels in a northwesterly curve towards point C with data again taken at time increments t 1 , t 2  . . . t n−1 , t n . From point C, the inertial frame shifts after travel duration T to point D and finally to point E, which is a final point along the vehicle&#39;s trajectory in the example shown in FIG. 2. In this manner, the inertial reference frame for calculation of the vehicle&#39;s trajectory shifts over time, and each single “trajectory” segment, corresponding to time period T, includes only the portion of the total travel path covered starting from the respective starting reference point. The gathered trajectory data may be stored in several ways. Each data point t i  may be calculated and then immediately stored in one or more non-volatile memory nodes  20 , or the ECU RAM  14  may store an entire trajectory, e.g., from point B to point C, and then transfer the entire trajectory data for non-volatile memory storage when the vehicle reaches point C and the reference point switches to C. It is advantageous for the ECU RAM  14  to store data pertaining to the current reference coordinate system, i.e., data for a given time period T, and to transfer the trajectory data for the given period T at the end of the given period T.  
         [0024]    [0024]FIG. 3 illustrates, in a functional block diagram, one embodiment of the method for vehicle trajectory estimation according to the present invention. As shown in FIG. 3, block  205  represents direct measurements from a vehicle, which measurements used in the algorithm may be the yaw rate ({dot over (ψ)}) from the yaw rate sensor  30  and the lateral acceleration (a y ) from the lateral acceleration sensor  32 . In addition, block  210  represents which signals include several estimated dynamics parameters, e.g., the longitudinal acceleration and velocity (a x , v x ), the yaw acceleration ({umlaut over (ψ)}), yaw sensor drift (Δ{dot over (ψ)}), and the slope offset (g sin θ) (where g is the gravitational constant and θ is the slope angle). As represented by summation block  214 , the yaw rate from block  205  and the sensor drift from block  210  are optionally added to correct the measured yaw rate, and as represented by block  218 , the lateral acceleration from block  205  is combined with the slope angle offset from block  210  to form a corrected lateral acceleration. Furthermore, as represented by integration block  220 , the corrected yaw rate is numerically integrated over the time of the trajectory, yielding a current yaw angle (ψ).  
         [0025]    Each of the corrected lateral acceleration, the corrected yaw rate, the integrated yaw angle, the yaw acceleration and the longitudinal acceleration and velocity are communicated to transformation block  230 , which performs a coordinate transformation from the moving vehicle frame to a stationary reference frame, for example, the earth. The transformation block  230  also incorporates information received from the vehicle-specific parameters block  235  concerning the offset of the yaw rate and lateral acceleration sensors  30 ,  32  from the center of gravity of the vehicle. As an example of such a transformation, the coordinate transformation for the lateral acceleration in the earth stationary reference frame, can be computed as 
           a   y(inertial) =( a   y   −{umlaut over (ψ)}l   cx   +{dot over (ψ)}   2   l   cy )cos(ψ)+( a   y   −{umlaut over (ψ)}l   cy   +{dot over (ψ)}   2   l   cx )sin(ψ)± g  sin(θ)  (1) 
         [0026]    where l cx  and l cy  denote the distances of the lateral acceleration sensor from the center of gravity of the vehicle in the respective longitudinal and lateral directions. The other dynamics parameters, both directly measured and estimated, are also transformed using similar coordinate transformation functions which are known in the art.  
         [0027]    The transformation block  230  delivers the transformed dynamics parameters to a trajectory integration block  240 , where x and y components of trajectory vector r (=sum of r x  and r y ) are computed by double integration over cycle time T of the estimated longitudinal acceleration (a x ) and the measured lateral acceleration (a y ): 
           r   x   =∫∫a   x ( dt ) 2   ,r   y   =∫∫a   y ( dt ) 2   (2) 
         [0028]    The vector addition of the x and y components provides the position of the vehicle in the inertial x-y plane during any of the sampled points t i  during period T. The heading of the vehicle—the direction in which the vehicle is pointing at a given time—is also computed at each sampled point by the yaw angle (ψ) computed from integration block  220 .  
         [0029]    It is emphasized that the above-described method for estimating the trajectory of a vehicle represents only one embodiment of the invention, as there are many other dynamics parameters, e.g., the body slip angle, which may be estimated in block  210  and then used to further derive the trajectory and/or heading of the vehicle. The body slip angle may be interpreted as the angle between the instantaneous heading of the vehicle and its actual trajectory. Under certain conditions, for example, when the vehicle is spinning on a low friction surface such as ice, the instantaneous heading of the vehicle is decoupled from its actual trajectory. According to the well-known equation for the rate of change of body slip angle (β), e.g., from “ Fundamentals of Vehicle Dynamics ” by T. D. Gillespie,  
               β   .     =       1     1   +     β                2           ·     {         a   y     v     -     ψ   ¨     -     β   ·       a   x     v       -       β   2     ·     ψ   ¨         }               (   3   )                               
 
         [0030]    the body slip angle can be estimated (for small slip angles) by integrating the above equation over time, yielding:  
               β        (   t   )       =       β   0     +       ∫   0   t            {         a   y     v     -     ψ   .       }                        t                   (   4   )                               
 
         [0031]    where ν is the absolute velocity of the vehicle. Thus, from the yaw rate, the lateral acceleration and estimated vehicle velocity (lateral and longitudinal), the heading of the vehicle under small slip-angle condition can be estimated via equation (4).  
         [0032]    As briefly described above, the trajectory estimation can be made in real-time (online) or when the vehicle is not running (offline), and can be made either in the vehicle itself or at a remote location. In one method according to the present invention, illustrated as a flow chart in FIG. 4A, the vehicle trajectory is computed in real-time in the vehicle. In step  300 , input data is obtained at sampling time t i . The ECU  10  then (in step  310 ) derives dynamics parameters and estimates the position of the vehicle in an appropriate coordinate system at a time t i  (r(t i )). The calculated position is then stored in step  320  within the ECU  10  or at one or more non-volatile memory nodes  20 , or is delivered via a remote communication system transmitter to a remote data storage location. Steps  300 ,  310  and  320  are repeated (step  330 ) for subsequent sampling points t i+1 , t i+2  until the end of the duration T (step  340 ), at which point the reference coordinate system is updated.  
         [0033]    A second embodiment of the trajectory estimation method according to the present invention is shown in FIG. 4B. In step  400 , input data is obtained during vehicle operation at sampling time t i , and the input data, rather than the trajectory, is stored (step  410 ) internally or externally. A time series of data is obtained and stored until the end of the duration T (step  420 ), at which point the coordinate system is updated. The input-data storage location, as noted, may be the ECU  10 , one or more non-volatile storage nodes  20  within the VDC system  5 , or it may be an external storage location. Regardless of the particular storage implementation, in step  430  the input-data storage location accesses the ESP at the ECU  10  (or at an external processor that also stores and executes an ESP equivalent to the vehicle program) to calculate the trajectory (step  440 ). In the second embodiment the trajectory is not calculated in real time, providing for trajectory estimation and reconstruction either while the vehicle is in operation or after operation of the vehicle ends. Moreover, locally stored data can be transferred from the ECU  10  or storage nodes  20  to an external analysis station that may be coupled to the ECU  10 , or include its own ESP. The analysis station may also include a display apparatus and a geographical database. This combination may provide for a realistic or semi-realistic illustration of the actual trajectory of the vehicle on a road section or open space upon which it traveled.  
         [0034]    In the foregoing description, the method and system of the present invention have been described with reference to a number of examples that are not to be considered limiting. Rather, it is to be understood and expected that variations in the principles of the method and apparatus herein disclosed may be made by one skilled in the art, and it is intended that such modifications, changes, and/or substitutions are to be included within the scope of the present invention as set forth in the appended claims. For example, the trajectory estimation algorithm is not limited to using only the ESP estimates described. Other estimated states and parameters could also be used as vehicle maneuvers become more critical. Also, estimates could be replaced with measurements as they become available.