(email: alag@pawn.Berkeley.EDU, goebel@ton.Berkeley.EDU, aagogino@euler.Berkeley.EDU )

Department of Mechanical Engineering

University of California at Berkeley, Berkeley, CA 94720, USA

ABSTRACT

For longitudinal control the automated vehicles in Intelligent Vehicle Highway System (IVHS) require sensors to estimate the relative distance and velocity between vehicles. High data fidelity of these sensors is required to maintain the reliability and safety of the IVHS. In this paper, we develop a methodology for validation and fusion of sensory readings obtained from multiple sensors used for tracking automated vehicles and for avoiding objects in its path. We introduce tracking models for the various operating states of the automated vehicle, namely vehicle following, maneuvering, i.e. split, merge, lane change, emergencies, and for the lead vehicle in a platoon. The Kalman filtering approach is proposed for the formation of real time validation gates. This along with the Algorithmic Sensor Validation filter is used for sensory data validation. The validated data are then fused by using a Bayesian method called the probabilistic data association filter. The procedure is demonstrated by two examples using simulated data, data obtained from a platooning test set-up.

1. INTRODUCTION

The IVHS envisions to significantly increase safety and highway capacity via the integration of control, communication and computing technologies [1]. An early analysis indicates that with a completely automated vehicle control system, freeway lane capacity will double or even triple while safety conditions will improve. In the IVHS paradigm, closely spaced automated vehicles will be traveling at high velocities in their respective lanes. For this task the automated vehicles will be equipped with multiple sensors to detect and if present to estimate the relative position, velocity and acceleration of the closest target (term used in general for a vehicle or any obstruction) in its path (lane). For this purpose, no single sensor can be relied on to deliver (acceptably) accurate information all the time. This is especially true in the IVHS context where reliability is of prime importance, as it deals with human safety. Associated with any sensor is a set of limits that defines its useful operating range. Unless these limits are taken into account incorrect inferences could be drawn. Sensor signals are also inevitably corrupted by noise. However, when two or more sensors are operating within their limits, combining (or integrating, or fusing) their measurements can provide a more robust or reliable reading than that provided by any one sensor. This is because signals tend to be correlated between sensors whereas noise is uncorrelated. In this paper, we present a methodology for real time validation and data fusion of the sensory data obtained from the multiple longitudinal sensors.

We begin by developing models for the various operating states of the IVHS system. A rule-based system is used to find the operating state of the vehicle and for switching between the models [2]. Next, the Kalman filtering scheme is used to develop validation gates. These validation gates along with the Algorithmic Sensor Validation Filter are used for validation of the sensory data. A rule-based system is proposed to estimate sensor faults such as sensor bias, drift and to predict the degradation in sensor performance over time. Next, we present the Bayesian method: probabilistic data association filter (PDAF) for data fusion. The methodology is demonstrated with two examples. The first example shows the detailed methodology (validation, fusion, detecting sensor bias) on simulated data. The second shows the PDAF fusion methodology applied to sensor output from three sensors, namely a radar, sonar and an infrared sensor operating under dynamic test conditions.

2. METHODOLOGY

In the IVHS system a string of closely spaced vehicles, called platoons will be traveling under automated control (at high velocities). Depending on their position on the automated highway, the vehicles can be divided into two types: those that are in the platoon (follower vehicles) and those which lead the platoon (leader) or which are traveling alone (equivalent to a platoon of length one). The tracking tasks for these two vehicle states is different. It is therefore necessary to distinguish between these two cases in our validation and fusion methodology. To deal with the various situations, we further divide the operating conditions into several states. There are three states for the follower vehicles: steady state, transient, and hazardous states. The first two are desired states while the third is undesired. A vehicle under automatic control is defined to be in the steady state when it is in a platoon and is trying to follow the one before it at a fixed (known) distance. Whenever the vehicle carries out a maneuver such as a split (leaving the platoon), merge (joining the platoon) or lane change it is defined to be in a transient state. This state involves relative acceleration between the vehicles and includes the state of the lead vehicle in the platoon. The last state, the hazardous state is defined as the state when the vehicle carries out emergency maneuvers to avoid a catastrophe (accident). The lead vehicle, on the other hand, has only two states, a desired state, and a hazardous state.

