Abstract:
The present invention features a system and method for estimating body states of a vehicle. The system includes at least two sensors mounted to the vehicle. The sensors generate measured vehicle state signals corresponding to the dynamics of the vehicle. A signal adjuster transforms the measured vehicle states from a sensor coordinate system to a body coordinate system associated with the vehicle. A filter receives the transformed measured vehicle states from the signal adjuster and processes the measured signals into state estimates of the vehicle, such as, for example, the lateral velocity, yaw rate, roll angle, and roll rate of the vehicle.

Description:
BACKGROUND  
       [0001]     This invention relates to a system and method of estimating body states of a vehicle.  
         [0002]     Dynamic control systems have been recently introduced in automotive vehicles for measuring the body states of the vehicle and controlling the dynamics of the vehicle based on the measured body states. For example, certain dynamic stability control systems know broadly as control systems compare the desired direction of the vehicle based on the steering wheel angle, the direction of travel and other inputs, and control the yaw of the vehicle by controlling the braking effort at the various wheels of the vehicle. By regulating the amount of braking torque applied to each wheel, the desired direction of travel may be maintained. Commercial examples of such systems are known as dynamic stability program (DSP) or electronic stability program (ESP) systems.  
         [0003]     Other systems measure vehicle characteristics to prevent vehicle rollover and for tilt control (or body roll). Tilt control maintains the vehicle body on a plane or nearly on a plane parallel to the road surface, and rollover control maintains the vehicle wheels on the road surface. Certain systems use a combination of yaw control and tilt control to maintain the vehicle body horizontal while turning. Commercial examples of these systems are known as active rollover prevention (ARP) and rollover stability control (RSC) systems.  
         [0004]     Typically, such control systems referred here collectively as dynamic stability control systems use dedicated sensors that measure the yaw or roll of the vehicle. However, yaw rate and roll rate sensors are costly. Therefore, it would be desirable to use a general sensor to measure any body state of the vehicles, that is, a sensor that is not necessarily dedicated to measuring the roll or yaw of the vehicle.  
       BRIEF SUMMARY OF THE INVENTION  
       [0005]     In general, the present invention features a system and method for estimating body states of a vehicle. The system includes at least two sensors mounted to the vehicle. The sensors generate measured signals corresponding to the dynamic state of the vehicle. A signal adjuster or signal conditioner transforms the measured vehicle states from a sensor coordinate system to a body coordinate system associated with the vehicle. A filter receives the transformed measured vehicle states from the signal adjuster and processes the measured signals into state estimates of the vehicle, such as, for example, the lateral velocity, yaw rate, roll angle, and roll rate of the vehicle.  
         [0006]     The filter may include a model of the vehicle dynamics and a model of the sensors such that the states estimates are based on the transformed measured signals and the models of the vehicle dynamics and sensors. The filter may also include an estimator implemented with an algorithm that processes the transformed measured vehicle states and the models of the vehicle dynamics and sensors and generates the state estimates.  
         [0007]     The present invention enables measuring the body states of a vehicle with various types of sensors that may not be as costly as dedicated roll or yaw rate sensors. For example, the sensors may all be linear accelerometers. However, in some implementations, it may be desirable to use an angular rate sensor in combination with linear accelerometers.  
         [0008]     Other features and advantages will be apparent from the following drawings, detailed description and claims. 
     
    
     BRIEF DESCRIPTION OF THE DRAWINGS  
       [0009]      FIG. 1  depicts a block diagram of the processing of the vehicle states in accordance with the invention.  
         [0010]      FIG. 2  depicts a general array of sensors for measuring body states of a vehicle. 
     
    
     DETAILED DESCRIPTION  
       [0011]     In accordance with an embodiment of the invention,  FIG. 1  illustrates a system  10  that measures the vehicle states of a vehicle identified as block  12 . Specifically, the system  10  includes a plurality of sensors  14  that measure signals which contain parts related to components of the vehicle states of the vehicle dynamics  16  produced, for example, when the angle of the steering wheel δ is changed.  
         [0012]     The system  10  also includes a signal conditioner or adjuster  18  that receives measured signals from the sensors  14  and a filter  20  that receives the adjusted signals from the signal adjuster  18 . In certain embodiments, the filter  20  is a Kalman filter including a model of the vehicle dynamics  22  and a model of the sensors  24 . These models are described below in greater detail.  
         [0013]     The signal adjuster  18  and the sensor model  24 , which incorporates the model of the vehicle dynamics  22 , provide inputs to an estimator  26 . An algorithm with a feed back loop  28  is implemented in the estimator  26  to process the transformed signals with the models of the vehicle dynamics and the sensors. The output from the estimator  26  is the state estimates {right arrow over (x)} v . The body states estimates may include the roll angle, roll rate, yaw rate, and lateral velocity, as well as other body states.  
         [0014]     In some embodiments, the sensors  14  measure the linear acceleration at a particular location where the sensor is mounted to the vehicle. When the sensors are not aligned in a plane perpendicular to the axis of interest, the measured values contain biases proportional to the angular rates about other axes. Similarly, when the measurement axes of the sensing devices are not coincident, the measured values contain biases proportional to the angular acceleration about other axes. Moreover, when the measurement axes of the sensing devices are not coincident and are not mounted along a body reference axis, the measured values contain unique gravity biases dependent upon the difference in mounting angle of the sensors and the body lean angle of the vehicle.  
         [0015]     To address these biases, a general implementation of the system  10  can be employed as illustrated in  FIG. 2 . Here the sensors  14  (identified individually as S 1  and S 2 ) are in known and fixed positions on the vehicle body  12  and the orientation of the measurement axes of the sensors S 1  and S 2  are known and fixed. Specifically, the location and orientation of a sensor S i  is provided by the relation 
 
