Model-based Statistical Tracking and Decision Making for Collision ...

Model-based Statistical Tracking and Decision Making for Collision Avoidance Application Rickard Karlsson Control & Communication Link¨oping University SE-58183 Link¨oping, Sweden E-mail: [email protected]

Jonas Jansson Volvo Car/Control & Communication Active Saftey SE-405 31 G¨oteborg, Sweden E-mail: [email protected]

Fredrik Gustafsson Control & Communication Link¨oping University SE-58183 Link¨oping, Sweden E-mail: [email protected] Abstract A growing research topic within the automotive industry is active safety systems. These systems aim at helping the driver avoid or mitigate the consequences of an accident. In this paper a collision mitigation system that performs late braking is discussed. The brake decision is based on estimates from tracking sensors. We use a Bayesian approach, implementing a Kalman filter and a particle filter to solve the tracking problem. The two filters are compared for different sensor noise distributions and driver models. In particular a bi-modal Gaussian distribution is proposed to model measurement noise. The decision making is based on a hypothesis test, where collision risk is measured in terms of required acceleration to avoid collision. Since the hypothesis test is not analytically solvable a stochastic integration is performed. The system is evaluated using Monte Carlo simulations. We will show that in the non-Gaussian cases a significant improvement can be obtained using the particle filter.

1 Introduction Current trends in automotive industry is to introduce active safety systems that avoid or mitigate collisions. One system with a potential large positive impact on accident statistics is forward collision avoidance systems (FCAS), using sensors such as radar, lidar and cameras to monitor the region in front of the vehicle. A tracking algorithm is used to estimate the state of the objects ahead and a decision algorithm uses the estimated states to determine any action. In Figure 1 a schematic picture over a FCAS is presented. In this paper we will specifically look at a system that per-

Figure 1: Collision avoidance system.

form late braking to reduce the collision speed, which is referred to as collision mitigation by braking system (CMbBS). This type of system is discussed further in [11]. There are several motivations for this kind of system. One is its potential ability to affect rear-end collisions which constitute approximately 30 % [16] of all collisions. Furthermore human factors contribute to approximately 90 % of all traffic accidents [15]. For reasons such as driver acceptance and legal requirements of the system, the tracking and decision algorithms are crucial.

2 Tracking Model For a CMbB system the measurement noise (radar, lidar camera etc.) and the process noise (driver inputs) are not necessarily Gaussian. Thus we need a recursive estimation method that can handle both non-linear and non-Gaussian models. Traditionally, such estimation is handled using EKF as the tracking filter or multiple

filters such as the interacting multiple method (IMM) [5, 3] or Gaussian-sum filters [2].

Tracking model. The general tracking model take the form of a non-linear state space model for the vehicle dynamics and sensor measurements: xt+1 = f (xt ) + wt yt = h(xt ) + et .

(1a) (1b)

For CMbB, the state vector, xt ∈ Rn , contains relative position to the other vehicle of potential collision risk, and relative velocity and possibly also relative acceleration. This may be in the longitudinal direction or both longitudinal and lateral directions. Further, wt denotes the driver inputs from the accelerator, brake and steering wheel. Since these also include actions in the other car, wt has to be considered as un-measurable. The measurement relation (1b) comes from distance and bearing sensors such as radar and lidar. The measurement noise, et , includes clutter, multi-path and multiple reflection points in the vehicle ahead.

State noise model. The classical assumption in target tracking is to assume a Gaussian distribution for wt . Here, we will compare this approach to a nonparametric approach, where recorded data from a study done on data provided by Volvo is used to make histograms of wt , see Figure 2. A further and natural option is to switch between different distributions depending on the driving sitution. In urban traffic one distribution is used, on highway another one, etc. This leads to a mode dependent distribution pw (w; ξ). Here the mode can be determined from the host vehicle speed, it may depend on the state vector, external information from navigation and telematics systems etc. A simple approach is to use one mode when v < 70 km/h and another one for v > 70 km/h.

Measurement noise model. The measurement equation involves range measurements to the vehicle ahead. Similarly to the above, the range error distribution can be modeled. A common model is to assume Gaussian noise. However, we will extend this to a bimodal-Gaussian distribution according to Figure 3. This measurement noise model is motivated by model-

0.16 0.14 0.12 Probability