Figure 1 shows the methodology followed for the validation and data fusion process. We first begin by building models for the operating states of the two types of vehicles: follower and leader, During operation these models are used to build validation gates for the sensory data by using a Kalman filter estimate. Inherent with the IVHS system is the availability of additional information such as the velocity and acceleration of the lead vehicle in the platoon. This is the velocity and acceleration that other vehicles in the platoon are trying to follow. In addition to the desired velocity and acceleration, the coordination layer controller also transmits information about maneuvering techniques (merge, split and lane change) to the vehicles in a platoon. This information is transmitted through the communication channel and can be used for switching between the models. A knowledge based system is used to perform this task. In the absence of information a test for hypothesis can be carried out to determine the states of the vehicle. To estimate the deviation in the process from the model and the sensor noise we use the Kalman filtering algorithm to form validation gates. This along with the Algorithmic Sensor Validation (ASV) is used in the validation process. After the validation process, a Bayesian method namely the Probabilistic Data Association Filter is used for data fusion. There is also a knowledge based system which tries to estimate the sensor bias, detect other sensor faults such as drift and degradation and looks at the sensor performance.

     


     Figure 1: Flowchart of the Methodology

Follower and lead vehicle will be modeled differently because of their distinct operating conditions. In particular, the operating states (steady, transient, and hazardous for follower, and desired and hazardous for lead vehicle) are considered and different models for each case are introduced. We begin with the follower states.

2.1.1 Steady State

Steady state is defined as the state in which the automated vehicle in a platoon is following the one before it at a constant (zero) relative velocity, i.e. at . However, in practice the velocity will undergo at least slight changes. We model these changes as a continuous time white noise as follows,

          (1)
     ,
 
          (2)

where 
      is the expected value of 

     q(t)  is the covariance of .

          (3)
          (4)

where  Q  is the covariance of v(k).

Varaiya [1] defines three kinds of maneuvering techniques for the vehicles in IVHS: merge, split and lane change. Merge is the procedure by which a vehicle joins a platoon, while in split it leaves the platoon. In lane change the vehicle moves to the next lane. Smooth trajectories have been designed for these procedures [3]. These trajectories have been designed to keep the vehicle jerk (ride quality) and acceleration within acceptable limits. For example, for a lane change maneuver the trajectory is relatively simple as it requires a lateral position change of a fixed distance which is the distance between the centers of adjoining lanes and the final reference for trajectory design (i.e. center of target lane) is fixed. In general, for merge and split the desired spacing and desired relative velocities at the beginning and end ( , respectively) are known. Using these four conditions a desired spacing profile can be generated which is of the form

          (5)

where 
     spd is the desired spacing profile

     co, c1, c2, c3  are constants.

Trajectories for various conditions (with and without platoon acceleration) have been developed by Narendran [3] and will not be repeated here. The important point is that the desired relative distance and relative velocity of the vehicles during the maneuver is known and can be used for our sensor validation procedure. In this case, we model the residuals, i.e. the difference between the actual distance and the desired distance

          (6)

where  
     r  is the residual  and 
     xd is the desired distance. 

2.1.3 Hazardous State

The hazardous state demands special attention because it is here where the safety of the system is at stake. Five major hazards [4], and sequence of events that precursor these hazards have been identified. For example, hazard A is defined as the condition when the distance between two vehicles in a platoon is less than the safe inter-platoon distance. A knowledge based system is required which identifies the hazard and consequently switches to the proper model. To this end, two models have been developed which deal with specific hazardous situations. One assumes a constant velocity between the target and the follower vehicle (which is too close). The other one assumes a constant acceleration between the target and the follower vehicle (which is too close). These would be a combination of the constant distance model (explained above), the constant velocity and constant acceleration models (discussed next).