P i (x i , y i , z i , θ i , χ i , φ i ),   (1) 
 
 where x i , y i , z i  are the space coordinates of the sensor S i , θ i  is the sensor yaw angle, that is, the orientation of the sensor&#39;s measurement axis in the X B , Y B  plane with respect to the X B  axis, χ i  is the sensor pitch angle, that is, the orientation of the sensor&#39;s measurement axis with respect to the X B , Y B  plane, and φ i  is the sensor roll angle, which is the rotation about the respective measurement axis. 
 
         [0016]     The sensors S i  measure the linear acceleration at the location Pi, namely, {right arrow over (α)} i ={right arrow over (m)} i ·|m i |=[α xi , α yi , α zi ] T , where {right arrow over (m)} i  is the unit vector along the measurement axis, and |m i | is the magnitude of the acceleration along the measurement axis.  
         [0017]     Since the acceleration {right arrow over (α)} i  measured by the sensor S i  is the acceleration in the sensor coordinate system, the measured accelerations are transferred to a body coordinate system. In certain embodiments, it is assumed that in an array of single axis accelerometers each accelerometer has a measurement axis referred to as the x sensor  axis. Accordingly, the transformation from the sensor coordinate system to the body coordinate system is provided by the expression  
                           a   ⇀     i     ×       Body   _     i       =       ⁢           a   ⇀     i     ⁡     [           x     body   ,   i                 y     body   ,   i                 z     body   ,   i             ]       =     [           a     x   ,   body                 a     y   ,   body                 a     z   ,   body             ]                     where   ⁢           ⁢       Body   _     i       =       ⁢     [           x     body   ,   i                 y     body   ,   i                 z     body   ,   i             ]                 =       ⁢       [             θ   i   c     ⁢           ⁢     χ   i   c                 -     θ   i   s       ⁢     ϕ   i   c       -       θ   i   c     ⁢           ⁢     χ   i   s     ⁢     ϕ   i   s                   θ   i   s     ⁢     ϕ   i   s       +       θ   i   c     ⁢           ⁢     χ   i   s     ⁢     ϕ   i   c                     θ   i   s     ⁢           ⁢     χ   i   c                 θ   i   c     ⁢     ϕ   i   c       +       θ   i   s     ⁢           ⁢     χ   i   s     ⁢     ϕ   i   s                   -     θ   i   c       ⁢     ϕ   i   s       -       θ   i   s     ⁢           ⁢     χ   i   s     ⁢     ϕ   i   c                           ⁢     χ   i   s                     ⁢       χ   i   c     ⁢     ϕ   i   s                       ⁢       χ   i   c     ⁢     ϕ   i   c               ]     ·                     ⁢     [           x   sensor               y   sensor               z   sensor           ]             ⁢     
     ⁢   where   ⁢     
     ⁢           _c     ⁢     =     cos   ⁡     (   _   )           ⁢     
     ⁢           _s     ⁢     =     sin   ⁢           ⁢     (   _   )           ⁢     
     ⁢       θ   i     =     sensor_yaw   ⁢   _angle       ⁢     
     ⁢       χ   i     =     sensor_pitch   ⁢   _angle       ⁢     
     ⁢       ϕ   i     =     sensor_roll   ⁢   _angle               (   2   )             
 
 and [x sensor  y sensor  z sensor ] T =[1 0 0] T , since x sensor  is assumed to be the measurement axis for each of the single axis accelerometers. 
 
         [0018]     Note that the transformation identified in Equation (2) is typically performed in the signal adjuster  18  ( FIG. 1 ). The signal adjuster  18  may also provide a DC bias offset compensation to compensate for the biases discussed above.  
         [0019]     Regarding the Kalman Filter  20 , the model of the vehicle dynamics  22  for a state vector 
 