In this paper we will study a general non-linear and non-Gaussain Bayesian estimation problem. This is in general non-tractable, but using the particle filter method [6] (see Section 4) a recursive solution to the problem is given.

Empirical distribution of vehicle acceleration 0.18

0.1 0.08 0.06 0.04 0.02 0 −6

−4

−2 0 2 Acceleration [m/s ]

2

4

Figure 2: Histogram of acceleration of a vehicle driven in urban traffic for 45 minutes.

ing the vehicle by two point reflectors, which is a simplified way of describing both multi-path propagation and complex reflection geometry. The first Gaussian is due to high reflection in the rear-end of the car, whereas the second one models phenomena such as multi-path propagation and the fact that other parts of the vehicle also reflect radar energy.

3 Statistical Decision Making Statistical decision making requires to estimate the distribution of xt from measurements yt . A general modelbased statistical decision rule for the desicion making criterion g(x) takes the form P r{g(xt ) > 0} > 1 − α,

(2)

where α determines the confidence level. Note that this is a more flexible approach than a rule like g(ˆ xt ) > 0, since in (2) we use the complete a posteriori distribution of the state vector and not only the estimate x ˆt . How successful is the rule (2) in mitigating a collision? That depends of course on the rule itself, but also on how accurately p(x) reflects the true distribution of the relative distance between the vehicles. When KF or EKF is used, an analytical expression for (2) is often hard to compute. Stochastic numerical integration techniques can be used to compute an approximation. Here, one takes a large number of samples x(i) , i = 1, 2, . . . , N from the distribution p(xt |y0 , ..., yt ), and then (i)

P r{g(xt ) > 0} ≈

#g(xt ) > 0 . N

(3)

for a short period of time. Setting v˜ to zero or p˜ large will cause the required host acceleration to equal the tracked object acceleration. This does not always reflect the required longitudinal acceleration in real life. A better longitudinal risk metric than (6), would be to assume that the tracked object will have constant acceleration until the velocity equal zero and then remain stationary. However in our simulations we assume constant velocity, and simulates a scenario where this assumption holds.

1.4 Histogram Gauss sum pdf Normal approx. 1.2

1

0.8

0.6

0.4

For constant velocity the risk metric simplifies further and the hypothesis test will be according to

0.2

0 −5

−4

−3

−2

−1

0

1

2

3

4

5

P r{−

v˜2 < ath } > 1 − α, 2˜ p

(7)

Figure 3: Bimodal measurement noise (sum of two Gaussian distributions).

where ath is the acceleration threshold for when we consider an accident to be imminent.

This procedure is much simpler when the particle filter (i) is used, since the samples xt exist internally in the algorithm. One can simply count the number of particles for which the inequality is satisfied. This fact decreases the gap in computational burden between the Kalman and particle filter approaches. One natural and representative decision rule (2) will be suggested below. In this paper we will study a CMbB system, which performs maximum braking when a collision is becoming imminent. By imminent we mean that a maneuver that is close to or exceeds the vehicles handling limits is required to avoid collision. Since a vehicle’s handling properties is normally measured in terms of acceleration, one appropriate risk metric for a CMbB system is to calculate the acceleration required to avoid collision. To simplify the analysis we will only consider longitudinal motion. However, the reasoning can be extended to higher dimensions. Under the assumption that the tracked vehicle continues with constant acceleration the relative required acceleration is given by the solution to 0 = p˜ + v˜t +

a ˜t2 , 2

(4)

where t is the time to accelerate to zero relative velocity v˜ t=− . a ˜

(5)

The host required acceleration will thus be given by ahost = aobj − a ˜ = aobj −

v˜2 . 2˜ p

(6)

We note that the expression above will not by itself give a good criterion for a real CMbB system. The assumption of constant acceleration in general will only hold