For the first case with constant velocity we model the changes in the relative acceleration as a continuous time white noise ,

     ,
where 
      has the same properties as in (2).  

          (7)

where 
      is a scalar valued zero-mean white noise sequence

	
      (8)

	
      (9) 

	
      (10)

where 
      is the standard deviation, and 
     Q  is the covariance of the process noise. 

For the second case with constant relative acceleration between the two vehicles . This model is applicable in case of emergency braking. However, in practice the acceleration will never be exactly constant. We model its changes by means of a zero-mean white noise as follows . The smaller the variance of the noise the more constant is the acceleration. In this case the state vector and the continuous time state equation is

          (11)

          (12)	

          (13)
          (14)

where 
     Q is  the covariance matrix for the process noise discretized from
continuous time. 

As for the other cases we can directly define the process noise in the discrete case. where and . In this case, we assume that the acceleration is a discrete time Wiener process, i.e. non stationery random process with mean zero and variance . is the acceleration increment in a sampling time ("jerk"). The process noise covariance is

      (15)

2.1.4 Modeling the Lead Vehicle

The lead vehicle in a platoon sets the velocity and the acceleration for the other vehicles in the platoon. This is determined by the conditions on the highway. A number of different scenarios are possible, for example there may be no target in the operating range of the longitudinal sensor, or the target may be a part of another platoon or the target could be a stationery target (e.g. a stalled car in the same lane), etc. The lead vehicle is more prone to misinterpret longitudinal sensor data than the follower vehicle. The short distance range sensor readings will in most cases be out of range. False readings can occur when the signal is reflected by roadside objects in which case it could be mistaken as an object on the lane. To tackle the problems of misinterpretation a rule-based module helps to categorize different states for which models are applied. The distance to the object sensed, the road conditions, the speed, and the acceleration all play a role in the design of these rules.

The desired state occurs when the lead vehicle either follows another platoon in a safe distance or the next object is out of range. Other states are considered as hazardous. These cases can be accommodated by the methods described for the hazardous state of the follower vehicle but with a different set of sensor signals. An example for the use of the constant velocity model is resembled by the case when a lead vehicle moving with a constant velocity detects a stationery object in its path. For the time instant between detecting the object and taking corrective action to its findings (such as braking), the constant velocity model would be active. Once the deceleration procedure is initiated, the constant (negative) acceleration model would be active. The rule-base module is used for switching between these models. The main advantage of using multiple state model is that it improves the performance and decreases the uncertainty. Figure 3 gives the classification of the various operating states.

     


     Figure 3:  Classification of Operating States

A sensor validation scheme should fulfill the following two tasks: detection and diagnosis. We use two methods for sensor validation, one is model based, while the other does not rely on a model. The latter is based on first principles and on physical constraints while the model based validation uses Kalman filtering techniques.

2.2.1 Algorithmic Sensor Validation

We first use the Algorithmic Sensor Validation filter (ASV), a technique which is not model (operating states of the vehicles developed in the previous section) based. This is useful for detecting outlier readings. It compares the difference between the sensor readings and the validated reading at the previous sampling time to the maximum possible change that is possible in the relative distance between the two vehicles in one sampling time. This is obtained by looking at the physical constraints of the system. For example, the acceleration range of a vehicle lies between [-5,2] m/s^2. (during normal operation, the range is even smaller). Therefore, the maximum relative acceleration between two vehicles (assuming worst case) is 7 m/s^2. Knowing the approximate operating velocity of the vehicle (between [0, 30] m/s) one can calculate the maximum possible change in distance between the two vehicles. In general, an upper absolute bound can be obtained by

          (16)

This technique is active at all times and is not bound to a particular state or model. Therefore, it is the first check within the system for process deviations.

2.2.2 Model Based Validation