{right arrow over (x)} v   =[{dot over (y)}   v   r   v θ v {dot over (θ)} v ] T    (3) 
 
 is provided by the expression  
                     x   -&gt;     .     v     =       A   ·       x   ⇀     v       +     B   ·     u   ⇀           ⁢                   (   4   )               where   ⁢           ⁢             [             y   ¨     v                 r   .     v                 θ   .     v                 θ   ¨     v           ]     =       [           -         C   F     +     C   R       mu                     C   R     ⁢   b     -       C   F     ⁢   a       mu     -   u         0       0                   C   R     ⁢   b     -       C   F     ⁢   a           I   z     ⁢   u                   -     C   F       ⁢     a   2       +       C   R     ⁢     b   2             I   z     ⁢   u           0       0           0       0       0       1             -     h       I   x     ⁢   u                 h   ⁡     (         C   R     ⁢   b     -       C   F     ⁢   a     -     mu   2       )         I   x             -     K     I   x               -     C     I   x               ]     ⁢           ⁢               [             y   .     v               r   v               θ   v                 θ   .     v           ]     +         [             C   F     m         0                 C   F     ⁢   a       I   z           0           0       0               C   F     m         0         ]     ⁢           [         δ           g         ]     ⁢           ⁢   and   ⁢           ⁢   where   ⁢     
     ⁢       y   .     v         =       lateral   ⁢           ⁢   velocity   ⁢           ⁢   of   ⁢           ⁢   the   ⁢           ⁢   vehicle   ⁢     
     ⁢   r     =       yaw   ⁢           ⁢   rate   ⁢           ⁢   of   ⁢           ⁢   the   ⁢           ⁢   vehicle   ⁢     
     ⁢     θ   v       =       roll   ⁢           ⁢   angle   ⁢           ⁢   of   ⁢           ⁢   the   ⁢           ⁢   vehicle   ⁢     
     ⁢       θ   .     v       =       roll   ⁢           ⁢   rate   ⁢           ⁢   of   ⁢           ⁢   the   ⁢           ⁢   vehicle   ⁢     
     ⁢     C   F       =       cornering   ⁢           ⁢   stiffness   ⁢           ⁢   of   ⁢           ⁢   the   ⁢           ⁢   front   ⁢           ⁢   axle   ⁢     
     ⁢     C   R       =       cornering   ⁢           ⁢   stiffness   ⁢           ⁢   of   ⁢           ⁢   the   ⁢           ⁢   rear   ⁢           ⁢   axle   ⁢     
     ⁢   a     =       distance   ⁢           ⁢   from   ⁢           ⁢   center   ⁢           ⁢   of   ⁢           ⁢   gravity   ⁢           ⁢   to   ⁢           ⁢   the   ⁢           ⁢   front   ⁢           ⁢   axle   ⁢     
     ⁢   b     =       distance   ⁢           ⁢   from   ⁢           ⁢   center   ⁢           ⁢   of   ⁢           ⁢   gravity   ⁢           ⁢   to   ⁢           ⁢   the   ⁢           ⁢   rear   ⁢           ⁢   axle   ⁢     
     ⁢   m     =       mass   ⁢           ⁢   of   ⁢           ⁢   the   ⁢           ⁢   vehicle   ⁢     
     ⁢   h     =       height   ⁢           ⁢   of   ⁢           ⁢   the   ⁢           ⁢   center   ⁢           ⁢   of   ⁢           ⁢   gravity   ⁢     
     ⁢           ⁢   above   ⁢           ⁢   the   ⁢           ⁢   roll   ⁢           ⁢   axis   ⁢           ⁢          ⁢     I   z       =       yaw   ⁢           ⁢   moment   ⁢           ⁢   of   ⁢           ⁢   inertia   ⁢     
     ⁢     I   x       =       roll   ⁢           ⁢   moment   ⁢           ⁢   of   ⁢           ⁢   inertia   ⁢     
     ⁢   C     =       vehicle   ⁢           ⁢   roll   ⁢           ⁢   dampening   ⁢     
     ⁢   K     =       vehicle   ⁢           ⁢   roll   ⁢           ⁢   stiffness   ⁢     
     ⁢   u     =       longitudinal   ⁢           ⁢   vehicle   ⁢           ⁢   speed   ⁢     
     ⁢   δ     =       steering   ⁢           ⁢   angle   ⁢           ⁢   of   ⁢           ⁢   the   ⁢           ⁢   tires   ⁢     
     ⁢   g     =       gravitational   ⁢           ⁢   acceleration   ⁢     
     ⁢     *   .       =         ⅆ     ⅆ   t       *     
     ⁢   and   ⁢           ⁢     *   ..       =       ⅆ   2       ⅆ     t   2                                                               (   5   )             
 
         [0020]     As for the model of the sensors  24 , the model of laterally oriented sensors is provided by the expression  
         [0000]      A   y,meas   =ÿ   v   +{dot over (r)}   v   d   xtoYA   +{umlaut over (θ)}   v   d   ztoRA   +r   v   u    (6)  
         [0021]     Accordingly, since A y,meas =α y,body  from Equation (2), substituting the expressions for ÿ v , {dot over (r)} v , {umlaut over (θ)} v , and r v  from Equation (5) into Equation (6) yields the expression  
                     a     y   ,   body       =       ⁢       [         a   11     ⁢       y   .     v       +       a   12     ⁢     r   v       +         C   F     m     ⁢   δ       ]     +                     ⁢         [         a   21     ⁢       y   .     v       +       a   22     ⁢     r   v       +           C   F     ⁢   a       I   z       ⁢   δ       ]     ⁢     d   xtoYA       +                     ⁢         [         a   41     ⁢       y   .     v       +       a   12     ⁢     r   v       +       a   43     ⁢     θ   v       +       a   44     ⁢       θ   .     v       +         C   F     m     ⁢   δ       ]     ⁢     d   ZtoRA       +       r   v     ·   u                   =       ⁢         [       a   11     +       a   21     ⁢     d   xtoYA       +       a   41     ⁢     d   ztoRA         ]     ⁢       y   .     v       +                     ⁢         [       a   12     +       a   22     ⁢     d   xstoYA       +       a   42     ⁢     d   ztoRA       +   u     ]     ⁢     r   v       +                     ⁢         [       a   43     ⁢     d   ztoRA       ]     ⁢     θ   v       +                     ⁢         [       a   44     ⁢     d   ztoRA       ]     ⁢       θ   .     v       +                     ⁢       [         C   F     m     +           C   F     ⁢   a       I   z       ⁢     d   xtoYA       +         C   F     m     ⁢     d   ztoRA         ]     ⁢   δ                   (   7   )             
 
 where α kl  is the element in the k row and l column of the matrix A, d xtoYA  is the distance along the x axis from a sensor to the yaw axis, and d ztoRA  is the distance along the z axis from the sensor to the roll axis. 
 
         [0022]     The model for vertically oriented sensors is 
 
 A   z,meas   =−g+{umlaut over (θ)}   v   d   yraRA    (8) 
 
 Hence, from Equations (2) and (5)  
                     a     z   ,   body       =       ⁢       -   g     +       [         a   41     ⁢       y   .     v       +       a   42     ⁢     r   v       +       a   43     ⁢     θ   v       +       a   44     ⁢       θ   .     v       +         C   F     m     ⁢   δ       ]     ⁢     d   ytoRA                     =       ⁢         ⌊       a   41     ⁢     d   ytoRA       ⌋     ⁢       y   .     v       +                     ⁢         ⌊       a   42     ⁢     d   ytoRA       ]     ⁢     r   v       +                     ⁢         ⌊       a   43     ⁢     d   ytoRA       ]     ⁢     θ   v       +                     ⁢         [       a   44     ⁢     d   ytoRA       ]     ⁢       θ   .     v       +                     ⁢         [         C   F     m     ⁢     d   ytoRA       ]     ⁢   δ     +                     ⁢     [     -   g     ]                   (   9   )             
 
 where d ytoRA  is the distance along the y axis to the roll axis. 
 
         [0023]     And for longitudinally oriented sensors, the sensor model is provided by the expression 
 
 A   x,meas   =−{dot over (r)}   v   d   ytoYA    (10) 
 
 such that upon employing Equations (2) and (5), Equation (10) becomes  
               a     x   ,   body       =         -     a   21       ⁢     d   dtoYA     ⁢     y   .       -       a   22     ⁢     d   dytoYA     ⁢     r   v       -       b   21     ⁢     d   ytoYA     ⁢   δ               (   11   )             
 
 where d dytoYA  is the distance along the y axis to the yaw axis and b 21  is the element in the second row and first column of the matrix B. 
 
         [0024]     The algorithm implemented in the estimator  26  processes the expressions from Equations (7), (9), and (11) through a filter (an estimation algorithm) to provide the estimates for the state vector {right arrow over (x)} v =[{dot over (y)} v  r v  θ v  {dot over (θ)} v ] T .  
         [0025]     Note that the above discussion is directed to obtaining a solution for the state vector {right arrow over (x)} v  in continuous time. Therefore, {right arrow over (x)} v , is typically discretized according to the expression 
 
 {right arrow over (x)}   v ( k+ 1)= A   d   {right arrow over (x)}   v ( k )+ B   d   {right arrow over (u)}(   k )   (12) 
 
 where k identifies the k th  time step and the matrices A and B can be discretized according to the approximations 
 
 A   d   =I   n +Δ k   ·A  
 
 and 
 
 B   d =Δ k   ·B  
 
 where I n  is the nth order identity matrix, which in this case is a fourth order identity matrix, and Δ k  is the time step. 
 
         [0026]     Although the above embodiment is directed to a sensor set with linear accelerometers, hybrid-sensor-sets are contemplated. For example, an angular rate sensor can be used in the vehicle  12  and a model of that sensor can be used in the “Kalman Filter” box  20 . Specifically, for a yaw rate sensor, the model is [0 1 0 0], that is, the sensor measures yaw rate and nothing else.  
         [0027]     Hence, in stability control, in which measuring yaw rate and roll rate/angle is useful, four accelerometers can be used for the sensors  14 . Alternatively, for a hybrid system, two accelerometers and an angular rate sensor may be employed. Other examples of hybrid systems include, but are not limited to, two lateral and two vertical accelerometers; two lateral, two longitudinal, and two vertical accelerometers; and two lateral, two vertical accelerometers, and an angular rate sensor.  
         [0028]     Other embodiments are within the scope of the claims.