4 Particle Filtering The general Bayesian problem is non-tractable and not analytically solvable for a general system. Therefore many numerical approximate methods have been studied. Due to improved computer performance, simulation based methods are now studied and applied more frequently. In [10, 9], conditional mean and covariance were calculated using importance sampling for recursive Bayesian estimation. However, the paper of [8] with sequential Monte Carlo simulation techniques for solving the optimal estimation problem started the research in particle filtering techniques. It was the introduction of the crucial resampling step introduced in [14, 8] that solved previous divergence problems. In this section the presentation of the sequential Monte Carlo method, or particle filter theory is according to theory and notations described thoroughly in [8, 4, 13, 6]. The particle filter method provides an approximative Bayesian solution to discrete-time recursive problem by updating an approximative description of the posterior filtering density, p(xt |Yt ). Let Yt = {yi }ti=1 be the set of observations until present time. The particle filter approximates p(xt |Yt ) by a set of N particles (samples) (i) {xt }N i=1 , where each particle has an assigned relative (i) weight, wt , such that all weights sum to unity. The location and weight of each particle reflect the value of the density in the region of the state space. The particle filter updates the particle location and the corresponding weights recursively with each new observation. The non-linear prediction density p(xt |Yt−1 ) and filtering density p(xt |Yt ) for the Bayesian interference, [12], are

given by

5 Simulations

p(xt+1 |Yt ) =

Z

p(xt+1 |xt )p(xt |Yt )dxt

(8a)

Rn

p(xt |Yt ) =

p(yt |xt )p(xt |Yt−1 ) . p(yt |Yt−1 )

(8b)

The likelihood p(yt |xt ) is calculated from (1b) using the known measurement noise density pet . Often we assume additive noise, yt = h(xt ) + et , yielding wt = p(yt |xt ) = pet (yt − h(xt )),

(9)

for the method discussed below in Alg 1. The main idea is to approximate p(xt |Yt−1 ) with p(xt |Yt−1 ) ≈

N 1 X (i) δ(xt − xt ), N i=1

(10)

To simplify analysis of the system performance for different noise distributions, we consider a single scenario. The scenario to be studied is one where the target object is not moving and the vehicle with the CMbB system is approaching according to Figure 1 with v˜ = 10 m/s and p˜ = 30 m. This type of scenario corresponds to approximately 30 % of all rear-end collisions [1]. Consider the state vector with relative position p˜ and velocity v˜ where the sensor measures the relative position.   p˜ . (12) xt = v˜ The tracking model with sample time T is then xt+1 = Axt + Bwt

(13a)

yt = h(xt ) + et ,

(13b)

where δ is the delta-Dirac function. Using the importance weights the posterior can be written as with p(xt |Yt ) ≈

N X

(i)

(i)

w ˜t δ(xt − xt ),

(11)

i=1

(i)

where w ˜t are the normalized importance weights. The weights are resampled to avoid filter divergence as proposed in [8], where the filter is referred to as sampling importance resampling (SIR). The algorithm is given in Alg 1. Sampling Importance Resampling (SIR) 1. Set t = 0 and generate N samples from the initial distribution p(x0 ). (i)

(i) {x0 }N i=1

(i)

2. Compute the weights wt = p(yt |xt ) and (i) (i) PN (j) normalize, i.e., w ˜t = wt / j=1 wt , i = 1, . . . , N . (i⋆)

3. Generate a new set {xt }N i=1 by resampling (i) with replacement N times from {xt }N i=1 , with (j) (i⋆) (j) probability w ˜t = P r{xt = xt }. (i)

4. Predict (simulate) new particles, i.e., xt+1 = (i⋆) (i) f (xt , wt ), i = 1, . . . , N using different noise realizations for the particles. 5. Increase t and iterate to step 2. Alg. 1. Sampling Importance Resampling. Sometimes the resampling step is omitted and just imposed when needed to avoid divergence in the filter as in the sequential importance sampling (SIS), [7], where the weights are updated recursively.

A=



1 0

T 1



,B =



 T 2 /2 , h(xt ) = p˜ T

(14)

The process noise, w, and measurement noise, e, are assumed to be independent. We consider two models for the measurement sensor. The traditional assumption is to assume additive Gaussian measurement noise. We also use the more realistic bimodal Gaussian pdf (Figure 3) according to et ∈ p1 N(µ1 , σ12 ) + p2 N(µ2 , σ22 ),

(15)