After the ASV, the model based validation takes place. In particular, a validation gate is obtained by using a Kalman filtering estimate applied to an appropriate model for the vehicle state. Readings that lie outside the validation gate are classified as faulty. Unlike the ASV filter, the validation gate is possible only when the vehicle is in a state for which a model exists. First, the principle of Kalman filtering is outlined and then the process of formation of the validation gates is shown.

2.2.2.1 Kalman Filtering

We begin by reviewing the principle of Kalman filtering which is used in the validation process [5, 6] Consider a discrete time dynamic system described by

          (17)

where 
     x(k) is the state at the time k; 
     u(k) is the (known) input or control signal; 
     v(k)  is a sequence of zero-mean, white, Gaussian process noise with covariance Q(k);
     F is the system model, and 
     G is the gain through which the input is multiplied

where z(k) = is the stacked observation

vector; w(k) is a sequence of zero-mean white Gaussian measurement noise with covariance R(k); H is the observation model

The initial state is assumed to be Gaussian with mean and covariance P(0|0). The two noise sequences and the initial state are assumed to be independent, i.e. we assume

For the above system the Kalman filter provides a recursive solution for the estimate of the state x(k) in terms of the estimate and the new measurements z(k).

The one step prediction of the state is

          (18)		
          (19)		
          (20)

          (21)		

where 
     P(k+1|k) is the one step prediction covariance ; and
     S(k+1) is the measurement prediction covariance
          (22)	
          (23)

          (24)

2.2.2.2 Validation Gates

For the longitudinal sensors the following method is used for validation. It is assumed that the true measurement of the distance between the vehicles at sampling time k+1 is normally distributed, conditional on the sensor readings up to sample k, i.e.

     

		

where  
     S(k+1) is the associated measurement prediction covariance matrix
obtained by the Kalman filtering process.

Based on this one can define a region in the measurement space where the measurement will be found with some (high) probability (for example a 3 sigma bound corresponds to a confidence of 99.8%)

     


              
          (25)	

where 
     
is the region defined above is called the validation region or the gate. It is
the ellipse (or ellipsoid) of probability concentration-the region of minimum
volume that contains a given probability of mass under the Gaussian assumption.
Measurements that lie within the gate are considered valid; those outside are
labeled as questionable. 

2.3 Data Fusion: Probabilistic Data Association Filter (PDAF)

Data fusion can be tackled by several different methods. For example by Generalized Evidence Processing Theory [7], Bayesian [8], Dempster-Shafer methods [9] and linear estimators [10]. After sensor validation the set of measurements consist of correct and incorrect measurements the latter originating from clutter or false alarms. Clutter means detections or returns from nearby objects, weather, electromagnetic interferences, acoustic anomalies, false alarms, etc., that are generally random in number, location and intensity.

The simplest approach for tracking a target in a cluttered environment is to select the validated measurement that is closest to the predicted measurement and use it in the tracking filter as if it were the correct one. This results in the nearest neighbor standard filter (NNSF). Another common approach is to use the a priori information about the senosrs to build a Kalman filter model of a system with no uncertainty and use it for the fusion [11].

It may be possible that more than one measurement lies in the validated region. This implies that all of these could have originated from the target. Hence, one should try and use all these measurements for estimating the distance of the target. One method to do this is to split the track into multiple hypotheses every time more than one measurement is found in the validation region. The likelihood function can then be used for deciding which hypothesis to accept. We use the Bayesian approach for data fusion. Here, the probabilities of the validated readings being correct are calculated using the Probabilistic Data Association Filter [12].

Let the set of validated measurements at time k be

         (26)

where  
     Z(k) is the set of validated measurements; 
     zi(k) is the validated measurement; 
     mk  (which is also a random variable) is the number of measurements in the validation region. 

In PDAF we make the following assumption about the past (for ease of computation)

          (27)

i.e. the state is assumed to be normally distributed (Gaussian) according to the latest estimate and covariance matrix. We define the following events

is the target originated measurement,

none of the measurements at time k is target originated

Let the probabilities with which these events occur be denoted as

     
          (28)

          (29)

where 
 is the updated state estimate conditioned on the event 
 that the ith validated measurement is correct.

From the Kalman Filter estimate ,

          (30)

where 
      is the corresponding innovation.	

Hence, the state update equation of the PDAF is

          (31)
          (32)

where 
      is the combined innovation. 

A probabilistic inference is made on the number of measurements in the validation region, i.e.

The associated probabilities are given by

where

PG is the probability that the correct measurement falls in the validation region; PD is the target detection probability; nz is the dimension of the measurement z and cnz is the volume of the nz dimensional hypersphere (c1=2, c2= p, c3=4 p/3, etc.) [13].

Assuming a Poisson density the probabilities of the events conditioned only on the number of validated measurements are

The volume of the elliptical (i.e. Gaussian based) validation region is

2.4 Rule Based Module

The multifunctional rule-based module performs model selection and assists in the sensor validation process. It also detects sensor bias and the degradation of sensors. For model selection, it uses information from the longitudinal sensors as well as from the communication channel. This way, it can determine whether a preceding vehicle (or other object) is moving with a constant speed or acceleration. Information about hazards found in preceding vehicles are transmitted to the follower vehicles and can be used for this purpose. Signals from the longitudinal sensors of the lead vehicle are difficult to interpret because roadside objects and vehicles on other lanes can be misinterpreted as objects on the lane. The distance and allowable bandwidth of the sensor will give information about the validity of such readings. Also of importance is roadside information which gives information, for example about the radius of the road which allows for a potential correction of the reading.

To detect abnormal operation of the sensors and for testing malfunction hypothesis we use validation modules. The rule-based system also contains specifications about the various sensors, such as the measurement range, accuracy, effect of changing environment on sensor performance etc. Furthermore, complete malfunction (failure) of the sensors is relatively easy to detect, but when failure occurs, it can lead to catastrophic events. Therefore, it is imperative to detect latent malfunctions in the sensor to predict the degradation in the sensor performance. For this purpose a sliding window (set of several successive measurements) can be used. The statistical properties of the measurement residue w(k) (which is the fused Kalman filter estimate for the relative distance between the vehicles given by w(k)=z(k)- ). A failure signature typically takes the form of residual biases that characterize the specific failure. It is relatively simple to check for sensor bias when multiple sensors are present. The measurement residue ideally should be zero mean, white and Gaussian. An estimate of the sensor bias can be obtained by looking at the mean of measurement residue over a number of sliding windows. If the mean is non zero and remains constant then it can be attributed to the bias in the sensor. This can be tested by a test of hypothesis. The statistical properties mean, variance and whiteness for various sliding windows can be used to detect any changes in the sensor performance by comparing it against those under normal conditions and for fault signatures contained in the rule-based system. For further details interested readers are referred to [11].

3. Example

To illustrate the methodology we use two examples. The first example explains in detail the validation, fusion and sensor bias detection methodology. The second example uses data obtained from platooning test set up using a radar, a sonar, and an optical sensor.

We first consider the case of constant distance in a platoon. Assuming a sampling time of 0.02 seconds, a conservative estimate for change in distance is 0.02 m under normal conditions. Hence, for the simulations the covariance of the process Q was taken as .02*.02. The initial distance between the vehicles was taken as 4 meters. Three sensors are used with data generated as follows. The covariance of the sensor noise R was taken as 0.5 for all three sensors for the first fifty samples. The covariance of the sensor noise for sensor 1 was increased to 1 for samples 50 to 80. The same was done for sensors 2 and 3 for samples 60 to 80 and samples 70 to 80. respectively. The process covariance was changed to .04*.04 for samples 80 to 90 and to .01*.01 from 90 to 100. A sensor bias of 0.25 meters was introduced in sensor 1 from sample 100 to 150. This shows the robustness of the method to unmodeled disturbances. .

Figure 4 shows the fused estimate along with the actual value of the process and an estimate obtained by averaging the values of the three sensors. As can be seen clearly the fused estimate follows the actual process value very well, in spite of unmodeled disturbances and changes in the process.

     



     Figure 4: Fused estimate, the average values from the sensors and the actual