where N(µ, σ 2 ) denotes a Gaussian density with mean (µ) and covariance (σ 2 ). A sample of et belongs P to one of the distributions, with probability pi , and i pi = 1. Here p1 = 0.7 and p2 = 0.3 since the rear-end of the vehicle is assumed to have a larger radar cross section. Since we use a deterministic driver maneuver (zero acceleration), it is clear that tracking performance can be improved in our specific scenario by setting the process noise to something small. This does not however give good tracking in a general traffic situation. We use the empirical distribution according to Figure 2 to model driver behavior. For the KF a Gaussian noise with the same covariance as the empirical distribution is used. We perform 5000 Monte Carlo (MC) simulations for SIR and EKF according to Table 1. The decision is based on a hypothesis test on the expected required acceleration according to (7). We have chosen the acceleration threshold to ath = −8 m/s and the confidence level to α = 0.05. For the SIR approach the hypothesis is evaluated for each particle. For the EKF the assumption is that all noise is Gaussian. Hence, when the bimodal measurement

Case I II III IV

Filter SIR SIR KF KF

Meas. pdf pet N(0, 0.52 ) N(0, 0.32 ) + N(1.5, 1) N(0, 0.52 ) N(0, 0.32 ) + N(1.5, 1)

RMSE(˜ p) [m] 0.26 0.28 0.29 0.68

RMSE(˜ v ) [m/s] 0.47 0.46 0.46 0.84

Mean -9.59 -9.45 -9.59 -10.60

Std. 0.88 0.80 0.90 1.79

Min -12.96 -14.20 -13.62 -27.28

Max -6.06 -4.86 -7.36 -6.42

Table 2: Different simulation cases.

SIMULATION OUTLINE

Intervention Acceleration: SIR, pe=bimodal, pw=empirical

1. Simulate scenario. The system is simulated with measurement noise (Gaussian or bimodalGaussian).

3. CMbB Decision rule. Decision making according to the hypothesis test (7).

0.05 Probability of intervention

2. State estimate. The Kalman or the SIRmethod is used to estimate position, velocity and acceleration. The process noise is assumed to be from the empirical distribution.

0.06

4. Evaluation The result of the decision is evaluated by comparison with the true value of the required acceleration.

0.04

Intervention boundary

0.03

0.02

0.01

0 −20

−18

−16

Table 1: Simulation outline.

−14 −12 −10 −8 2 Required acceleration [m/s ]

−6

−4

(a) Case II

Intervention Acceleration: KF, pe=bimodal, pw=empirical 0.06

0.05 Probability of intervention

noise is used an equivalent one-peak Gaussian is calculated as shown in Figure 3. The analytical expression for the hypothesis test is not solvable. Therefore, we use the estimated mean and covariance and the probability is calculated with them as input to a stochastic integration. In the particle filter we use N = 1000 particles. The different simulation cases are summarized in Table 2 together with tracking and CMbB system performance. The case with Gaussian distributed measurement is well described by the Kalman filter. Hence case I and III yield similar RMSE performance, whereas the bimodal Gaussian (case II and IV) yield a substantial performance increase for the SIR-method. Performance of the CMbB system for case II and IV is also presented in the histograms in Figure 4 respectively. For the case I and III tracking and CMbB system performance were almost similar, where the KF is not completely optimal for the empirical process noise. Comparing case II and case IV, the SIR method outperform the KF, which suffer from the mono-modal Gaussian approximation of the bimodal measurement noise. The CMbB system performance was affected both in terms of mean and standard deviation of the true required acceleration at the intervention (Table 2). For case I, II and III an intervention at the mean true required acceleration (-9.6 m/s2 ) corresponds to a collision speed of 16.9 km/h whereas for case IV braking at (-10.6 m/s2 ) corresponds to a collision speed of 20.7

0.04

Intervention boundary

0.03

0.02

0.01

0 −20

−18

−16

−14 −12 −10 −8 2 Required acceleration [m/s ]

−6

−4

(b) Case IV

2

Figure 4: Pdf of required acceleration (− v2˜p˜ ) when the CMbB system intervenes. The results are from 5000 MC simulations.

km/h. Thus there is a significant difference in the average speed reduction depending on the choice of tracking method for the bimodal measurement distribution. Histograms of the collision speed for case II and IV are presented in Figure 5.

KF stochastic integration was used. This makes the computational complexity of the KF approach similar to that of the SIR filter. In the simulation study the simulations for both the SIR and KF approach were run faster than real-time on an ordinary desk-top computer.

Collision speed: SIR, pe=bimodal, pw=empirical

Probability

0.06 0.04 0.02 0 0