process value

Figure 5 shows the normalized innovations for the first two sensors. Here, a validation gate corresponding to a confidence of 95.9% (innovation should be less than 6) was used for the sensor validation process.

     


     Figure 5: Normalized Innovations for Sensor 1 and 2 used in the validation
process
     


     Figure 6: Probabilities with which sensor readings 1 and 2 were used in the
fusion process

     



     Figure 7: Sensor residue for sensor 1. 

     


     Fig. 8: An example of the methodology applied to data obtained from the
longitudinal sensors during platooning experiments.

In this paper, we have presented a methodology for sensor validation and fusion of multiple sensors used in longitudinal control of the automated vehicles in IVHS. The methodology proposed consists of dividing the operating states of the automated vehicles into three states, building models for each of these, formation of validation gates by using the Kalman filter estimate and then fusing the sensor values by using the PDAF. Several cases have to be considered depending on the operating state. Each case demands its own model. The model selection is supported by a rule-based system. The methodology is illustrated with the help of two examples using both simulated and real data. Uncertain sensor values are fused and validated. The methodology will help to maintain high fidelity of the data obtained from the longitudinal sensors and will improve the reliability and safety of the IVHS system.

5. Acknowledgment

The work was supported by a grant from Caltrans through PATH (Partners in Advanced Transit Highways), grant number MOU 132.

6. References

[1] Varaiya, P., "Smart Vehicles on Smart Roads: Problems of Control", PATH Technical Memorandum 91-5, UC Berkeley, 1991.

[2] Alag, S., Goebel, K., and Agogino A., "A Framework for Intelligent Sensor Validation, Sensor Fusion, and Supervisory Control of Automated Vehicles in IVHS", Proceedings ITS America 1995, Washington, DC., 1995.

[3] Narendran, V. K., "Transition Maneuvers in Intelligent Vehicle Highway Systems", doctoral dissertation University of California, Berkeley, 1994.

[4] Hitchcock, A., "Methods of Analysis or IVHS Safety," PATH Research Report UCB-ITS-PRR-92-14, 1992a.

[5] Bar-Shalom, Y. and T. E. Fortmann, Tracking and Data Association. Boston, MA: Academic, 1988.

[6] Grewal, M. S. and Andrews, A. P., Kalman Filtering: Theory and Practice, Prentice Hall Information and System Sciences Series, New Jersey, 1993.

[7] Thomopoulos, S. C. A., "Sensor Integration and Data Fusion", Journal of Robotic Systems. vol. 7, no. 3, 1990, pp. 337-372.

[8] Agogino, A.M., S. Srinivas and K. Schneider, "Multiple Sensor Expert System for Diagnostic Reasoning, Monitoring, and Control of Mechanical Systems," Mechanical Systems and Signal Processing, Vol. 2(2), 1988, pp. 165-185.

[9] Blackman, S. S., "Association and Fusion of Multiple Sensor Data". in Bar-Shalom, Y. (ed.): Multi-Target Multi-Sensor Tracking. Norwood, MA: Artech House, 1990.

[10] Ayache, N. and Faugeras, O. ,"Building, Registering, and Fusing Noisy Visual Maps", Int. J. Robot. Res. vol. 7, no. 6, 1988.

[11] Alag, S. and A.M. Agogino, "A Methodology for Intelligent Sensor Validation, Fusion and Sensor Fault Detection for Dynamic Systems," # 95-0301-P, Berkeley Expert Systems Lab., UC Berkeley, 1995.

[12] Bar-Shalom, Y., editor. Multitarget-Multisensor Tracking: Advanced Applications. Artech House, Norwood, MA, 1990.

[13] T. E. Fortmann, Y. Bar-Shalom, and M. Scheffe, "Sonar Tracking of Multiple Targets Using Joint Probabilistic Data Association", IEEE Journal of Oceanic Engineering, vol. OE-8, July 1983, pp. 173-184.