2 4 6 8 10 Collision speed: KF, p =bimodal, p =empirical

12

2

12

e

w

Probability

0.04 0.03 0.02 0.01 0 0

4

6 8 Collision speed [m/s]

10

Figure 5: Pdf of the collision speed for case II and IV. The initial speed is 10 m/s.

To calculate the collision speed the brake system characteristics need to be known. A simple yet accurate description can be obtained using a first order system for the acceleration. We have used a brake-system risetime of 300 milliseconds and a maximum deceleration of 9.8 m/s2 to derive the collision speeds presented above. In terms of worst case performance the SIR filter performed braking a little bit earlier than the KF. Braking at -4.86 m/s2 compared to the nominal -8 m/s2 is equivalent to braking 0.4 seconds too early. Looking at cases where braking is initiated to late we find that the KF performed worse than the SIR filter. In the simulation study the SIR (with 1000 particles) and KF (using 1000 realizations for the numerical integration) scenarios were run faster than real-time on a Sun Blade 100 machine using a MATLAB implementation.

6 Conclusions We have implemented a decision rule in a CMbB system for late braking using a hypothesis test based on estimates of the relative geometry. Both a Kalman filter and a particle filter was evaluated for different noise assumptions. For non-Gaussian distributions such as the bimodal-Gaussian proposed for the measurement noise, the SIR method increases performance. The mean difference in collision speed for the two filters (under assumption of bimodal measurement noise) was 3.1 km/h. For the SIR filter a hypothesis test is a suitable decision rule. The hypothesis is easily solved using the particle-cloud description of the pdf. For the

References [1] Automotive collision avoidance system field operational test. Technical Report DOT HS 809 453, NHTSA, May 2002. [2] B. Anderson and J. Moore. Optimal Filtering. Prentice Hall, Englewood Cliffs, NJ, 1979. [3] Y. Bar-Shalom and X. Li. Estimation and Tracking: Principles, Techniques, and Software. Artech House, 1993. [4] N. Bergman. Recursive Bayesian Estimation: Navigation and Tracking Applications. PhD thesis, Link¨ oping University, 1999. Dissertations No. 579. [5] H. Blom. An efficient filter for abruptly changing systems. In Proc. of the 23rd IEEE Conference on Decision and Control, pages 656–658, 1984. [6] A. Doucet, N. de Freitas, and N. Gordon, editors. Sequential Monte Carlo Methods in Practice. Springer Verlag, 2001. [7] A. Doucet, S. Godsill, and C. Andrieu. On Sequential Monte Carlo Sampling Methods for Bayesian Filtering. Statistics and Computing, 10(3):197–208, 2000. [8] N. Gordon, D. Salmond, and A. Smith. A novel approach to nonlinear/non-Gaussian Bayesian state estimation. In IEE Proceedings on Radar and Signal Processing, volume 140, pages 107–113, 1993. [9] J. Handschin. Monte Carlo techniques for prediction and filtering of non-linear stochastic processes. Automatica, 6:555–563, 1970. [10] J. Handschin and D. Mayne. Monte Carlo techniques to estimate the conditional expectation in multi-stage nonlinear filtering. International journal of control, 9:547–559, 1969. [11] J. Jansson. Tracking and Decision Making for Automotive Collision Avoidance. Licentiate Thesis no. 965, Department of Electrical Engineering, Link¨ oping University, Sweden, September 2002. [12] A. Jazwinski. Stochastic processes and filtering theory, volume 64 of Mathematics in Science and Engineering. Academic Press, 1970. [13] R. Karlsson. Simulation Based Methods for Target Tracking. Licentiate Thesis no. 930, Department of Electrical Engineering, Link¨ oping University, Sweden, Feb 2002. [14] A. Smith and A. Gelfand. Bayesian statistics without tears: A sampling-resampling perspective. The American Statistican, 46(2):84–88, 1992. [15] J. Treat, N. McDonald, D. Shinar, R. Hume, R. Mayer, R. Stansifer, and N. Castellan. Tri-level study of the causes of traffic accidents. Technical report, NHTSA, 1977. [16] B. Zhu. Potential effects on accidents from forward collision warning/avoidance system. Master Thesis LITH-ITN-EX-150-SE, ITN, Link¨ oping University, S-581 83 Link¨ oping, Sweden, 2001.