vehicle collision risk prediction using a dynamic bayesian ...1431653/fulltext02.pdf · jonas...

IN DEGREE PROJECT MATHEMATICS,SECOND CYCLE, 30 CREDITS

, STOCKHOLM SWEDEN 2020

Vehicle Collision Risk Prediction Using a Dynamic Bayesian Network

JONAS LINDBERG

ISAK WOLFERT KÄLLMAN

KTH ROYAL INSTITUTE OF TECHNOLOGYSCHOOL OF ENGINEERING SCIENCES

Vehicle Collision Risk Prediction Using a Dynamic Bayesian Network

JONAS LINDBERG

ISAK WOLFERT KÄLLMAN

Degree Projects in Mathematical Statistics (30 ECTS credits)

Master's Programme in Applied and Computational Mathematics

KTH Royal Institute of Technology year 2020

Supervisors at Scania CV AB: Laura Dal Col, Christoffer Norén Supervisor at KTH: Jimmy Olsson

Examiner at KTH: Jimmy Olsson

TRITA-SCI-GRU 2020:070

MAT-E 2020:033

Royal Institute of Technology School of Engineering Sciences KTH SCI SE-100 44 Stockholm, Sweden URL: www.kth.se/sci

Abstract

This thesis tackles the problem of predicting the collision risk for vehiclesdriving in complex traffic scenes for a few seconds into the future. Themethod is based on previous research using dynamic Bayesian networksto represent the state of the system.

Common risk prediction methods are often categorized into threedifferent groups depending on their abstraction level. The most complexof these are interaction-aware models which take driver interactions intoaccount. These models often suffer from high computational complexitywhich is a key limitation in practical use. The model studied in this worktakes interactions between drivers into account by considering driverintentions and the traffic rules in the scene.

The state of the traffic scene used in the model contains the physicalstate of vehicles, the intentions of drivers and the expected behaviourof drivers according to the traffic rules. To allow for real-time risk as-sessment, an approximate inference of the state given the noisy sensormeasurements is done using sequential importance resampling. Twodifferent measures of risk are studied. The first is based on driver in-tentions not matching the expected maneuver, which in turn could leadto a dangerous situation. The second measure is based on a trajectoryprediction step and uses the two measures time to collision (TTC) andtime to critical collision probability (TTCCP).

The implemented model can be applied in complex traffic scenarioswith numerous participants. In this work, we focus on intersection androundabout scenarios. The model is tested on simulated and real datafrom these scenarios. In these qualitative tests, the model was able tocorrectly identify collisions a few seconds before they occur and is alsoable to avoid false positives by detecting the vehicles that will give way.

Keywords: collision risk prediction, dynamic Bayesian network, sequentialimportance resampling, autonomous vehicles, ADAS, intelligent driver model

i

Forutsagelse av kollisionsrisk for fordonmed ett dynamiskt Bayesianskt natverk

Sammanfattning

Detta arbete behandlar problemet att forutsaga kollisionsrisken for for-don som kor i komplexa trafikscenarier for nagra sekunder i framti-den. Metoden ar baserad pa tidigare forskning dar dynamiska Bayesian-ska natverk anvands for att representera systemets tillstand. Vanligariskprognosmetoder kategoriseras ofta i tre olika grupper beroende paderas abstraktionsniva. De mest komplexa av dessa ar interaktionsmed-vetna modeller som tar hansyn till forarnas interaktioner. Dessa mod-eller lider ofta av hog berakningskomplexitet, vilket ar en svar begransningnar det kommer till praktisk anvandning. Modellen som studeras idetta arbete tar hansyn till interaktioner mellan forare genom att beaktaforarnas avsikter och trafikreglerna i scenen.

Tillstandet i trafikscenen som anvands i modellen innehaller for-donets fysiska tillstand, forarnas avsikter och forarnas forvantade be-teende enligt trafikreglerna. For att mojliggora riskbedomning i realtidgors en approximativ inferens av tillstandet givet den brusiga sensor-datan med hjalp av sekventiell vagd simulering. Tva olika matt pa riskstuderas. Det forsta ar baserat pa forarnas avsikter, narmare bestamtatt ta reda pa om de inte overensstammer med den forvantade manovern,vilket da skulle kunna leda till en farlig situation. Det andra riskmattetar baserat pa ett prediktionssteg som anvander sig av time to collision(TTC) och time to critical collision probability (TTCCP).

Den implementerade modellen kan tillampas i komplexa trafiksce-narier med manga fordon. I detta arbete fokuserar vi pa scerarier ikorsningar och rondeller. Modellen testas pa simulerad och verklig datafran dessa scenarier. I dessa kvalitativa tester kunde modellen korrektidentifiera kollisioner nagra fa sekunder innan de intraffade. Den kundeocksa undvika falsklarm genom att lista ut vilka fordon som kommer attlamna foretrade.

Nyckelord: forutsagelse av kollisionsrisk, dynamiskt Bayesianskt natverk,sekventiell vagd simulering, autonoma fordon, ADAS, intelligent forarmodell

ii

Acknowledgements

This master thesis was carried out at Scania in Sodertalje as a part ofour degree program in mathematical statistics. We would therefore liketo express our gratitude to Laura Dal Col and Christoffer Noren, oursupervisors at Scania, who continuously supported us throughout thiswork and provided us with regular feedback and ideas. We would alsolike to thank the industrial PhD students and the other thesis workersat Scania for discussing ideas and keeping everyone positive during thecorona-quarantine.

Finally, we thank Jimmy Olsson, our supervisor at KTH for the guid-ance on scientific quality and for the input regarding some theoreticalaspects of the work.

Isak Kallman & Jonas LindbergStockholm, May 2020

iii

Contents

Acknowledgements iii

List of Figures vi

List of Tables x

Acronyms xi

1 Introduction 11.1 Background . . . . . . . . . . . . . . . . . . . . . . . . . 11.2 Related Work . . . . . . . . . . . . . . . . . . . . . . . . 21.3 About this Thesis . . . . . . . . . . . . . . . . . . . . . . 4

1.3.1 Aims and Objectives . . . . . . . . . . . . . . . . 41.3.2 Outline of the Report . . . . . . . . . . . . . . . . 5

2 Theoretical Framework 62.1 Markov Models and Hidden Markov Models . . . . . . . 62.2 Dynamic Bayesian Networks . . . . . . . . . . . . . . . . 82.3 Filtering . . . . . . . . . . . . . . . . . . . . . . . . . . . 92.4 Particle Filter . . . . . . . . . . . . . . . . . . . . . . . . 9

2.4.1 Sequential Importance Sampling . . . . . . . . . . 102.4.2 Weight Degeneracy . . . . . . . . . . . . . . . . . 122.4.3 Sequential Importance Resampling . . . . . . . . 122.4.4 Connection to HMMs and DBNs . . . . . . . . . 13

3 Method 143.1 Overview . . . . . . . . . . . . . . . . . . . . . . . . . . . 143.2 Variables . . . . . . . . . . . . . . . . . . . . . . . . . . . 163.3 Variable Dependencies . . . . . . . . . . . . . . . . . . . 193.4 A Modified Intelligent Driver Model . . . . . . . . . . . . 293.5 Speed Profiles . . . . . . . . . . . . . . . . . . . . . . . . 303.6 Applying SIR . . . . . . . . . . . . . . . . . . . . . . . . 313.7 Estimating Risk Using the DBN . . . . . . . . . . . . . . 333.8 Collision Zones . . . . . . . . . . . . . . . . . . . . . . . 343.9 Making use of Collision Zones . . . . . . . . . . . . . . . 383.10 TTC and TTCCP . . . . . . . . . . . . . . . . . . . . . . 403.11 Pre-Processing . . . . . . . . . . . . . . . . . . . . . . . 41

iv

CONTENTS

3.12 Model Summary . . . . . . . . . . . . . . . . . . . . . . 42

4 Case Studies 454.1 4-way Intersection . . . . . . . . . . . . . . . . . . . . . . 454.2 Roundabout . . . . . . . . . . . . . . . . . . . . . . . . . 49

5 Main Results 535.1 Simulations . . . . . . . . . . . . . . . . . . . . . . . . . 53

5.1.1 Scenario 1 . . . . . . . . . . . . . . . . . . . . . . 565.1.2 Scenario 2 . . . . . . . . . . . . . . . . . . . . . . 585.1.3 Scenario 3 . . . . . . . . . . . . . . . . . . . . . . 605.1.4 Scenario 4 . . . . . . . . . . . . . . . . . . . . . . 625.1.5 Scenario 5 . . . . . . . . . . . . . . . . . . . . . . 645.1.6 Scenario 6 . . . . . . . . . . . . . . . . . . . . . . 665.1.7 Scenario 7 . . . . . . . . . . . . . . . . . . . . . . 685.1.8 Performance . . . . . . . . . . . . . . . . . . . . . 70

5.2 Test on Log Data . . . . . . . . . . . . . . . . . . . . . . 705.2.1 Scenario 8 . . . . . . . . . . . . . . . . . . . . . . 745.2.2 Scenario 9 . . . . . . . . . . . . . . . . . . . . . . 765.2.3 Scenario 10 . . . . . . . . . . . . . . . . . . . . . 78

5.3 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . 80

6 Conclusion and Future Work 836.1 Main Findings . . . . . . . . . . . . . . . . . . . . . . . . 836.2 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . 84

Bibliography 84

v

List of Figures

2.1 Schematic representation of a two-layer hidden Markovmodel with unobservable states Xt and observable statesYt. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

3.1 Schematic representation of the DBN. Black arrows indi-cate a dependence on vehicle k. Red arrows indicate adependence on all vehicles in the scene. . . . . . . . . . . 16

3.2 Example intersection showing courses coming from southand east. The horizontal road is a primary road and ve-hicles on the other road are thus expected to yield at theintersection when other vehicles are present. . . . . . . . 19

3.3 Example roundabout showing two courses that intersectin two different regions. Vehicles should only yield forvehicles on the other course when they approach the firstintersection with that course, that is when entering theroundabout. If a vehicle has already entered the round-about, then it should not yield to other vehicles. . . . . . 23

3.4 A vehicle position, speed and heading update examplefor a vehicle within a particle. The vehicle state at time t(red) is updated to the new state at time t+1 (green). Thegreen updated state illustrates the mean of the Gaussiandistributions for the new position, speed and heading aspresented above. . . . . . . . . . . . . . . . . . . . . . . 27

3.5 The local coordinate frame of each actor. The vehicleshave a length L, width W and an offset Lrear for the origin. 35

3.6 A visualization for the separating axis theorem in whichthe two vehicles do not collide. As we see in this figurethere is (more than) one axis where the projections do notoverlap leading to the conclusion that the vehicles are notcolliding. . . . . . . . . . . . . . . . . . . . . . . . . . . . 36

vi

LIST OF FIGURES

3.7 A visualization for the separating axis theorem in whichthe two vehicles collide. As we see in this figure there isno axis where the projections do not overlap leading tothe conclusion that the vehicles are in fact colliding. . . . 37

3.8 An overview of the complete algorithm. . . . . . . . . . . 433.9 An overview of the particle propagation procedure. . . . 443.10 An overview of the pre-processing procedure. . . . . . . . 44

4.1 The speed profiles used in the 4-way intersection scene forthe two different longitudinal intentions as a function ofdistance to the stop line. A positive distance to the stopline indicates a position that is past the stop line in thedirection of the course. . . . . . . . . . . . . . . . . . . . 46

4.2 The pairs of courses used in the examples shown in Fig-ure 4.3. Figure 4.2a shows the first pair and Figure 4.2bshows the second pair. The blue courses are referred toas the ego courses and the red courses are referred to asthe other courses. The normalized arc lengths are shownfor some positions along each trajectory. . . . . . . . . . 47

4.3 Two examples of collision zones and critical collision zones.The axes represent arc lengths while the color white rep-resents the collision zone. Black indicates that there isno collision and the critical collision zone is shown in red.Plots (a) and (b) show the collision zone and critical col-lision zone for the intention pair Approaching from southand continuing straight and Approaching from west andcontinuing straight, shown in Figure 4.2a. Plots (c) and(d) show the collision zone and critical collision zone forthe intention pair Approaching from south and turningright and Approaching from west and continuing straight,shown in Figure 4.2b. . . . . . . . . . . . . . . . . . . . . 48

4.4 The roundabout at the test track. Four different courseswere defined. . . . . . . . . . . . . . . . . . . . . . . . . 49

4.5 The speed profiles used in the roundabout scene for thetwo different longitudinal intentions as a function of dis-tance to the stop line. A positive distance to the stop lineindicates a position that is past the stop line in the direc-tion of the course. The top graphs show the profiles forthe courses approaching from southeast while the bottomgraphs show the profiles for the courses approaching fromnorthwest. . . . . . . . . . . . . . . . . . . . . . . . . . . 50

vii

LIST OF FIGURES

4.6 The pair of courses used in the examples shown in Fig-ure 4.7. Figure 4.6a shows the first pair and Figure 4.6bshows the second pair. The blue courses are referred toas the ego courses and the red courses are referred to asthe other courses. The normalized arc lengths are shownfor some positions along each trajectory. . . . . . . . . . 51

4.7 Two examples of collision zones and critical collision zones.The axes represent arc lengths while the color white rep-resents the collision zone. Black indicates that there isno collision and the critical collision zone is shown in red.Plots (a) and (b) show the collision zone and critical col-lision zone for the intention pair Approaching from south-east and continuing straight and Approaching from north-west and making a U-turn, shown in Figure 4.6a. Plots(c) and (d) show the collision zone and critical collisionzone for the intention pair Approaching from southeastand making a U-turn and Approaching from northwestand making a U-turn, shown in Figure 4.6b. . . . . . . . 52

5.1 Scenario 1. The first row show the position of all vehiclesat times t1, t2 and t3. The estimated intended coursesis shown in the second row. The estimated intention tostop and expectation to stop is shown in the third rowtogether with the risk induced on the scene by each ve-hicle. For these two rows the first column shows the bluevehicle (ego-vehicle) and the second column shows the redvehicle. The TTCCP and the collision probability for theego-vehicle is shown in the fourth row. . . . . . . . . . . 57

5.2 Scenario 2. The figure consists of the same kind of plotsas Figure 5.1. . . . . . . . . . . . . . . . . . . . . . . . . 59


5.4 Scenario 4. The figure consists of the same kind of plots asFigure 5.1. For the two middle rows, where the estimatedexpected behaviour and the risk measure R(t) is plotted,the first column shows the blue vehicle (ego-vehicle), thesecond row shows the red vehicle and the third row showsthe yellow vehicle. . . . . . . . . . . . . . . . . . . . . . . 63



viii

LIST OF FIGURES

5.7 Scenario 7. The figure consists of the same kind of plots asFigure 5.1. For the two middle rows, where the estimatedexpected behaviour and the risk measure R(t) is plotted,the first column shows the blue vehicle (ego-vehicle), thesecond row shows the red vehicle and the third row showsthe yellow vehicle. . . . . . . . . . . . . . . . . . . . . . . 69

5.8 Two of the vehicles used at the test track. The left pictureshows the bus Klasse and the right picture shows the truckCopper. Klara, which is not shown here, is a red bus verysimilar to Klasse. . . . . . . . . . . . . . . . . . . . . . . 71

5.9 The speed profiles found from the log data gathered. Apositive distance to the stop line indicates a position thatis past the stop line in the direction of the course. Thetop graphs show the profiles for the courses approachingfrom southeast while the bottom graphs show the profilesfor the courses approaching from northwest. . . . . . . . 72

5.10 Scenario 8. The figure consists of the same kind of plots asFigure 5.1. In the overhead view, the ego-vehicle Klasseis shown in blue, Klara is shown in red and Copper isshown in yellow. . . . . . . . . . . . . . . . . . . . . . . . 75

5.11 Scenario 9. The figure consists of the same kind of plotsas Figure 5.1. The ego-vehicle Klasse is shown in blue,Klara is shown in red. . . . . . . . . . . . . . . . . . . . 77

5.12 Scenario 10. The figure consists of the same kind of plotsas Figure 5.1. The ego-vehicle Klasse is shown in blue,Klara is shown in red. . . . . . . . . . . . . . . . . . . . 79

ix

List of Tables

3.1 A summary of all variables used in the model. . . . . . . 183.2 The parameterization for traffic rule compliance before

the stop line given past intention and current expectation. 253.3 The parameterization for intention compliance after the

stop line given past intention. . . . . . . . . . . . . . . . 25

5.1 The parameter values used when running the algorithmon the simulated scenarios. . . . . . . . . . . . . . . . . . 55

5.2 Maximum number of particles for real-time filtering in thepresented scenarios. . . . . . . . . . . . . . . . . . . . . . 70

5.3 The parameter values used when running the algorithmon the real data from the test track. . . . . . . . . . . . . 73

x

Acronyms

ADAS Advanced Driving Assistance System.

CCP Critical Collision Probability.

DBN Dynamic Bayesian Network.

HMM Hidden Markov Model.

IDM Intelligent Driver Model.

ROW Right of Way.

SIR Sequential Importance Resampling.

SIS Sequential Importance Sampling.

TG Time Gap.

TOCCZ Time Out of Critical Collision Zone.

TTC Time to Collision.

TTCCP Time to Critical Collision Probability.

TTCCZ Time to Critical Collision Zone.

UTM Universal Transverse Mercator.

xi

Chapter 1

Introduction

1.1 Background

There are many different vehicle safety technologies in the market in-cluding seat belts, airbags, various vehicle reinforcements, electronicbraking systems and so on. The next step in vehicle safety could beto introduce an electronic control system intended to prevent accidents.These systems, called advanced driving-assistance systems and abbrevi-ated ADAS, aim to assist the driver to increase safety and convenience.If such a system can assess the collision risk for the next few seconds itcan act to reduce the damage if the risk is to high.

For autonomous driving systems, where safety is a requirement, itis of great importance that dangerous situations are detected so thatcollisions can be avoided. The autonomous vehicles must be able totake well grounded decisions even in complex traffic scenes, such as anintersection or a roundabout. The decisions should be based on somemeasure of risk in order to plan a safe route efficiently.

Currently, there are many different approaches to how the collisionrisk can be modeled. Evaluating the collision risk usually consist oftwo steps, the first being a prediction or intention estimation step andthen second the actual risk assessment. When only considering the nearfuture the first task is rather straight forward since the physical stateof the environment (position, velocity and acceleration of other objects)provides sufficient knowledge for prediction. However, for a longer timehorizon the task is more complex since the intentions of drivers must beconsidered. Key ingredients for autonomous driving are therefore builtupon statistical models that aim to predict the future of a traffic scene.

1

CHAPTER 1. INTRODUCTION

This is done by inferring not only the current physical state of othervehicles but the current intentions of drivers as well.

Data about road geometry, other vehicles etc. is usually gatheredusing various sensors located on the autonomous vehicle. This datatogether with a priori knowledge of driving behaviour can be used toestimate the future state of the system to work as a foundation in riskestimation.

Scania has already started to test autonomous vehicles in pilot projectsfor use in the mining industry. These settings are far simpler than gen-eral traffic situations because of the confined and controlled environ-ment. The reason why autonomous vehicles is an interesting researchtopic to companies is due to the fact that a large portion of the costfor driver-operated vehicles is associated with just that, the driver. An-other area where autonomous driving is a hot topic for the future ispublic transport where both savings and improved safety are key ar-guments. To eventually get there requires tons of research in order todevelop fully autonomous vehicles that meet all safety standards andcost efficiency requirements.

1.2 Related Work

There have been multiple different approaches to the risk assessment andrisk prediction problems. In the survey by Lefevre et al. [14] the mostcommon approaches are summarized and described. For the predictionstep, the authors state that there are three main categories of meth-ods used. The first category is physics-based models where one modelsthe environment dynamics using simple kinematic models. Examples ofsuch could be constant velocity or constant acceleration models. Themain advantage of this kind of approach is the simplicity, interpretabil-ity and that these models are not very computationally demanding. Thenext category is maneuver-based models where it is postulated that alldrivers are performing some certain maneuver at any point in time.The admissible maneuvers are often limited to some smaller set con-taining actions such as turn left, continue straight, make a lane changeetc. These models are more complex than the physics-based ones andconsequently allow for more accurate prediction, even for longer timehorizons. One severe limitation of the models in the first two categoriesis the common assumption of independence between the vehicles, whichin practice does not hold. More advanced strategies are found in thelast category known as interaction-aware motion models which all try totake into account that the vehicles will have an effect on each other to

2


some degree. Such models seem very reasonable in its essence, howeveras the number of agents in a traffic scene increases the models of thistype become increasingly more advanced. These kinds of models oftenyield better results than other motion models but because of their com-plexity some parts might be computationally expensive and thereforenot feasible in real-time risk assessment applications.

The authors in [12] present a maneuver based model used to predictthe collision risk in a highway scenario. The maneuvers were limitedto some predefined behaviours, for example lane changes or overtakesand the available behaviours at any given point were determined by theenvironment and the road geometry. A measure of collision risk was in-troduced through the sampling of Gaussian processes and by identifyingcolliding paths. This is an example of trajectory prediction which is alsoa common feature among many approaches for inferring collision risk.

Since the state space and action space is in general very large ina typical traffic scene with multiple actors, one often has to limit thenumber of admissible behaviours. Some recent approaches to counteractthe problem of large action spaces consider machine learning and neuralnetworks to estimate the driver intentions [8], [9].

There are fewer examples of interaction-aware models in literature.Most of these use Dynamic Bayesian Networks [7], [13], [18], which turnsout to be a natural tool for modelling traffic scenes. Other approacheshave also been used, for instance models based on prototype trajectories[11].

In predicting the future states of other vehicles, most models in themaneuver-based and interaction-aware categories often rely on identi-fying the intentions of drivers. The articles [13] and [15] present aninteraction-aware model by introducing the concept of expected be-haviours. The expected behaviour is similar to driver intention butinstead represents what the driver is expected to do based on trafficrules and on the events unfolding in the traffic scene. By introducingtraffic rules and making the intentions of drivers biased towards fol-lowing these rules, interactions between vehicles are incorporated. Animportant aspect of this model is that the actual intention of the drivermay differ from the expected behaviour and could in turn lead to a riskyscenario. In the articles they specifically estimate the probability of eachdriver violating the traffic rules in a way that might result in a collision.In their case, this equated to vehicles not yielding in an intersectionwhen they were expected to. The estimated probabilities of violatingthe traffic rules were then used to form an overall measure of risk in thescenario.

3


Another popular measure used to infer collision risk is the TTC [2],[3], [4] which is defined as the time remaining until the vehicle of interestcollides with another vehicle. This measure has also been generalizedinto the time to critical collision probability (TTCCP) in [17] and wasdefined as the time until the total probability of collision with any othervehicle exceeds some threshold. These two measures enable risk predic-tion for an ego-vehicle, the vehicle of interest that we are controlling,either in person or autonomously.

1.3 About this Thesis

In this section we shortly present the aims of the project, the researchmethod and the outline of the report.

1.3.1 Aims and Objectives

The aim of this project at Scania is to investigate and implement amodel so that the collision risk in a traffic environment with multipleparticipants can be predicted for a few seconds into the future. The workwill consist of two parts. The first part will be to implement the modelpresented in [13] and [15]. The second part will consist of expandingupon this model to enable usage in roundabout scenarios. Since thework will consider applications in autonomous driving, the model mustbe able to predict the collision risk for an autonomous vehicle specifically.The risk measure introduced in [13] and [15] only assesses the overall riskin a scene and can not be used to predict the risk for a specific vehicle.In this work we therefore also introduce another risk measure based ontrajectory prediction so that the collision risk can be predicted for aspecific vehicle.

We will then test the model in two specific scenes, one four-wayintersection and one roundabout, and investigate how the model assessesthe risk of collision between vehicles in these scenarios. These scenarioswill be simulated using a prototype implementation. Some log datawas also gathered in the roundabout at Scania’s test track so that theperformance of the model could be evaluated when running on real data.The main research questions to be answered in this work are:

• Given data from the environment of a specific traffic scenario,how can one predict the collision risk between vehicles usinga statistical model based on a Dynamic Bayesian Network aspresented in [13] and [15]?

4


• Does the predicted collision risk agree with our intuition ofrisk in these scenarios?

• What changes to the model presented in [13] and [15] areneeded to enable risk prediction in roundabouts?

• How can the proposed model be used to predict the collisionrisk for an ego-vehicle?

1.3.2 Outline of the Report

The report is structured as follows: Chapter 2 covers the theoreticalframework upon which the presented model is based on. Chapter 3describes in detail how a Dynamic Bayesian Network can be used tomodel and predict the collision risk between vehicles. Chapter 4 presentsthe two specific scenes used for testing the implemented model. Chapters5 and 6 summarize the results and gives suggestions for future work.

5

Chapter 2

Theoretical Framework

Before presenting the complete statistical model for assessing the colli-sion risk there are a few mathematical tools needed to be understood.This chapter will start by presenting Markov models and hidden Markovmodels (HMMs). After that, Dynamic Bayesian networks (DBNs) whichgeneralize the concept of HMMs are shortly covered. A particle filter isalso introduced, since it is used for Bayesian inference in this thesis.

2.1 Markov Models and Hidden Markov

Models

Consider a sequence of observations XtTt=0 of a stochastic process atdiscrete time steps and of arbitrary sequence length defined on the prob-ability space (Ω,P). The sequence is a Markov chain if it satisfies theMarkov property, that is, all relevant information to predict the futureobservation XT+1 is captured in XT . This can be expressed in terms ofthe predictive distribution of XT+1 given the past observations XtTt=0

as

pXT+1|X0:T(xT+1 | x0:T ) = pXT+1|XT

(xT+1 | xT )

= qT (xT ;xT+1),(2.1)

where qT (xT ;xT+1) is the transition density from xT to xT+1. This den-sity is said to be time homogeneous if it does not depend on time, thatis, qT ≡ q.

For the observed Markovian sequence xtTt=0 the joint distribution,also called the likelihood of the data, can be computed as

6

CHAPTER 2. THEORETICAL FRAMEWORK

X0

Y0

Xt−1

Yt−1

Xt

Yt

Xt+1

Yt+1

Figure 2.1: Schematic representation of a two-layer hidden Markovmodel with unobservable states Xt and observable states Yt.

L(x0:T ) = pX0:T(x0:T )

= pX0(x0)pX1|X0(x1 | x0) . . . pXT |XT−1(xT | xT−1)

= χ(x0)T∏t=1

q(xt−1;xt),

(2.2)

where χ is a probability measure on X0 for the initial state.In the case of finite state spaces St ⊆ Ω where, for all t, Xt ∈ St

and |St| < ∞ it is possible to enumerate all admissible transitions andform a transition probability matrix A ∈ R|St|×|St+1|. The entry Aij(t)describes the probability of going from state i to state j at time t,

Aij(t) = P(Xt+1 = j | Xt = i). (2.3)

Again, if the transition probabilities Aij(t) do not depend on t, theMarkov chain is called time homogeneous.

Consider now a Markov chain with observable states Yt and unob-servable states Xt according to Figure 2.1.

This network can be modeled by letting:

– χ be a probability distribution on X,

– q be a Markov transition density on X,

– p be a Markov transition density from X to Y .

A Hidden Markov Model is now defined by the Markov chain Xt, Ytt≥0induced by the above probability densities. As in the case of a Markov

7


model with only one chain of observable states, it is possible to define thelikelihood of the observed state sequence ytTt=0 of this Hidden MarkovModel in a similar manner. To do this, the joint density of the completechain is formed and then integrated to find the marginal distribution forthe observable states. The likelihood becomes

L(y0:T ) = pY0:T (y0:T ) =

∫pX0:T ,Y0:T (x0:T , y0:T )dx0:T

=

∫χ(x0)p(x0; y0)

T∏t=1

q(xt−1;xt)p(xt; yt)dx0:T ,

(2.4)

where p(xt; yt) denotes the transition density from xt to yt. Using Bayes’theorem, we get an expression for the conditional distribution of X0:T

given Y0:T = y0:T as

pX0:T |Y0:T (x0:T | y0:T ) =pX0:T ,Y0:T (x0:T , y0:T )

L(y0:T )

=χ(x0)p(x0; y0)

∏Tt=1 q(xt−1;xt)p(xt; yt)

L(y0:T ).

(2.5)

One can now imagine adding even more layers of observable andunobservable variables. For such a system to be Markovian it is requiredthat the nodes of all variables at a given time t must only be affectedby the variable nodes at the previous time step. Such a generalizationleads to what is called Dynamic Bayesian Networks which is covered inthe next section.

2.2 Dynamic Bayesian Networks

A Dynamic Bayesian Network is a direct generalization of HMMs con-sisting of multiple interconnected layers of both observable and unob-servable variables. The variables in the network are related to eachother over adjacent time steps and the dependencies are often repre-sented using a directed acyclic graph. Hidden Markov Models fulfillthese conditions and are thus simple examples of DBNs. The DBN usedin this project is visualized in Figure 3.1.

As in Markov models and HMMs the initial states must be generatedfrom some initial distribution. The transition probabilities and condi-tional densities are often parameterized and designed suitably for a givenproblem to be modelled.

8


2.3 Filtering

In the setting of HMMs and DBNs, the filtering problem refers to theestimation of hidden states of a system given all previous observations.In most cases, these estimations need to be updated in an online mannerwhich means that the estimations are updated based only on the previ-ous estimations and the new data available. With online estimation, thecomputation time is ensured to be linearly dependent on the number oftime steps.

For the Hidden Markov model in Figure 2.1, the filtering problemconsists of computing the probability distribution of Xt at arbitrarytimes t given the observed data up to that time point. That is, thedensity

pXt|Y0:t(xt | y0:t), (2.6)

needs to be computed. In this HMM case, x0:t−1 can be marginalizedout in Equation 2.5 to establish that

pXt|Y0:t(xt | y0:t) =

∫χ(x0)p(x0; y0)

∏tk=1 q(xk−1;xk)p(xk; yk)dx0:t−1L(y0:t−1)

.

(2.7)

Filtering problems are of high relevance in this project since, forexample, driver intentions will be modelled using hidden states. Atany time, what is needed is the ability to compute the distribution foreach driver’s intention in order to make accurate predictions of how theenvironment will evolve.

In some specific cases this integral can be solved analytically. Oneexample is linear Gaussian models which result in the Kalman filter.However, in most cases, only approximations of the distribution areattainable.

Since the integral increases in dimension as new data observations be-come available, it quickly becomes computationally intractable to solvethe integral numerically with standard Monte Carlo methods. There-fore, sequential Monte Carlo methods must be used instead. These willbe discussed in the next section.

2.4 Particle Filter

Particle filtering is a widely used technique for finding an approximatesolution to the filtering problem in general systems and is in its essence asimulation, or a sequential Monte Carlo algorithm. It uses a set of sam-ples to represent the posterior distribution, for instance Equation 2.7,

9


and updates the samples online when a new observation becomes avail-able.

2.4.1 Sequential Importance Sampling

Assume that we want to approximate integrals on the form

τn := Efn [φ(X0:n)] =

∫φ(x0:n)fn(x0:n)dx0:n, (2.8)

where φ : Rn+1 7→ R and fnn≥0 are probability densities which areassumed to be known up to normalizing constants so that the densitiescan be written as

fn(x0:n) =zn(x0:n)

cn, (2.9)

where zn is a non-negative known function and cn is an unknown nor-malizing constant. Sequential importance sampling (SIS) uses a MonteCarlo approach to approximate the integral by generating so called par-ticles. A particle is a draw X i

0:n = (X i0, . . . , X

in) with corresponding

importance weight ωin = ωn(X i0:n). In general, the densities zn might

be hard to sample from to generate the particles. One can then samplefrom another simpler distribution gn(x0:n) instead, known as an instru-

mental density, and then define the importance weights as ωin =zn(Xi

0:n)

gn(Xi0:n)

.

One requirement on this instrumental density is that if gn(x0:n) = 0 thenfn(x0:n) = 0. The integral in Equation 2.8 can be rewritten as

τn =

∫φ(x0:n)fn(x0:n)dx0:n =

∫φ(x0:n)

fn(x0:n)

gn(x0:n)gn(x0:n)dx0:n. (2.10)

What is needed for online inferences is an instrumental density whichcan be written as a product of transition densities such that

gn(x0:n) = g0(x0)n∏k=1

gk(x0:k−1;xk). (2.11)

Why this is of importance for online inference can be seen by looking atthe transition from the state X0:n to X0:n+1 and the importance weights.The density of X0:n+1 is under this assumption given by gn+1(x0:n+1) =gn(x0:n)gn(x0:n;xn+1). Given a sample of the states X0:n, the next stateXn+1 can be sampled from gn(x0:n;xn+1). An updated state is thenformed by setting X0:n+1 = (X0:n, Xn+1). The importance weights can

10


be updated in a similar online fashion by rewriting them as

ωin+1 =zn+1(X

i0:n+1)

gn+1(X i0:n+1)

=zn+1(X

i0:n+1)

zn(X i0:n)gn(X i

0:n;X in+1)

zn(X i0:n)

gn(X i0:n)

=zn+1(X

i0:n+1)

zn(X i0:n)gn(X i

0:n;X in+1)

ωin.

(2.12)

Given particle draws X i0:n ∼ gn(x0:n), i = 1, . . . , N , the Monte Carlo

estimate for τn is then computed as

τn '1

Ncn

N∑i=1

ωinφ(X i0:n), (2.13)

where N denotes the number of particles used. Since both the impor-tance weights and the states can be updated easily in an online manner,one can similarly do an online update from τn to τn+1.

The problem that arises now is that the normalizing constants cn areusually unknown. One solution to this problem is to use self normal-ized importance sampling. Since fn is a density, the expectation can bewritten as

τn =

∫φ(x0:n)fn(x0:n)dx0:n∫

fn(x0:n)dx0:n

=

∫φ(x0:n) zn(x0:n)

cngn(x0:n)gn(x0:n)dx0:n∫ zn(x0:n)

cngn(x0:n)gn(x0:n)dx0:n

=

∫φ(x0:n) zn(x0:n)

gn(x0:n)gn(x0:n)dx0:n∫ zn(x0:n)

gn(x0:n)gn(x0:n)dx0:n

=Egn [φ(X0:n)ωn(X0:n)]

Egn [ωn(X0:n)].

(2.14)

Using the same particles to estimate both expectations, the Monte Carlostep given the particle draws X i

0:n ∼ gn(x0:n), i = 1, . . . , N now becomes

τn '1N

∑Ni=1 ω

inφ(X i

0:n)1N

∑Ni=1 ω

in

=N∑i=1

ωinΩn

φ(X i0:n) := τSISn,N , (2.15)

where Ωn =∑N

i=1 ωin. Again the estimator τSISn can be updated online

as new data points arrive.One can prove through a generalized form of the Marcinkiewicz-

Zygmund inequality that, provided ωn and φ are bounded

‖τSISn,N − τn‖p ≤Cp√N‖ωn‖∞‖φ‖∞, (2.16)

where Cp is a constant and ‖Z‖p = E1/p [|Z|p] denotes the Lp-norm. TheMonte Carlo estimator τSISn,N thus converges as 1√

Nto the true value τn

and is therefore an unbiased estimator.

11


2.4.2 Weight Degeneracy

One drawback of the presented importance sampling method is weightdegeneracy meaning that after just a few steps in the algorithm, mostweights are very small compared to the largest weights. This degeneracyis due to the multiplicative form of the weight updates in Equation 2.12and the increasing dimension of the state space. If the variance of theparticle weights is large, resources are essentially being wasted by up-dating these particles since these will not contribute to the posterior.One possible solution to this problem is presented in the next section.

2.4.3 Sequential Importance Resampling

The problem of weight degeneracy in the standard SIS algorithm canbe solved by adding a resampling step, which results in the sequentialimportance resampling (SIR) algorithm. Since many of the particleswith small weights will not contribute significantly to the posterior, itwould be beneficial to keep the particles with large weights in the sub-sequent updates. This can be done through resampling the particleswith replacement according to the probabilities ωin/Ωn for each particlei. This is also known as multinomial resampling. Given the particlesX i

0:n, ωjnNi=1, new indices are drawn according to

Ki = j, with probabilityωinΩn

. (2.17)

The new set of particles is then defined by

X i0:n = XKi

0:n, (2.18)

with corresponding uniform importance weights ωin = 1N

. The estimatorof τn is in the resampling procedure then replaced by

τn '1

N

N∑i=1

φ(X i0:n) := τSIRn,N . (2.19)

This estimator is still an unbiased estimator since

E[τSIRn,N

]= E

[1

N

N∑i=1

φ(X i0:n)

]=

1

N

N∑i=1

E[φ(X i

0:n)]

= E[φ(X i

0:n)]

= E

[N∑i=1

ωinΩn

φ(X i0:n)

]= E

[τSISn,N

].

(2.20)

12


One can view the resampling step as a form of natural selection amongthe particles where particles with small weights die and particles withlarge weights multiply. One can similarly as before use the Marcinkiewicz-Zygmund inequality to prove that the estimator τSIRn,N converges to τn as1√N

.

2.4.4 Connection to HMMs and DBNs

What is of interest for the HMM filtering problem is the ability to com-pute expectations conditioned on the observed data. Because of this, weset

fn(x0:n) = pX0:n|Y0:n(x0:n | y0:n) =pX0:n,Y0:n(x0:n, y0:n)

L(y0:n)

=χ(x0)p(x0; y0)

∏nt=1 q(xt−1;xt)p(xt; yt)

L(y0:n),

(2.21)

and

zn(x0:n) = pX0:n,Y0:n(x0:n, y0:n) = χ(x0)p(x0; y0)n∏t=1

q(xt−1;xt)p(xt; yt).

(2.22)A common choice of instrumental density for HMMs and DBNs is themarginal distribution of the hidden states unconditioned on the data.For the HMM presented, that is

gn(x0:n) = pX0:n(x0:n) = χ(x0)n∏t=1

q(xt−1;xt), (2.23)

which results in

gk(x0:k−1;xk) = q(xk−1;xk), for any k > 0,

g0(x0) = χ(x0).(2.24)

Inserting Equations 2.22 and 2.24 into 2.12 establishes that

ωin+1 = p(xn+1; yn+1)ωin. (2.25)

Since we are using resampling, the previous weights ωin are all uniformand equal to 1/N . Therefore,

ωin+1 ∝ p(xn+1; yn+1). (2.26)

13

Chapter 3

Method

In this chapter we will go through the model used for estimating thecollision risk in this thesis. In section 3.1 we present an overview of themodel and give a brief introduction to the whole chapter. In sections3.2-3.11, the model is presented in depth and, lastly, in section 3.12 wesummarize the complete model.

3.1 Overview

The model we present here is based on what is done in [13] and [15].It uses abstractions of drivers’ behaviour to interpret the state of thescene. More specifically, the model incorporates the intended course ofdrivers, the drivers’ intention to stop and their expected behaviour giventhat they will follow the traffic rules. A DBN is used for modelling thebehaviour evolution and physical evolution of vehicles in the scene. Thisis a popular choice for interaction-aware models, such as this one, sinceit yields a lot of freedom and makes it easy to add complexity to themodel by introducing additional states.

The structure of the DBN is illustrated in Figure 3.1. For eachtime step and each vehicle we have 4 variables. The hidden variablesare expected behaviour, intended behaviour and the true physical statedenoted by E, I and ζ respectively. The observable variable is themeasured physical state denoted by M . In the figure, the superscriptdenotes the vehicle that is considered and the subscript denotes thetime step. We will define these variables and describe them thoroughlyin section 3.2.

Arrows in the graph show the variable dependencies. The black ar-rows indicate a dependency only on vehicle k while red arrows indicate adependency on all vehicles in the scene. Note that it is only the expected

14

CHAPTER 3. METHOD

behaviour that depends on all other vehicles. This can be intuitively un-derstood as the expected behavior being directly influenced by what theother vehicles in the scene are doing. The intended behaviour on theother hand is not directly dependent on this, but rather is decided bythe driver and what is expected of the driver. Also, note that Figure 3.1only represents the system for one vehicle in the scene. The completegraph for all vehicles consists of multiple layers where each layer repre-sents one vehicle. More details on variable dependencies will be given insection 3.3.

In section 3.4 the intelligent driver model (IDM) is introduced. Thismodel is a common tool used to predict the speed of vehicles in a scenewith multiple participants and it will be used to construct an alterna-tive update of the expected behaviour compared to the update used in[13] and [15]. In section 3.5, speed profiles and their connection to asimplified IDM are discussed.

The model studied in this work has nonlinearities in the transitiondensities, both for the expected maneuver and the true physical state.Because of this it is not possible to get the optimal solution to thefiltering problem on closed form [10]. An approximation is thereforeneeded which in this thesis will be based on particle filtering, as in [13]and [15]. The filtering procedure is outlined in section 3.6.

In section 3.7 we introduce the first measure of risk. This measure isthe one presented in [13] and [15] and is closely coupled with the model.Intuitively, it is the maximum probability of any vehicle in the sceneviolating the traffic rules.

In [13] and [15] the authors only studied intersections. We haveexpanded upon this model so that it can be used in roundabouts aswell. In the expanded model, a more involved process is needed whencomputing the expected behaviour update. This process uses the newconcept of collision zones which is introduced in sections 3.8 and 3.9.

The collision zones are also used as a foundation for the other riskmeasure which is based on trajectory prediction. This measure yieldsa risk induced on the ego-vehicle specifically which combats the mainweakness of the first measure. More on this in section 3.10.

15

CHAPTER 3. METHOD

Ekt Ek

t+1

Ikt Ikt+1

ζkt ζkt+1

Mkt Mk

t+1

Figure 3.1: Schematic representation of the DBN. Black arrows indicatea dependence on vehicle k. Red arrows indicate a dependence on allvehicles in the scene.

3.2 Variables

In this section the variables used in the model are defined. Since theDBN has the same structure as in [13] and [15], the set of variables willbe similar. These variables are Ek

t , Ikt , P kt and Mk

t .

1. Intended behaviour (Ikt ): The intended behaviour represents theintentions of the driver and is divided into two components. Thefirst one being the lateral component which is denoted as Icnt . Thisrepresents the current intended course of vehicle k at time step t.The set of all possible courses will be denoted by C = ciNc

i=1,where ci is a course and Nc is the total number of courses in thescene. As an example of a set of courses, consider the intersectionsetting shown in Figure 3.2. Here 6 different courses are defined;3 of these are approaching from the south and turning right/leftor continuing straight (red) and 3 are approaching from east and

16

CHAPTER 3. METHOD

turning right/left or continuing straight (blue). A course in thissetting could for instance be ’approaching from the south and turn-ing left’. For each course there will be a corresponding normalizedarc-length parameterization of the trajectory γci(s) : [0, 1] → R2.To clarify, γci is constructed such that γci(a/length(ci)) yields thearc-length parameterization (with a as parameter given in metersfrom the beginning of the course) of the trajectory. Here length(ci)denotes the total length of the course ci. Setting s = 0 will yieldthe first point of the course and s = 1 will yield the last point ofthe course.

The second component is the longitudinal one, denoted by Iskt .This component represents the driver’s intention to stop or go. Inthe intersection context the intention to stop will translate to thedriver aiming to stop just before the stop line of the intersection.If the vehicle is past the intersection this variable will representthe driver’s prior intention to stop. That is, whether the driverhad the intention to stop before entering the intersection. It isimportant to keep track of the longitudinal intention even afterthe stop line since the speed will still depend on this. If the priorintention was to go, then a high speed is expected just after thestop line. On the other hand, if the prior intention was to stop,then the speed should be low just after the stop line.

2. Expected behaviour (Ekt ): The expected behaviour represents what

the driver should do in order to follow the traffic rules. Both theintention and expectation are based on the same set of behaviours,so one could expect the same pattern here as for the intentions,with an expected lateral component and an expected longitudinalcomponent. However, the expected behaviour is limited to only in-clude the longitudinal motion (Eskt ). This limitation is motivatedby the reasonable assumption that all drivers have a goal that theywill stick to independently of how the scene evolves. Therefore, theintended course does not depend on the other vehicles in the sceneand the notion of an expected course is not needed. This is incontrast to the intention to go/stop where each driver’s intentionis influenced by all other vehicles in the scene. As above, Esktdenotes the expected longitudinal behaviour for vehicle k at timestep t.

3. Physical state (ζkt ): This state consists of the true physical stateof the vehicle. For vehicle k at time step t, the true position isdenoted by Lkt , the true speed by Skt and the true heading by θkt .

17

CHAPTER 3. METHOD

4. Measurements (Mkt ): This state consists of the measured physical

state of the vehicles. Similar to what was done above, for vehi-cle k at time step t the measured position is denoted by Lmk

t ,the measured speed by Smk

t and the measured heading by θmkt .

In our case the measured position will be given in the UniversalTransverse Mercator (UTM) coordinate system, but in theory anyglobal frame will work.

A summary of all variables together with their respective domains isgiven in Table 3.1.

Variable category Variable DomainEkt Eskt go, stopIkt Iskt go, stop

Ickt C = ciNci=1

ζkt Lkt R2

Skt (−∞,∞)θkt (−π, π]

Mkt Lmk

t R2

Smkt (−∞,∞)

θmkt (−π, π]

Table 3.1: A summary of all variables used in the model.

18

CHAPTER 3. METHOD

Figure 3.2: Example intersection showing courses coming from southand east. The horizontal road is a primary road and vehicles on theother road are thus expected to yield at the intersection when othervehicles are present.

3.3 Variable Dependencies

Recall now the framework of Hidden Markov models and Dynamic Bayesiannetworks in Chapter 2. We will start by using the same techniques as forHMMs to derive the joint distribution of the presented DBN. Bold sym-bols will represent variables containing data of all vehicles in the scenecollectively, for example ζt = (ζ1t , . . . , ζ

Kt ) represents the joint physical

state for all K vehicles. For simplicity we assume that the number ofvehicles K in the scene remains fixed for a given scenario. For a prac-tical implementation however, one must ensure that the model is ableto handle a varying number of vehicles. A possible implementation tohandle vehicles that enter or leave the scene would be to add or removelayers dynamically in the DBN.

Throughout the rest of the thesis we will write pX|Y (x | y) as

19

CHAPTER 3. METHOD

P (X = x | Y = y) and pX|Y (· | ·) as P (X | Y ) for continuous variablesin order to simplify notation.

Consider now the joint distribution of all nodes and all vehicles as inFigure 3.1 for discrete time steps between 0 and T . Using the Markovproperty, this can be written as

P (E0:T , I0:T , ζ0:T ,M0:T ) =P (E0, I0, ζ0,M0)

×T∏t=1

×K∏k=1

[P (Ek

t | It−1, ζt−1)

× P (Ikt | Ekt , I

kt−1, ζ

kt−1)

× P (ζkt | Ikt , ζkt−1)× P (Mkt | ζkt )

],

(3.1)

where P (E0, I0, ζ0,M0) is the initial state distribution. We will now tryto describe and simplify the factors in this product.

First of all, we assume that the intended longitudinal and lateralmotions are conditionally independent given (Ek

t , Ikt−1, ζ

kt−1) which leads

to

P (Int | Ekt , I

kt−1, ζ

kt−1) = P (Ickt | Ek

t , Ikt−1, ζ

kt−1)× P (Iskt | Ek

t , Ikt−1, ζ

kt−1).(3.2)

To treat the physical state ζ which contains the position, speed andheading of the vehicle we here also assume that these are conditionallyindependent given (Ikt , ζ

kt−1), that is

P (ζkt | Ikt , ζkt−1) =P (Lkt | Ikt , ζkt−1)× P (Skt | Ikt , ζkt−1)× P (θkt | Ikt , ζkt−1).

(3.3)

For the measured position, speed and heading we assume that theseonly depend directly on their true hidden counterparts which leads tothe decomposition

P (Mkt | ζkt ) = P (Lmk

t | Lkt )× P (Smkt | Skt )P (θmk

t | θkt ). (3.4)

Lastly, we again emphasize that the expected behaviour E only con-sists of the longitudinal motion meaning that Ek

t = Eskt . Using the

20

CHAPTER 3. METHOD

above, the resulting joint distribution can be expressed as

P (E0:T , I0:T , ζ0:T ,M0:T ) =

P (E0, I0, ζ0,M0)×T∏t=1

×K∏k=1

[P (Eskt | It−1, ζt−1)

× P (Ickt | Ekt , I

kt−1, ζ

kt−1)× P (Iskt | Ek

t , Ikt−1, ζ

kt−1)

× P (Lkt | Ikt , ζkt−1)× P (Skt | Ikt , ζkt−1)× P (θkt | Ikt , ζkt−1)

× P (Lmkt | Lkt )× P (Smk

t | Skt )× P (θmkt | θkt )

].

(3.5)

Now follows a description of the parametric forms used for updatingthe particles. We adapt many of the proposed forms in [13] and [15] onwhich this model is mainly based and tweak some to our liking.

The initial state distribution will be written as

P (E0, I0, ζ0,M0) =K∏k=1

P (Ek0 , I

k0 , ζ

k0 ,M

k0 )

=K∏k=1

P (Esk0)P (Ick0)P (Isk0)P (ζk0 |Mk0 )P (Mk

0 ).

(3.6)

The choice of priors will be discussed in section 3.6.Next we need to specify all the transition densities inside the product

of the joint distribution in Equation 3.5 and motivate these choices.

1. Expected longitudinal motion Eskt : P (Eskt | It−1, ζt−1).The expected behaviour of a vehicle contains information aboutwhether vehicle k is expected to yield or not in an upcoming in-tersection or roundabout. This behaviour is of course governed bythe traffic rules and the relative positions of all vehicles. A vehicleis expected to yield at a give way intersection if there is anothervehicle with right of way to which the time gap is not too large.If the time margin to all other vehicles with right of way is largeenough and the intended maneuver can be performed safely, thenthe vehicle would not be expected to stop at the intersection. Theexpected longitudinal motion should therefore only depend on theintended courses of all vehicles and also their positions and veloci-ties. We do not need to take the own intention to stop into accountsince this should not influence our expected behaviour. This leadsus to an expression on the form

P (Eskt | It−1, ζt−1) = P (Eskt | Ict−1, ζt−1). (3.7)

21

CHAPTER 3. METHOD

We therefore need to construct a function that computes Eskt tak-ing into account if the time gap is large enough for vehicle k toexecute its intended maneuver given Ict−1,Lt−1, St−1 and θt−1.The procedure is as follows:

In a pre-processing step, for the courses in the scene, construct aboolean right of way-matrix (ROW ) Yij which tells us if a vehicleon course i has right of way with respect to vehicles on course jaccording to the traffic rules. As an example, consider a 4-waycross section as in Figure 3.2. If course i is being on the primaryroad and continuing straight from east and course j is being onthe other road and turning left from south we would have Yij = 1since course i has right of way with respect to course j.

There are also cases where the courses cross each other multipletimes and it is less obvious how this boolean should be set. Itshould here be added that each course is constructed so that itonly has one place for which vehicles are supposed to stop, rightbefore the first collision zone for the course pairs (more on this insection 3.8). Consider the roundabout in Figure 3.3 and the twohighlighted courses. Both courses are expected to yield to vehicleson the other course but only when entering the roundabout. Afterthe vehicles have passed this point they should no longer yield toother vehicles since they will then have right of way. In these casesthe ROW -matrix boolean is set to true, since the vehicles shouldyield to vehicles on the other course at one place.

22

CHAPTER 3. METHOD

Figure 3.3: Example roundabout showing two courses that intersect intwo different regions. Vehicles should only yield for vehicles on the othercourse when they approach the first intersection with that course, thatis when entering the roundabout. If a vehicle has already entered theroundabout, then it should not yield to other vehicles.

In the next step, for each other vehicle k′ 6= k, use the intendedcourses Ict−1 together with the matrix Y to compute the set ofvehicles ROWk which has right of way with respect to the intendedcourse Ickt−1 of vehicle k. This set makes up vehicles to whichvehicle k might have to yield to. Now find the vehicle k ∈ ROWk

which is most probable to make vehicle k stop, for example bylooking at the time gaps between vehicle k and the vehicles inROWk when reaching the collision zone. More details on how thisis done can be found in section 3.9.

If vehicle k has already passed the entry-point to its first collisionzone, the vehicle is not expected to stop anymore. The logic behindthis is that it would be unwise to stop at a place where other

23

CHAPTER 3. METHOD

vehicles could crash into you in the future. In this case, we simplyset Ek

t = 0. If the vehicle has not entered the collision zone withrespect to vehicle k a probabilistic gap acceptance model is usedto determine if the driver of vehicle k will accept the gap or not.An example study of gap acceptance behaviour can be found in[20]. If the gap is not accepted, then the vehicle will be expectedto stop, Ek

t = 1, otherwise Ekt = 0.

2. Intended course Ickt : P (Ickt | Ekt , I

kt−1, ζ

kt−1).

The intended course of each driver is assumed to be fix but un-known independently of what happens in the scene. One can viewthis as the driver having already decided beforehand on what paththey want to take through the intersection and that this decisioncan not change due to the actions of other road-users. Since thecourse of each driver is not known we must make sure that the par-ticles explore the different courses during the filtering in a greedyfashion. Particles that have chosen the wrong intended course willall get small importance weights as this will lead to an update tothe position and velocity that does not match the measurementdata very well. With this in mind we let the intended course forvehicle k be updated as

P (Ickt = ci | Ickt−1 = cj) =

1− ε+ ε

|C| , if ci = cj,ε|C| , otherwise.

(3.8)

This means that with a probability ε we explore a random courseand with probability 1− ε we keep the same course as before.

3. Intended longitudinal motion Iskt : P (Iskt | Ekt , I

kt−1, ζ

kt−1).

The intention to stop or go at the intersection is assumed as in [13]and [15] to depend on the driver’s past intention Ikt−1 as well aswhat is expected of the driver at the moment, Eskt . Furthermore,we incorporate a dependence on the previous position Lkt−1, thatis

P (Iskt | Ekt , I

kt−1, ζ

kt−1) = P (Iskt | Eskt , Ikt−1, Lkt−1). (3.9)

This probability will model how likely the driver is to comply withthe traffic rules at any time given the previous intention and lo-cation. First of all, it is determined if the vehicle has passed thestop line or not. A vehicle is considered to be past the stop lineif the point L′′ (see Figure 3.4) is past the predefined point of thestop line at the intended course.

24

CHAPTER 3. METHOD

If the vehicle has not yet passed the stop line we would like theintention to stop to be closely related to the expected maneuver.In this case we define the discrete distribution

P (Iskt | Ekt , I

kt−1, ζ

kt−1) = P (Iskt | Eskt , Iskt−1), (3.10)

as specified in Table 3.2. The parameter Pcomply is a hyperparam-eter that describes the probability of having the intention to gogiven that the past intention was to go and that it is the currentexpected behaviour as well.

Iskt−1 Eskt P (Iskt = go | Iskt−1, Eskt )go go Pcomplygo stop 0.5stop go 0.5stop stop 1− Pcomply

Table 3.2: The parameterization for traffic rule compliance before thestop line given past intention and current expectation.

If the vehicle has driven past the stop line, the expected behaviourwill always be to go and is therefore no longer relevant for theintention. As mentioned in section 3.2 the intention to stop willin this case reflect the intention the driver had before the stopline. Because of this the dependence of Ek

t is dropped and thedistribution for the intention to stop is set to

P (Iskt | Ekt , I

kt−1, ζ

kt−1) = P (Iskt | Iskt−1), (3.11)

as specified in Table 3.3.

Iskt−1 P (Iskt = go | Iskt−1)go Pcomplystop 1− Pcomply

Table 3.3: The parameterization for intention compliance after the stopline given past intention.

4. True position Lkt : P (Lkt | Ikt , ζkt−1).The updated true position is computed from the intended courseand the previous position and velocity. The position is updatedusing a constant velocity model together with the assumption that

25

CHAPTER 3. METHOD

the driver wants to follow the intended course. The likelihood ofthe new location is given by a bivariate normal distribution witha diagonal covariance matrix.

P (Lkt | Ikt , ζkt−1) = P (Lkt | Ickt , Lkt−1, Skt−1, θkt−1) = N (µL,ΣL).(3.12)

To calculate the mean of this distribution, we first propagate theprevious position Lkt−1 using a constant velocity model to find anew location L′. This new position is then projected onto thetrajectory of the current intended course Ickt , which gives the po-sition L′′, using the Matlab function distance2curve as providedin [5]. The mean µL is taken as the mean position of these twonew points, µL = 1

2(L′+L′′). The covariance matrix is ΣL and we

will cover later exactly how this parameter was set. See Figure 3.4for a visualization.

5. True speed Skt : P (Skt | Ikt , ζkt−1).The true speed is updated in a similar way to the true position.Again it is assumed that the driver wants to follow the intendedcourse and adapt the speed according to the road geometry. If theintention is to stop, the driver is assumed to follow some predefinedspeed profile that makes a stop before the intersection. The speedprofile will be called u and is function of the intention and theposition along the intended course (the arc-length parameter ofthe course). The likelihood in this case is specified as a normaldistribution similar to the case for the true position.

P (Skt | Ikt , ζkt−1) = P (Skt | Ickt , Iskt , Lkt−1, θkt−1, Skt−1) = N (µS, σ2S).

(3.13)The mean µS is computed as the mean of the previous speed Skt−1and the speed of a typical driver following the intended courseat the projected point L′′. The typical speed at L′′ is given byu(Ikt , s), where s = γ−1

Ickt(L′′) is the arc-length parameter that yield

L′′. Details on this is discussed in section 3.5. The choice ofvariance σ2

S will be covered later. See Figure 3.4 for a visualization.

6. True heading θkt : P (θkt | Ikt , ζkt−1)The heading is computed by the wrapped normal distribution

P (θkt | Ikt , ζkt−1) = P (Ickt , Iskt , L

kt−1, S

kt−1, θ

kt−1) =WN (µθ, σ

2θ).(3.14)

The wrapped normal distribution is used since a draw will thenalways end up inside the domain for the polar angle. The mean

26

CHAPTER 3. METHOD

µθ is taken as the average of the current heading and the headingof the projected future point L′′ on the course. The second oneis attained in a similar manner as the typical speed of the courseby looking at a predefined function h(I, s) at s = γ−1

Lckt(L′′). The

function h is similar to u but returns the heading of the course atthe position s of the intended course. Note that since θ ∈ (−π, π]there could be a discontinuous jump from a heading close to −πto a heading close to π in one time step. We must therefore formthe mean µθ by taking a circular mean of the headings at the twopoints L′ and L′′. See Figure 3.4 for a visualization of how theheading is updated.

v

u

Lt

Lt+1

L′

L′′

Figure 3.4: A vehicle position, speed and heading update example for avehicle within a particle. The vehicle state at time t (red) is updated tothe new state at time t+ 1 (green). The green updated state illustratesthe mean of the Gaussian distributions for the new position, speed andheading as presented above.

27

CHAPTER 3. METHOD

7. Measured position Lmkt : P (Lmk

t | Lkt ).Given the true position of a vehicle we would like to model thedistribution of the measurement data we receive. A rather commonchoice of model for this is to assume that the noise of the sensoris Gaussian and thus let

P (Lmkt | Lkt = lkt ) = N (lkt ,ΣLm), (3.15)

where the covariance matrix ΣLm depends on the sensor arrange-ment.

8. Measured speed Smkt : P (Smk

t | Skt )

The same thing applies for the measured speed as for the mea-sured position where we again assume that the sensor noise can bemodeled as a Gaussian distribution.

P (Smkt | Skt = skt ) = N (skt , σ

2Sm). (3.16)

9. Measured heading θmkt : P (θmk

t | θkt )Lastly we also assume the measured heading to be wrapped nor-mally distributed given the true heading.

P (θmkt | θkt = ϑkt ) =WN (ϑkt , σ

2θm). (3.17)

The choice of variance σ2θm is specified later. The likelihood of a

measured heading is evaluated through the density of the wrappednormal distribution. The likelihood for a measurement θm giventhe true heading θ = ϑ is given by

LWN (θm | θ = ϑ) =1

σθm√

2π

∞∑k=−∞

exp

[− 1

2σ2θm

(θm+ 2πk − ϑ)2].

(3.18)Since we will be using very small variances σθm 1, a suitableapproximation to this infinite sum is computed using the threeterms for which k = −1, 0 and 1,

LWN (θm | θ = ϑ) ≈ 1

σθm√

2π

1∑k=−1

exp

[− 1

2σ2θm

(θm+ 2πk − ϑ)2].

(3.19)

28

CHAPTER 3. METHOD

3.4 A Modified Intelligent Driver Model

A commonly used model for car-following in highway and urban scenar-ios is the intelligent driver model (IDM). For a vehicle with speed v thatis potentially following another vehicle the dynamics are described bythe ordinary differential equation

v = a

(1−

(vu

)δ−(d∗(v,∆v)

d

)2), (3.20)

where a is the maximum acceleration, u the desired speed, δ anacceleration exponent, ∆v the relative speed of the two vehicles and dthe distance between the two vehicles. The function d∗ is given by

d∗(v,∆v) = d0 + vT +v∆v

2√ac, (3.21)

where d0 is the minimum allowed distance between the two vehicles, Tis the safe time headway and c is the comfortable deceleration.

The differential equation is designed so that the vehicle should ap-proach the desired speed without exceeding the maximum acceleration awhen there is no other car in front and adapt it speed to avoid collisionswith other vehicles in front. For our purpose we will limit ourselvesto only study the free road behaviour where the last term in the IDMmodel drops out and the dynamics instead becomes

v = a

(1−

(vu

)δ). (3.22)

The acceleration is bounded from above by a and we would similarlylike to have a bound on the deceleration. We therefore let

v = max

[−b, a

(1−

(vu

)δ)], (3.23)

where b is a parameter for the maximum deceleration. Using this modelwe will be able to predict the future speeds of other vehicles given theknowledge of typical speed profiles for the courses. Another key resultis that this model will enable us to estimate the time until a vehiclereaches a certain point along its course. Why this is important will beintroduced later in this chapter.

29

CHAPTER 3. METHOD

3.5 Speed Profiles

The speed profile was first introduced in section 3.3 and is denoted byu(Ikt , s). It is a function from the intended behaviours and the the arc-length parameter s ∈ [0, 1] to R.

For any course ci and any intended longitudinal motion (either goor stop), the speed profile u(Ikt , s) will take the value of a typical speedat the point γIct(s) along the course Ict in free flow conditions. Inother words, it represents the speed a typical driver will have whenperforming the maneuver Ikt , when the position γIct(s) is the closestpoint to the vehicle along the course Ickt and when there is no othervehicles in the scene. A good construction of u is crucial for the modelsince this function will be very influential when inferring the intentionof drivers. If the position and heading is the same for multiple courses,which is often the case, then the speed will be the only differentiatingaspect.

Consider again the 4-way intersection illustrated in Figure 3.2 andconsider a case where one vehicle is approaching from the south. If thevehicle is currently positioned before the stop line, the projected positionand heading will be identical for all courses coming from the south butthe typical speed might differ.

Other examples include cases that compare the different intendedlongitudinal motions stop and go. The speed profiles for multiple coursescould be identical in these cases. For example, when a vehicle is ap-proaching from the south in the 4-way intersection and when the driverhas the intention to stop. If the position of the vehicle is before the stopline, then all three courses are likely to have indistinguishable speed pro-files since the driver then intends to stop at the stop line. In other words,the transition probabilities for the true physical state will be identicalregardless of which intended course coming from the south is assigned.In these cases it will be hard to identify what exact course a driver isfollowing since the speed does not provide any useful information onwhere the vehicle is heading. This behaviour of the model can also beobserved in some of the results in Chapter 5. Of course, this is expectedsince a human observer would also not be able to tell what course thevehicle was following in this case. In practice however, drivers that forexample have the intention to turn left after making a stop might keepto the left side of the lane when approaching the intersection. Such be-haviour could quite easily be accommodated for by tweaking the coursepositions slightly, although we will not elaborate on this much further.

Some work has been done in analysing the behaviour of drivers in

30

CHAPTER 3. METHOD

intersections and roundabouts [6], [16], [19]. Since the typical speedswill depend on the scene, one approach is to base the speed profiles ondata from the scene or from other scenes with similar layouts. This kindof analysis is done in [6] and [16]. However, gathering data from allpossible intersection and roundabout layouts requires a lot of work andmakes it hard to scale the model for use in the road network.

Another approach is to use generic features of the scene to asses thetypical speed. Some examples of features could be the curvature of thecourse or the distance to the stop line. In [1], the speed of drivers inroundabouts is put into the following equation,

V =√

127 ·R · (e+ f), (3.24)

where V is the speed (km/h), R is the radius of the course (m), f is thefriction factor and e is the superelevation (m/m). Here the curvatureof the course among other features is used to predict the typical speed.When considering the speed profile for having the intention to stop, anobvious feature to incorporate is the distance to the stop line. In [19]a field study was performed where the deceleration behaviour before asignalized intersection was studied.

Using large amounts of gathered data from specific scenes is unfor-tunately out of the scope for this thesis. Instead, we use generic featuresand generalize the findings of other works, such as [16], to construct thespeed profiles manually for the two cases studied.

In order to generate speed profiles after the stop line when the in-tention is to stop, we use the modified IDM as presented in section 3.4together with the initial condition of standing still at the stop line. Thedesired velocity is set to some speed limit closely related to the speedprofile of the same course but with longitudinal intention to go.

3.6 Applying SIR

In this section we will introduce the notation used throughout the re-mainder of this chapter related to the particle filtering SIR algorithm.The main steps of the filtering algorithm will also be presented beforediving into the details in the later sections of this chapter.

First of all, we must introduce what is meant by a particle in ourproblem of interest. A particle can in this setting be viewed as a snap-shot of the entire scene, containing information of all vehicles and theirrespective hidden variables. The variable N will be used to denote thetotal number of particles. A particle for a scene with K vehicles will

31

CHAPTER 3. METHOD

look as follows: E1, I1, ζ1E2, I2, ζ2

...EK , IK , ζK

. (3.25)

The corresponding importance weight for particle i at time point t willbe denoted ωit. Note that this weight applies for the entire particle andnot a single vehicle. All variables within the particle are dependent ontime and will be updated at the same rate as new data becomes available.

The algorithm is initialized by generating the N particles from a priordistribution. For the K vehicles in the scene, a first measurement of thephysical state M1

0 , . . . ,MK0 arrives from the sensors. One particle is

generated by sampling the variables for all vehicles within it. In orderto get a reasonable first set of particles, the prior distribution is basedon the first measurements. For each vehicle we let

Lk0 ∼ N (Lmk0,ΣL),

Sk0 ∼ N (Smk0, σ

2S),

θk0 ∼ WN (θmk0, σ

2θ),

(3.26)

whereWN denotes the wrapped normal distribution. The other hiddenvariables Ek

0 and Ik0 are drawn uniformly over their respective domains,as defined in Table 3.1. A prior for the measurement does not need tobe specified since the particles are samples of the hidden states.

The importance weights are computed using Equation 2.26. Forparticle i at time step t the importance weight is

ωit ∝K∏k=1

P (Lmkt | L

k,it ,ΣLm)P (Smk

t | Sk,it , σ2

Sm)P (θmkt | θ

k,it , σ

2θm),

(3.27)where P (Lmk

t | Lk,it ,ΣLm) and P (Smk

t | Sk,it , σ2

Sm) are densities of nor-mal distributions. The first one has mean Lk,it and variance ΣLm andthe second one has mean Sk,it and variance σ2

Sm. P (θmkt | θ

k,it , σ

2θm) is

the density of a wrapped normal distribution with mean θk,it and vari-ance σ2

θm. In practice, the evaluation of the wrapped normal density isestimated as in Equation 3.19. After all weights have been computedthey are also normalized so that they sum to 1.

After the initial generation of particles is set up, the main procedurecan begin. Measurement data will now arrive at regular intervals and theparticles are to be used as an approximation of the posterior distributiongiven this data. For each time step, the particles are now propagated

32

CHAPTER 3. METHOD

using the transition probabilities specified in section 3.3. The weightsare then computed according to Equation 3.27 and normalized onceagain. The idea is that the particles where the hidden variables agreewith the true state of the system will after the propagation conformbetter with the next measurement data. Therefore, the resulting weightsshould be higher for particles that manage to correctly identify the driverintentions compared to the ones that do not.

The last step in the SIR algorithm is to resample the particles withreplacement using the importance weights as probabilities. Particleswith correctly identified hidden states will be drawn multiple times whileparticles with wrong states are left out. The weights are all reset to 1/Nafter the resampling step.

Another approach in approximating the solution to the filtering prob-lem is to use a nonlinear Kalman filter, such as the extended Kalmanfilter or the Unscented Kalman filter. The Unscented Kalman filter isused in [18] for a similar model and shows promising results. It wouldtherefore be interesting to use the Unscented Kalman filter with thismodel as well, but this will be left as future work.

3.7 Estimating Risk Using the DBN

During the algorithm the particle states are updated sequentially andeach assigned a corresponding importance weight. A particle in ourcase contains information about all K vehicles in the scene and theirrespective inferred hidden states. As described in [13] and [15], onepossible way to use the DBN to estimate the risk level in the scene isto look at the probability of intended longitudinal motion being to gowhile the expected behaviour is to stop. This should intuitively resultin an increase in the collision risk of the scene since most of the timeit is expected of other drivers to act in accordance to the traffic rules.In the case that someone violates the traffic rules there should be animminent increase in the collision risk. For each vehicle, it is possible tosequentially evaluate the probability

P (Iskt = go, Eskt = stop |M0:t), (3.28)

and use this as an indicator for risk. Using the SIR particle filter, it isstraightforward to estimate this risk for each vehicle k by counting thenumber of particles where Iskt = go and Eskt = stop and weighting themusing the importance weights. This leads to a sum over the particles

33

CHAPTER 3. METHOD

i = 1, . . . , N as,

Rk(t) := P (Iskt = go, Eskt = stop |M0:t) 'N∑i=1

ωit1Isk,it =go,Esk,it =stop.

(3.29)The global risk R(t) for a scene could then be defined as the maximumover all vehicles

R(t) = maxk=1,...,K

Rk(t). (3.30)

In most cases, for example a T-junction with only a few vehicles inthe scene, this probability translates well into a collision risk. However,in more complex scenarios where vehicles interact in multiple areas, thismeasure might not assess the risk accurately from the perspective ofthe ego-vehicle. An example of this could be a scenario in which theego-vehicle is approaching a roundabout and on the opposite side ofthe roundabout another vehicle violates the traffic rules. A risk is thenidentified which might in fact be relevant for some vehicles in the scene,but not for the ego-vehicle in this case.

What is needed here is a measure that can predict the risk othervehicles induce on the ego-vehicle. Risk measures which fulfill this arethe previously mentioned measures TTC and TTCCP. If the time tocollision for the ego-vehicle is long we would intuitively like to assigna low risk compared to times to collision of just a few seconds. Insections 3.8-3.10 we present an adaption to the model that will enablethe prediction of collision risk induced on an ego-vehicle.

3.8 Collision Zones

In this section the concept of collision zones is introduced as a newapproach in order to adapt the model in [13] and [15] so that it can beused for risk assessment based on trajectory prediction. This approachwas developed due to the pre-defined courses used in the model and theneed of a risk prediction measure for an ego-vehicle.

The collision zones enter the model in two ways. The first is in thegap acceptance model, where the time gap is based on the time to thecritical collision zone, explained in section 3.9, and the second use is inthe collision risk measure TTCCP, further discussed in section 3.10.

For each pair of courses, ci, cj, there will be a collision zone denotedCZi,j defined as the set of all pairs of normalized arc length parameterssi and sj such that two vehicles with positions γci(si) and γcj(sj) collide.

34

CHAPTER 3. METHOD

The collision zone will of course depend on the shape of the two vehi-cles. The vehicles may be represented by any convex two-dimensionalshape. However, for simplicity, all vehicles will be represented by two-dimensional rectangles with length L and width W . Two vehicles aresaid to be colliding if they have some overlap with each other. The localcoordinate frame of a vehicle is shown in Figure 3.5. The position of theorigin is located on the center line along the vehicle and has an offsetLrear from the rear of the vehicle.

y

x

rearL

L

W

Figure 3.5: The local coordinate frame of each actor. The vehicles havea length L, width W and an offset Lrear for the origin.

To check for collisions we use the same procedure as in [17]. Aquick way to rule out a collision between two vehicles is to only con-sider their positions. If the positions of their origins are further than2√

min(Lrear, L− Lrear)2 +W 2/4 apart, then there is no collision. Ifthis is not the case, their headings must be considered in order to de-termine whether they overlap or not. A useful tool is the hyperplaneseparation theorem [21], which states the following:

Theorem 3.8.1 (Hyperplane Separation Theorem) Let A and Bbe two disjoint convex subsets of Rn, then there exists a non-zero vectorv and a real number c such that 〈x,v〉 ≥ c and 〈y,v〉 ≤ c for all x ∈ Aand all y ∈ B, that is, the hyperplane 〈· ,v〉 = c separates A and B.

35

CHAPTER 3. METHOD

The theorem can be used for collision detection and is in this settingcalled the separating axis theorem. The theorem formulates an algorithmfor testing whether two convex objects collide or not. It states thattwo convex sets do not overlap if there is a separating axis where theirprojections onto this axis do not intersect. A visualization can be seenin Figure 3.6.

Figure 3.6: A visualization for the separating axis theorem in which thetwo vehicles do not collide. As we see in this figure there is (more than)one axis where the projections do not overlap leading to the conclusionthat the vehicles are not colliding.

To apply the theorem for determining if two vehicles are collidingit is sufficient in the case of two rectangles to look at four differentnormal vectors for potential separating axes. Each bounding rectangleof a vehicle has two pairs of parallel sides. One of these four candidatesides must then work as a separating axis if the two vehicles do notintersect. The collision test therefore consists of testing if at least oneof these sides defines a normal vector that works as a separating axis.

36

CHAPTER 3. METHOD

This is done by looking at the projections of the two objects onto theseaxes to see if there is at least one with no overlap. If all four axes failat separating the two bounding rectangles, then the vehicles must becolliding. A visualization of a collision can be seen in Figure 3.7.

Figure 3.7: A visualization for the separating axis theorem in which thetwo vehicles collide. As we see in this figure there is no axis where theprojections do not overlap leading to the conclusion that the vehiclesare in fact colliding.

Given two positions along two courses it is now easy to determineif two vehicles placed at these points would collide. The courses arethen discretized into small sections, say 0.2 m each. For each pair ofdiscretization points (i, j) along the two courses c1, c2, a collision ma-trix Cc1,c2ij consisting of ones and zeros is constructed. This matrixindicates if the two vehicles would be colliding if they were placed atthese two specific points on the respective courses and provided thattheir heading is directed along the courses. The collision matrices arecomputed for each pair of courses in a pre-processing step. By symmetryone concludes that

Cc1,c2ij = Cc2,c1

ji . (3.31)

37

CHAPTER 3. METHOD

3.9 Making use of Collision Zones

In the calculation of expected maneuver for all drivers it is not sufficientto just know who has right of way or not. As described in section 3.3, avehicle that does not have right of way with respect to another vehicleshould not be expected to stop if it has enough time to execute itsmaneuver with a large enough time margin to the other vehicles. Adefinition of what is meant by time margin, or time gap as it will becalled in this setting, is therefore needed. In all scenarios it is assumedthat the courses either merge with or cross each other as the first collisionzone is entered. The first part of this region is also assumed to be themost dangerous and prone to accidents since one of the vehicles shouldhave right of way while the other is expected to yield. This first partof the region will be referred to as the critical collision zone. Once avehicle enters this critical zone it is in general in danger of a collisionwith vehicles on the other course. In our case a vehicle must travel atleast one car length in order for another vehicle on the other course toenter the collision zone without accident. It will therefore be importantto study not only the time until a vehicle reaches the critical collisionzone, but also the time until it leaves the critical collision zone so thatanother vehicle can enter safely.

A precise definition of the critical collision zones will not be provided.Instead we will provide regions that approximate these zones. Given apair of courses with parameters si and sj and the collision zone CZi,j,the approximation is given by

(si, sj) : Si,1 ≤ si ≤ Si,2 and Sj,1 ≤ sj ≤ Sj,2, (3.32)

where

Si,1 = min(si,sj)∈CZi,j

si, where sj is free,

Si,2 = Si,1 + L+W

2,

Sj,1 = min(si,sj)∈CZi,j

sj, where Si,1 ≤ si ≤ Si,2,

Sj,2 = Sj,1 + L+W

2.

(3.33)

For each pair of courses there might be multiple regions that can beconsidered critical. An example of this can be seen in Figure 4.7c. It isalso possible that the courses never merge or cross, in which case therewill be no critical collision zone. The zone provided in Equation 3.32 willonly approximate the first critical collision zone entered by the vehicle

38

CHAPTER 3. METHOD

following course ci, provided that the zone exists. By doing this, amaximum of two critical collision zones can be handled for each pair(ci, cj). The first zone will be computed for (ci, cj) and the other for(cj, ci). This is sufficient for the two case studies we consider in thisthesis, but an improvement would of course be to consider all possiblecritical collision zones.

Within a particle and for each pair of vehicles, the time gap betweenthe vehicles at their respective collision zone is computed. The time gapbetween two vehicles (k, l) within a particle i is defined as

TGi(k, l) = max(TGi(k, l)+, 0) + min(TGi(k, l)−, 0), (3.34)

whereTGi(k, l)+ = TTCCZi(l, k)− TOCCZi(k, l), (3.35)

andTGi(k, l)− = TOCCZi(l, k)− TTCCZi(k, l). (3.36)

TTCCZi(k, l) is a function that calculates the time to the critical colli-sion zone for vehicle k with respect to l and within particle i using themodified IDM model. TOCCZi(k, l) calculates the time until vehiclek would leave the critical collision zone using the modified IDM modeland with respect to vehicle l within the particle.

In the articles [13] and [15], the vehicles are propagated forward orbackward using a constant speed model until they reach the collisionzone (in their case the intersection) to find which vehicle is most likelyto make the vehicle of interest stop. For each other vehicle, a time tocollision zone is calculated as well as a time gap between the vehiclesat the collision zone. Since the particles carry an uncertainty in speedbefore the propagation, the variance in position will increase with theprediction time horizon. This behaviour is desirable since a larger un-certainty is expected for predictions that look at the vehicle state a longtime into the future compared to shorter term predictions. The down-side to this method is that it might be unreasonable for the vehicle totravel at this constant speed in the future. It could be the case thatthe driver is approaching a sharp turn and would need to adjust thespeed due to this road geometry. In this case the calculated time tocollision zone would be misleading and could therefore result in an over-or underestimation in risk.

What one would like to do is to use the speed profile that driverstypically follow along each course and use this information to get a moreaccurate estimate of the time to the collision zone. For our implementa-tion, we therefore use typical speed profiles for each course and use the

39

CHAPTER 3. METHOD

IDM model to propagate the particles along the course, as described insections 3.4 and 3.5. This introduces another small problem however,namely that the prediction variance does not increase with time as inthe constant speed model. When using the IDM model all particles willeventually converge to have the same speed as the desired speed profileand the variance in the predicted position will not increase after thispoint in time. In our case this turned out to not be a significant prob-lem since the model does not make collision predictions too far into thefuture.

3.10 TTC and TTCCP

The TTC is, as mentioned in section 1.2, the time remaining until theego-vehicle collides with another vehicle. Each vehicle is in each particleand time step assigned to a course, has some position, speed, headingand an intention to either stop or go. With these given, it is possibleto predict the future state and calculate the TTC. If the intention of avehicle, other than the ego-vehicle, is to stop and the vehicle has notdriven past the stop line of its course, it is assumed that it will notcollide with other vehicles.

The TTC is a deterministic measure and will therefore be definedfor each particle. In our implementation, we use the time gap construc-tion from the previous section to calculate the TTC. This allows a timemargin to be introduced so that two vehicles being too close may beconsidered a collision as well. By doing this the model will not allowvehicles to drive unreasonably close to each other. The TTC for particlei at time t will be denoted TTCi

t . By incorporating the aforementionedideas we end up with the following definition:

TTCit =

minT∈CT i

tT, if CT it 6= ∅,

∞, if CT it = ∅,(3.37)

where the set CT it contains all times T such that for particle i at timet there is at least one collision predicted for the ego-vehicle at or beforetime t+ T .

CT it =T : (∃k 6= ego)

[ ∣∣TGi(ego, k)∣∣ ≤ Tm ∧ TTCi,k

t ≤ T

∧ (Isk,it = go ∨ vehicle k is past its stop line)].

(3.38)

Here Tm denotes the time margin, ego is the index for the ego-vehicleand TTCi,k

t is defined as

TTCi,kt = max

(TTCCZi(ego, k), TTCCZi(k, ego)

). (3.39)

40

CHAPTER 3. METHOD

Another metric used for risk estimation is the time to critical collisionprobability (TTCCP) as introduced in [17]. We will define the TTCCPin a similar fashion but with our setting in mind. The TTCCP is thetime until the total probability of collision exceeds some threshold valuecalled the Critical Collision Probability (CCP). For the ego-vehicle attime t using a prediction horizon Tp, the TTCCP is given by

TTCCP =

min(T : p(C(t+ T )) ≥ CCP ), if T ≤ Tp,

∞, otherwise,(3.40)

where the random variable C(t) takes the value 1 if the ego-vehicle iscolliding with another vehicle at time t and 0 otherwise.

In our setting the total probability of collision within a predictiontime horizon can be computed by counting and weighting the particles.When considering the ego-vehicle at time t, the probability of collisioncan be approximated using the particles as

p(C(t+ T )) 'N∑i=1

ωit1TTCit≤T, (3.41)

where TTCit denotes the time to collision for particle i at time t as

defined in Equation 3.37 and the sum is over the N particles.

3.11 Pre-Processing

One of the most important steps of the algorithm is the pre-processing.The purpose of the pre-processing step is to eliminate as much processingtime during run time as possible in order for the risk assessment to beperformed in real time. In this step, data structures for the courses areset up so that the information can be extracted quickly. These structuresrepresent for instance the trajectories γ, speed profiles u and headingsalong the courses. It is also in this step that the right of way matrix isset up.

There are many instances where we request the desired speed atarbitrary points along the course. To retrieve the data for arbitrary arc-lengths we use the Matlab function griddedInterpolant to performlinear interpolation. The same is true for the functions TTCCZ andTOCCZ which is used to compute the time to critical collision zone fortwo vehicles within a particle. These times depend directly on initialpositions of the two vehicles and their respective speeds. In the pre-processing step, both arc-lengths and speeds are discretized and the

41

CHAPTER 3. METHOD

time to the critical collision zone is computed for all combinations ofpositions and speeds for the course pairs. The pre-computed times aresaved and linearly interpolated in the same manner for fast extractionlater.

3.12 Model Summary

To summarize the model, we present a visual representation of the al-gorithm structure. The structure for the complete model is shown inFigure 3.8. The arrows in the figure show the flow of data. In the firststep, sensor data is used to sample the initial particles. The particles arethen propagated using the procedure with steps as shown in Figure 3.9.Sensor data is then used for re-weighting the particles and the risk iscomputed using the new particles and weights. Finally the particles areresampled before entering the loop to be propagated once again. Thiscontinues until the scenario is over.

A summary of the pre-processing step can be seen in Figure 3.10.

42

CHAPTER

3.METHOD

Sensor data

Map data

Prepocessing

Resample particles

No

YesTerminated? EndStart

Sample particles

from prior and set

weigths

Prior knowledge

of driving

behaviour

Compute risk

estimation online

Propogate

particles

Reweight

particles

Figure 3.8: An overview of the complete algorithm.

43

CHAPTER

3.METHOD

Update position

using constant

velocity

Project vehicles

positions onto

assigned course

Sample expected

behaviours using a

gap acceptance model

Sample intended behaviours,

positions, headings and speeds

according to the models transition

probabilities

Figure 3.9: An overview of the particle propagation procedure.

Preparation ofcourses and speed

profiles

Define right ofway matrix

Calculate collisionzones

Calculate times tocollision zone

Figure 3.10: An overview of the pre-processing procedure.

44

Chapter 4

Case Studies

In this chapter we present the two scenes in which the model was tested.The first is a 4-way intersection with 12 different courses defined. Theother scene presented is a roundabout from the test track at Scania with4 courses considered.

4.1 4-way Intersection

The first scene is the same as the running example seen throughoutthis thesis. We consider the 4-way intersection as seen in Figure 3.2with courses set to either turn left, continue straight or turn right, whenapproaching from either west, east, north or south. Vehicles on thevertical road do not have right of way with respect to vehicles on themain road and should therefore give way at the intersection.

To get a better understanding of the speed profiles and collisionzones, some examples will now be provided. A speed profile was definedas a function that should represent a typical speed along each course.In the 4-way intersection scene, the speed profiles used were identicalregardless of what direction the course is approaching from. The speedlimit was set to 15 m/s, although drivers should adapt to the road ge-ometry. This means that if the intention is to go, then a typical driveris expected to drive at around 15 m/s, except when performing a turnas the desired speed should then be lower. For intention to stop, allspeed profiles start to brake about 50 meters before the stop line. Afterthe stop, the remaining part of the profile is generated using the modi-fied IDM model with zero initial speed. The speed profiles for the twodifferent longitudinal intentions can be seen in Figure 4.1.

45

CHAPTER 4. CASE STUDIES

-100 -50 0 50 1000

5

10

15

left turn

right turn

straight

(a) Intention to go.

-100 -50 0 50 1000

5

10

15

left turn

right turn

straight

(b) Intention to stop.

Figure 4.1: The speed profiles used in the 4-way intersection scene forthe two different longitudinal intentions as a function of distance to thestop line. A positive distance to the stop line indicates a position thatis past the stop line in the direction of the course.

Collision zones were introduced in section 3.8. Given the 12 courseswe can now visualize the zones by plotting the collision matrices. Tounderstand this better, two different examples of collision zones andcritical collision zones can be seen in Figure 4.3. The pairs of coursesused in these two examples are shown in Figure 4.2. The vehicle widthswere set to 1.8 m and the lengths were set to 4.7 m.

46


s = 0.5 s = 1

s = 0.5

s = 0

s = 0

s = 1

(a)

s = 0.5 s = 1

s = 0.5

s = 0

s = 0

(b)

Figure 4.2: The pairs of courses used in the examples shown in Fig-ure 4.3. Figure 4.2a shows the first pair and Figure 4.2b shows thesecond pair. The blue courses are referred to as the ego courses andthe red courses are referred to as the other courses. The normalized arclengths are shown for some positions along each trajectory.

47


0 0.2 0.4 0.6 0.8 1

0

0.2

0.4

0.6

0.8

1

(a) Collision zone (white).

0 0.2 0.4 0.6 0.8 1

0

0.2

0.4

0.6

0.8

1

(b) Critical collision zone (red).

0 0.2 0.4 0.6 0.8 1

0

0.2

0.4

0.6

0.8

1

(c) Collision zone (white).

0 0.2 0.4 0.6 0.8 1

0

0.2

0.4

0.6

0.8

1

(d) Critical collision zone (red).

Figure 4.3: Two examples of collision zones and critical collision zones.The axes represent arc lengths while the color white represents the col-lision zone. Black indicates that there is no collision and the criticalcollision zone is shown in red. Plots (a) and (b) show the collisionzone and critical collision zone for the intention pair Approaching fromsouth and continuing straight and Approaching from west and continuingstraight, shown in Figure 4.2a. Plots (c) and (d) show the collision zoneand critical collision zone for the intention pair Approaching from southand turning right and Approaching from west and continuing straight,shown in Figure 4.2b.

48


4.2 Roundabout

The test track roundabout can be seen in Figure 4.4 together with thefour courses that were defined for it. The names of the courses will reflectthe direction from which the courses are approaching and what type ofmaneuver is being performed. The blue courses in the figure approachfrom southeast while the red courses approach from northwest. Theyare also divided into either continuing straight through the roundabout,or performing a U-turn.

Figure 4.4: The roundabout at the test track. Four different courseswere defined.

As in the 4-way intersection we have plotted the speed profiles andsome collision zone examples. These can be seen in Figures 4.5 and 4.7respectively. The pairs of courses used in the two examples of collisionzones are plotted in Figure 4.6.

49


-50 0 50 100 1500

2

4

6

8

10

12

Straight SE

U-turn SE


-50 0 50 100 1500

2

4

6

8

10

12

Straight SE

U-turn SE


-50 0 50 100 1500

2

4

6

8

10

12

Straight NW

U-turn NW

(c) Intention to go.

-50 0 50 100 1500

2

4

6

8

10

12

Straight NW

U-turn NW

(d) Intention to stop.

Figure 4.5: The speed profiles used in the roundabout scene for the twodifferent longitudinal intentions as a function of distance to the stopline. A positive distance to the stop line indicates a position that is pastthe stop line in the direction of the course. The top graphs show theprofiles for the courses approaching from southeast while the bottomgraphs show the profiles for the courses approaching from northwest.

50


s = 0

s = 1

s = 0.5 s = 0.2

s = 0

(a)

s = 0

s = 1

s = 0.5 s = 0.18

s = 0

s = 0.6

s = 0.35

s = 1

(b)

Figure 4.6: The pair of courses used in the examples shown in Figure 4.7.Figure 4.6a shows the first pair and Figure 4.6b shows the second pair.The blue courses are referred to as the ego courses and the red coursesare referred to as the other courses. The normalized arc lengths areshown for some positions along each trajectory.

51


0 0.2 0.4 0.6 0.8 1

0

0.2

0.4

0.6

0.8

1

(a) Collision zone (white).

0 0.2 0.4 0.6 0.8 1

0

0.2

0.4

0.6

0.8

1

(b) Critical collision zone (red).

0 0.2 0.4 0.6 0.8 1

0

0.2

0.4

0.6

0.8

1

(c) Collision zone (white).

0 0.2 0.4 0.6 0.8 1

0

0.2

0.4

0.6

0.8

1

(d) Critical collision zone (red).

Figure 4.7: Two examples of collision zones and critical collision zones.The axes represent arc lengths while the color white represents the col-lision zone. Black indicates that there is no collision and the criticalcollision zone is shown in red. Plots (a) and (b) show the collisionzone and critical collision zone for the intention pair Approaching fromsoutheast and continuing straight and Approaching from northwest andmaking a U-turn, shown in Figure 4.6a. Plots (c) and (d) show the col-lision zone and critical collision zone for the intention pair Approachingfrom southeast and making a U-turn and Approaching from northwestand making a U-turn, shown in Figure 4.6b.

52

Chapter 5

Main Results

In this chapter we present the results from some simulations and testson log data from the test track. Each scenario is presented using afigure, each containing four rows. The first row shows an overhead viewof the scene at some chosen time points. The second row shows thecourse probabilities for the different vehicles. The third row displays theintended and expected behaviours together with the first measure of risk,that is, the probability of each vehicle violating the traffic rules. The lastrow shows the second risk measure, including one plot of the TTCCPand one plot of the total probability of collision for the ego-vehicle withinthe time horizon. The leftmost plot in rows 2 and 3 correspond to theego-vehicle which is then followed by the other vehicles. Some referencetimestamps are also added in the plots and the positions of the vehiclescan for these times be seen in the overhead view.

Section 5.1 shows the results from simulated data, four scenarios inthe 4-way intersection and three scenarios in the roundabout. Section5.2 shows the results from log data gathered in the roundabout at thetest track.

5.1 Simulations

All simulations use 8000 particles and the simulated measurement dataarrives at a fixed frequency of 20 Hz. Looking at subsection 5.1.8 itbecomes apparent that this is too many particles for real-time filteringon our machine. However, the results for real-time filtering is compara-ble to the results when using 8000 particles so this amount is used forconsistency and clarity.

53

CHAPTER 5. MAIN RESULTS

Automated Driving Toolbox

To design and simulate traffic scenes, we used the Automated Driv-ing Toolbox in Matlab. This toolbox contains among other interestingfunctionality a driving scenario designer in which one quickly can put to-gether a traffic scenario and add actors such as cars, trucks, pedestriansand more. There is also the possibility of adding sensors such as radarsand cameras to the ego-vehicle for data collection. The parameters forthese sensors can be tweaked by for example changing the noise and therate of false positives. However, we instead opted to add Gaussian noiseto the ground truth output from the simulation and used this as ourmeasurements, since the functionality of the built in sensor fusion wasnot satisfactory.

To prepare for tests on real data from logs in the roundabout at thetest track we decided to import map data of this roundabout into theAutomated driving toolbox. This was done so that initial simulationscould be run to check the model’s performance in such scenarios. Thisalso provided us with some ideas of what scenarios we wanted to focuson while at the test track.

Parameter Specification

In Chapter 3, multiple parameters were introduced in the transitiondensities. Next follows a table with a complete specification of whatparameter values were used to produce the simulation results.

54


Parameter Valueε 0.2

1−|C|Pcomply 0.9

ΣL

(0.3 00 0.3

)σ2S 1σ2θ 0.01

ΣLm

(0.4 00 0.4

)σ2Sm 2σ2θm 0.02

a (IDM) 2b (IDM) 2δ (IDM) 4

N (# of particles) 8000Tm (time margin) 0.2

CCP 0.3

Table 5.1: The parameter values used when running the algorithm onthe simulated scenarios.

55


5.1.1 Scenario 1

The first scenario we will have a look at is shown in Figure 5.1. The ego-vehicle is approaching the intersection from west on the main road andis heading straight. On the other road, another vehicle approaches theintersection from south and is set to give way before continuing straight.

From the course probabilities it is apparent that the ego-vehicle hasbeen identified as approaching from west with similar probabilities onthose three courses, at time t1. It is not until about 50 m before the inter-section that the course straight is distinguished from the other coursessince the speed profiles for the two turns start slowing down at thatpoint. The model has a harder time identifying the course for the othervehicle. The other vehicle is correctly assigned to be approaching fromsouth, however since the vehicle is slowing down to a stop it is essentiallyimpossible to distinguish where the car is heading after the stop.

For the intended and expected behaviours we see that the ego-vehicleis never expected to stop throughout the scene, simply because it hasright of way and thus should not stop. The intention to stop or havestopped (after the intersection) is also low because the vehicle is driv-ing at a high speed. The other vehicle on the other hand becomesexpected to stop, especially as soon as the ego-vehicle is identified asgoing straight. In this scenario the other vehicle was set to slow downand the model manages to identify this as a high intention to stop. Theresulting risk R(t) is therefore low in this case.

Lastly, the total collision probability for the ego-vehicle did not ex-ceed the critical threshold and there was no TTCCP detected.

56


Figure 5.1: Scenario 1. The first row show the position of all vehiclesat times t1, t2 and t3. The estimated intended courses is shown in thesecond row. The estimated intention to stop and expectation to stop isshown in the third row together with the risk induced on the scene byeach vehicle. For these two rows the first column shows the blue vehicle(ego-vehicle) and the second column shows the red vehicle. The TTCCPand the collision probability for the ego-vehicle is shown in the fourthrow.

57


5.1.2 Scenario 2

The next scenario is shown in Figure 5.2 and is very similar to the firstone, except this time the other vehicle is set to not stop.

This time the course probabilities behave similarly for both vehi-cles. The correct direction is first identified and the correct course isdistinguished as soon as the speed profiles starts to differ, around timet1.

The intended/expected behaviours for the ego-vehicle were essen-tially the same as in the previous scenario. The other vehicle againbecomes expected to stop when approaching the intersection, howeverthis time the intention to stop decreases as it gets closer to the intersec-tion since it is not slowing down. The resulting risk R(t) is consequentlyhigh in this case, meaning that there is a high probability that the othercar will violate the traffic rules by not giving way to the ego-vehicle.The collision takes place at time t2.

The total collision probability for the ego-vehicle can be seen to in-crease as the intersection is approached. The model identifies the riskas soon as the critical probability threshold of 0.3 is exceeded, whichhappens to be about 2 seconds before the collision.

58


Figure 5.2: Scenario 2. The figure consists of the same kind of plots asFigure 5.1.

59


5.1.3 Scenario 3

The next scenario is also considered risky and can be seen in Figure 5.3.The ego-vehicle still drives straight on the main road and approachesfrom west. The other vehicle is in this case approaching from east andperforms a risky left turn without considering the oncoming ego-vehicle.

The course probabilities for the ego-vehicle look almost identical tothe two previous scenarios here. For the other vehicle we see that, sincethe vehicle has slowed down, the model is very certain that the car willturn either left or right when approaching from east. The probability ofleft turn for the other vehicle increases dramatically after time t2 whenthe maneuver is being performed and because of this it also becomesexpected to stop since it should wait for the ego-vehicle to pass. Inthis case, the car does not intend to stop and the resulting risk R(t) istherefore high.

This can also be observed in the TTCCP measure where the collisionprobability increases quickly as soon as the left turn is identified. Thecollision risk is detected about 1.8 seconds in advance.

Comment: Between the time points t1 and t2, the model assigns ahigher probability for the red car to turn right. The reason why the twoturns are not equally treated here by the model is due to the interaction-awareness. In this case, the red car does not have the intention to stopwhen performing its left turn. If we imagine that we were the driver ofthe ego-vehicle (blue) and we observe the red car’s behaviour, we wouldmost likely think that the red car intends to turn right, since if it wouldturn left it should have slowed down more in order to give way to us.

60



61


5.1.4 Scenario 4

The last scenario in the intersection is presented in Figure 5.4. Thisscenario showcases a non-risky scene with 3 vehicles. The ego-vehicleis approaching from east and turning left, another vehicle approachesfrom north and turns left and a third vehicle approaches from west andcontinues straight. In this case, the ego-vehicle should wait for the thirdvehicle to pass before performing its left turn. After the ego-vehicle’sturn, the second vehicle can proceed with its action. To make the scenenon-risky, these were also the prescribed behaviours.

In the figure we see that the third vehicle is correctly assigned tocontinue straight since it is keeping its high speed. The ego-vehicle’sleft turn is correctly identified after time t2, which is a couple of secondsbefore the action is initiated at time t3. The second vehicle slows downto stop and therefore it is again hard to tell where it will continue lateron, similar to the red vehicle in scenario 1.

The third vehicle can be seen as not expected to stop throughoutthe scene. Both the ego-vehicle and the second vehicle are identified asexpected to stop in this case. The model also manages to realize thatboth of these vehicles have an intention to stop and the resulting riskR(t) is low.

The collision probability exceeds the threshold of 0.3 at multipleinstances and gives readings of between 2 to 4 seconds in the TTCCP.

Comment: Looking at the course probabilities for the blue ego-vehiclewe notice that the left turn is the most probable course between t2 andt3. What happens here is that the ego-vehicle slows down and waitsfor the yellow car to pass in order to perform a safe left turn. Themodel understands that the blue vehicle has a high intention to stopand therefore infers that the vehicle is most likely to turn left. If theblue vehicle’s intention was to continue straight or turn right in thiscase, then there would be no need to actually stop to let the yellow carpass first, that is, the intention would be to go.

Furthermore, the model does identify a TTCCP after running thescene for roughly 3 to 5 seconds. The TTCCP here decreased from 4seconds down to 2 seconds and corresponds to a collision between theego-vehicle and the yellow vehicle. This decrease is reasonable since theTTCCP represents the time to collision if the ego-vehicle would havethe intention to go. However, the ego-vehicle was in this scene designedto stop and wait for the yellow car to pass in order to avoid the collision.From the figure we see that the inferred intention for the ego-vehicle isto stop around this time interval.

62


Figure 5.4: Scenario 4. The figure consists of the same kind of plotsas Figure 5.1. For the two middle rows, where the estimated expectedbehaviour and the risk measure R(t) is plotted, the first column showsthe blue vehicle (ego-vehicle), the second row shows the red vehicle andthe third row shows the yellow vehicle.

63


5.1.5 Scenario 5

Next, we move on to the roundabout scenarios with the first one shownin Figure 5.5. This is a scene designed to be non-risky. The ego-vehicleis driving through the roundabout while another vehicle approaches theroundabout from southeast and is set to give way to the ego-vehicle.

For the course probabilities we see that they are more or less uni-formly distributed over the courses available at a certain position. Attime t1 it is, for the ego-vehicle, impossible to distinguish between threecourses due to their similar speed profiles. At time t2 the most probablecourse was northwest U-turn since this is the only course close to thevehicle’s position at that time.

The intended/expected behaviours show that the other vehicle be-comes expected to stop as it gets closer to the roundabout. This coin-cides with the inferred intention of the other vehicle so the probabilityof not yielding to the ego-vehicle is therefore low.

The total collision probability did not exceed the critical thresholdeither, so there was no risk detected in the TTCCP.

64



65


5.1.6 Scenario 6

This scenario is similar to the previous one, but this time the othervehicle does not stop when entering the roundabout. The results can beseen in Figure 5.6.

The resulting course probabilities are almost identical to what wasobserved in the previous scenario. However, this time the model cor-rectly classified the other vehicle’s intention as not to stop. The dissim-ilarity in expected behaviour and intended behaviour then resulted in ahigh risk R(t).

For the TTCCP there is a similar outcome. As soon as the modelrealizes that the other car is not stopping when it is expected to, theego-vehicle’s collision probability increases dramatically and manages toidentify the risk approximately 1.5 seconds before the collision.

66



67


5.1.7 Scenario 7

The last simulated scenario we will have a look at is a risky one with3 vehicles in the roundabout as seen in Figure 5.7. The ego-vehicle isin this case driving behind another vehicle through the roundabout. Athird car is approaching from southeast and gives way to the first car inthe roundabout, but not the ego-vehicle.

The course probabilities again look similar to what was observed inthe previous scenarios. Most interesting is the plot of intended/expectedbehaviour for the car approaching from southeast (middle in the figure).It is apparent that this car is expected to stop and also has an intentionto do so up until the time t2. At this time, the vehicle entering theroundabout is still expected to stop since it should give way to the ego-vehicle, but according to the model the car does not have the intentionto stop. This results in a high risk R(t).

The ego-vehicle’s collision probability remains low up until time t2when this shift in intention of the car entering occurs. In this case therisk was identified and resulted in a TTCCP of about 1 second.

Comment: The model can here be seen to adapt very fast to thedynamic environment. We notice how the model quickly captures thechange in intention of the red car at time t2. The immediate risk wasin this case perceived very quickly by the model and resulted in anidentified TTCCP.

68


Figure 5.7: Scenario 7. The figure consists of the same kind of plotsas Figure 5.1. For the two middle rows, where the estimated expectedbehaviour and the risk measure R(t) is plotted, the first column showsthe blue vehicle (ego-vehicle), the second row shows the red vehicle andthe third row shows the yellow vehicle.

69


5.1.8 Performance

Instead of presenting the run time using 8000 particles, we will presentthe maximum number of particles that can be used for real-time filteringin the presented scenarios. The algorithm was implemented in Matlaband the runs were done on a HP ZBook Studio G5 with an Intel Corei7-8850H CPU and 16 GB of RAM. The result is shown in Table 5.2 forall scenarios.

number of particlesScenario 1 2800Scenario 2 2800Scenario 3 2800Scenario 4 1300Scenario 5 5300Scenario 6 5300Scenario 7 3700

Table 5.2: Maximum number of particles for real-time filtering in thepresented scenarios.

5.2 Test on Log Data

In order to test the model in practice, some real data from the round-about, described in section 4.2, was collected during 2 hours of drivingon the test track at Scania. The vehicles used were the two busses Klasseand Klara as well as the truck Copper and the data was passed betweenthe vehicles using V2X communication. Pictures of Klasse and Coppercan be seen in Figure 5.8. Klasse was set as the ego-vehicle during thetest-drive. The data of the ego-vehicle was recorded at 100 Hz whilethe data of the other two vehicles arrived at 20 Hz. The data was thenpre-processed through linear interpolation and resampling to achieve aconstant update frequency of 20 Hz for all vehicles.

These vehicles are significantly larger than cars and the collisionzones will therefore be wider in this case compared to when cars are con-sidered. When computing the collision zones we set the vehicle lengthsto 12.99 m and the widths to 2.55 m.

70


Figure 5.8: Two of the vehicles used at the test track. The left pictureshows the bus Klasse and the right picture shows the truck Copper.Klara, which is not shown here, is a red bus very similar to Klasse.

Using the data gathered at the test track we adjusted the speedprofiles to better conform with the observed driving behaviour in theroundabout. These adjusted speed profiles are shown in Figure 5.9.The hyperparameters were also adjusted slightly to work better withthe real data and these can be seen in Table 5.3.

The results presented in this section will have a similar layout to thesimulation results. The overhead view will show the ego-vehicle (Klasse)in blue while Klara is shown in red and Copper is shown in yellow.

71


-50 0 50 100 150 2000

5

10

15 Straight SE

U-turn SE


-50 0 50 100 150 2000

5

10

15 Straight SE

U-turn SE


-50 0 50 100 150 2000

5

10

15 Straight NW

U-turn NW

(c) Intention to go.

-50 0 50 100 150 2000

5

10

15 Straight NW

U-turn NW

(d) Intention to stop.

Figure 5.9: The speed profiles found from the log data gathered. Apositive distance to the stop line indicates a position that is past thestop line in the direction of the course. The top graphs show the profilesfor the courses approaching from southeast while the bottom graphsshow the profiles for the courses approaching from northwest.

72


Parameter Specification

Parameter Valueε 0.2

1−|C|Pcomply 0.9

ΣL

(0.3 00 0.3

)σ2S 1σ2θ 0.01

ΣLm

(0.8 00 0.8

)σ2Sm 2σ2θm 0.04

a (IDM) 0.7b (IDM) 2δ (IDM) 4

N (# of particles) 8000Tm (time margin) 0.2

CCP 0.3

Table 5.3: The parameter values used when running the algorithm onthe real data from the test track.

73


5.2.1 Scenario 8

We will start by having a look at a non-risky scenario. In this scenario,Klasse is waiting to enter the roundabout while Klara and Copper aredriving through the roundabout. Klara first leaves the roundabout andKlasse waits until Copper has passed. The results are shown in Fig-ure 5.10.

For the course probabilities we observe similar results to what wasseen in the simulations. Looking at the intentions we see that Klasse iscorrectly identified as having a high intention to stop since it gives wayto Copper in this case. It is also expected to stop before entering theroundabout and the risk R(t) remains low throughout the scenario.

In the collision probability we see two peaks, one corresponding toa collision with Klara and the other to a collision with Copper. Again,the collision probability provides a TTCCP given that the ego-vehiclewould have the intention to go. In this case, the inferred intention forKlasse is to stop, so there would be no reason to trigger a warning here.The TTCCP decreases from roughly 5 seconds down to 2.5 secondsbefore the collision probability falls below the critical threshold. Afterthis Klasse can safely change its intention to go and start driving sinceCopper would have enough time to pass.

Comment: The result from this scenario tells us that it would be abad idea for the ego-vehicle Klasse to start driving when a TTCCP isidentified since this would most likely result in a collision. In this case,Klasse has a high intention to stop at the times where a TTCCP isobtained. If one would design a warning system based on this model itwould therefore not be necessary to issue a warning in this scenario dueto Klasse’s intention being to stop.

74


Figure 5.10: Scenario 8. The figure consists of the same kind of plotsas Figure 5.1. In the overhead view, the ego-vehicle Klasse is shown inblue, Klara is shown in red and Copper is shown in yellow.

75


5.2.2 Scenario 9

Some attempts were made to create a risky scenario and we will heretake a look at one such scene. In this scenario, Klara is driving throughthe roundabout and Klasse enters the roundabout from southeast a bitin front of Klara. The result is shown in Figure 5.11.

In the course probabilities, we observe a similar behaviour to theother roundabout scenarios. The courses are hard to distinguish betweenwhere there are multiple similar ones available. The inferred intention forKlasse is to go shortly before entering the roundabout near the stop line.The expected behaviour for the ego-vehicle closely follows the probabilityof the red vehicle performing a U-turn, since the ego-vehicle should inthis case yield. As the probability of a U-turn for Klara increases, Klassealso becomes increasingly expected to stop. As this expected maneuverdoes not match the intention of Klasse, this results in a slight risk inthe measure R(t). The gap between the vehicles was however a bit toolarge in this scenario to give an effect on the collision probability for theego-vehicle, so there was no identified TTCCP.

Comment: We see that the risk R(t) after the stop line approximatelyfollows the value of the probability P (Es = stop) divided by 2. Thisreduction of the risk is seen since the probability of intention to stopis around 1/2, which is a result of the speed profiles for Is = go andIs = stop being similar quite soon after the stop line.

The fact that the previous intention to stop has an effect on therisk after the vehicle has passed the stop line is not desirable and this issomething that can easily be fixed by defining the risk R(t) in a differentway when a vehicle is past the stop line.

76


Figure 5.11: Scenario 9. The figure consists of the same kind of plots asFigure 5.1. The ego-vehicle Klasse is shown in blue, Klara is shown inred.

77


5.2.3 Scenario 10

Unfortunately we were not allowed to crash Scania’s busses which madeit hard to create a truly risky scenario. We have, therefore, for thislast scenario used the log from scenario 9 and modified the recordedtimestamps to make the time gap between the two vehicles much smaller.The result is shown in Figure 5.12.

As before, there is no surprise in the resulting course probabilitiesand the intentions look almost identical to the previous scenario. Thistime however, the time gap is small enough to give a drastic increasein the collision probability. A TTCCP is identified a full 5 secondsin advance. There is a slight decrease in the collision probability thatresults in a drop below the critical threshold and the TTCCP thereforedisappears in this region.

Comment: The reason to why the collision probability drops below thecritical threshold after time t1 can be seen in the course probability forKlara. The probability for the course northwest U-turn can be seen todecrease in this region. Since this is the only course for Klara that Klasseshould yield to the collision probability consequently also decreases.

The TTCCP can also be seen to reach 0 seconds even though acollision never actually takes place. This is due to our definition of timegaps where we also require a 0.2 second time margin to the other vehicle.Klara is in this case simply driving too close to Klasse.

Again we see the same reduction of the risk R(t) as was pointed outin scenario 9.

78


Figure 5.12: Scenario 10. The figure consists of the same kind of plotsas Figure 5.1. The ego-vehicle Klasse is shown in blue, Klara is shownin red.

79


5.3 Discussion

Regarding the results

Overall, the assessed risk seems reasonable in all of the presented sce-narios and collisions were identified shortly before occurring. For thesimulated scenarios in the intersection, the model was able to correctlyidentify the direction from which all the vehicles were approaching. Onething that the model struggled with was to identify where a vehicle wasgoing after making a stop at the stop line in the intersection. This resultis not that surprising since all courses coming from the same directionhave similar trajectories up to the stop line. In this case, one must sim-ply wait until the vehicle starts executing its maneuver in order to figureout what will happen. The scenarios that were designed to be risky wereall identified as such and the model was able to capture these events witha small time margin. Some effects of the model’s interaction-awarenesswas also observed in scenarios 3 and 4.

In the roundabout scenarios, a common observation was that thecourse probabilities became roughly uniform on the courses present ateach position. Take for example the course probabilities of the ego-vehicle in scenario 5 at time t1. At this point, we have approximatelyequal probabilities for the courses southeast U-turn, northwest straightand northwest U-turn. This could be interpreted as a probability of 1/3for the blue vehicle to continue driving through the roundabout anda probability 2/3 of it leaving the roundabout. Of course, this is nottrue and could result in a misleading risk estimation. The two coursessoutheast U-turn and northwest straight are essentially equivalent in thiscase since the vehicle would in both cases leave the roundabout. Whatwould be needed here is some way of grouping such equivalent coursesso that the risk assessment does not depend on the choice of courses,resulting in an even split in probabilities between groups of equivalentcourses. In the case mentioned above, the probability should be 1/2 forthe blue vehicle leaving the roundabout as well as for remaining in theroundabout.

In both of the risky roundabout scenarios 6 and 7, the model man-aged to identify the risk while in scenario 5 the model did not trigger afalse alarm.

Lastly, in the roundabout scenarios from the log data, we observedsimilar results to what was seen in the simulations. The problem ofuniform course probabilities was still an issue, however, this did notprevent the model from identifying the risk. The biggest problem wasthat we were not able to create some true risky scenarios since we had

80


to keep a margin between the vehicles at the test track. Fortunately,the gathered data could be used to create a risky scenario by shiftingthe vehicles in time as done in scenario 10. In this scenario, the modelmanaged to correctly identify the risk a few seconds before the event.

General comments

An important aspect in risk assessment is the computational complexity.Interaction-aware models are known for being computationally heavyand therefore in most cases not practical in real-world applications. Thecomplexity for the model implemented in this work increases with thenumber of courses present in the scene. For this model to work in prac-tice on a larger map, one would have to divide this map into smallerregions so that the particle filtering is limited to a smaller subset ofpossible courses in each time step. The number of courses that can beput in each region mostly depends on the number of particles used inthe filtering. Since the vehicles should be able to move between differ-ent regions, it would also be necessary to define transitions between thecourses in different regions. In this model, defining transitions betweenthe courses belonging to adjacent regions would suffice. This would en-sure that the vehicles are able to transfer into an adjacent region in eachtime step during the particle filtering.

The main advantage of the model is its inherent flexibility that stemsfrom the DBN construction. In theory, adding or removing a vehiclethat enters or leaves a region should be rather straight forward. Thehidden variables could for a vehicle that enters the scene be generatedfrom the prior distribution as described in section 3.6. Each particle isthen extended to also include the new vehicle and the particle filteringcontinues as usual. To ensure stability it could be a good idea to let theentering vehicle’s hidden variables run through a short burn-in so thatthe vehicle does not affect the other vehicles while its hidden variablesare highly influenced by the prior distribution. A vehicle that leaves thescene would instead be deleted from the particles and will not affect theexpected maneuver of other vehicles any longer.

Another advantage to the proposed model is that the maneuvers arelimited to a small and relevant set of pre-defined behaviours for thegiven scene. This reduces the complexity significantly and results in aninteraction-aware model that can assess the collision risk in real time.These limitations will also come with some drawbacks. The model willfor example not be able to infer any other intentions than the specifiedones, meaning that the model will not be able to handle vehicles thatdeviate significantly from the predefined courses and/or speed profiles.

81


This in turn means that the courses and speed profiles that are definedshould be highly relevant in the sense that the vast majority of driversshould be close to at least one of these predefined intentions at all times.This step requires a lot of preparatory work, including the gatheringof data or studying typical behaviours given a road geometry as wasdescribed in section 3.5.

We will end this discussion by highlighting some general limitationsof the model as it stands. The studied scenarios are all relatively simplein that they do not contain much else than moving vehicles. Some otherexternal factors that have not been treated in this work are for examplepedestrians, occluded areas or car-following, which are all frequentlyencountered in the real world, especially in urban driving. The problemsthat arise due to these factors are often quite different to the scenariosstudied in this work. In the end, a fully autonomous vehicle shouldbe able to handle any kind of traffic scenario. The work towards fullautomation therefore consists of solving many small problems which arethen finally assembled into a unified model.

82

Chapter 6

Conclusion and Future Work

In this chapter we present our main findings and discuss some futurework for this model and regarding the topic in general.

6.1 Main Findings

The model studied in this work was based on the one in the articles [13]and [15]. The proposed measure of risk that the articles presented wasimplemented and tested. This measure was based on comparing drivers’intentions with their respective expected maneuver in order to identifysituations in which these did not agree with each other.

The articles’ authors did not consider what vehicles would do afterthey came to a stop. In the intersection case, a vehicle should intuitivelynot just stay at the stop line when a vehicle on the main road haspassed, that is, they should be expected to go again. Therefore, thespeed profiles for intention to stop were in this work adapted to allowvehicles to continue driving after coming to a stop.

Another key ingredient in the risk prediction was the introductionof collision zones. This enabled the computation of times to collisionand the assessment of risk for an ego-vehicle. The IDM model was alsoimplemented to improve the state prediction step and the calculation ofTTC. The model framework gives an estimate of each driver intentionand this can be incorporated in the prediction as well.

The collision zones were also used to improve the update of expectedbehaviour in both intersections and roundabouts. In fact, they are es-sential for use in roundabouts since collisions can then occur at morethan one place for a given course pair.

The simulations in the different scenarios show that the risk is iden-tified a couple of seconds before a collision occurs. This is the case in

83

CHAPTER 6. CONCLUSION AND FUTURE WORK

the scenarios specifically designed to be risky, while the non-risky sce-narios did not trigger false positives. The model’s behaviour coincideswell with how a typical driver would assess the risk in these scenarioswhich of course is an important feature for usage in autonomous drivingsystems.

6.2 Future Work

Even though the results look promising for an interaction-aware modelof this kind, there is still much work left to improve upon the model.One such improvement would be to implement a method for groupingthe courses. At the moment the course probabilities, and so also therisk assessment, are slightly dependent on the choice of courses. Thiswas discussed in section 5.3 for the roundabout scenarios and the issuecould result in an overestimated or underestimated collision risk. A suit-able solution would therefore be to group the courses so that equivalentcourses end up in the same group. One would then need to rework themodel’s transition probabilities, for instance by letting the particles havea higher probability of staying in the same course group. A potentialgrouping of courses would also simplify the implementation of treatingindicators since these are closely related to what group of courses shouldbe the most probable.

Another thing for future consideration would be to look at a prac-tical implementation with significantly larger maps, also discussed insection 5.3. A lot of work would have to be aimed towards the gatheringof such map data in order to put together data structures for all courses,their speed profiles and the traffic rules. This work could perhaps bedone with some kind of learning algorithm, possibly using a combina-tion of clustering methods together with Gaussian process regression toextract suitable courses and speed profiles from the data.

Lastly, the model studied in this thesis could be compared to othermodels used in risk estimation through more extensive quantitative test-ing in the same scenarios.

84

Bibliography

[1] AASHTO GREEN BOOK (GDHS-7) - A Policy on Geometric De-sign of Highways and Streets, 7th Edition: AASHTO Green Book.The Green Book. American Association of State Highway andTransportation Officials, 2018. isbn: 9781560516767. url: https://books.google.se/books?id=MWHBDwAAQBAJ.

[2] Samer Ammoun and Fawzi Nashashibi. “Real time trajectory pre-diction for collision risk estimation between vehicles”. In: 2009IEEE 5th International Conference on Intelligent Computer Com-munication and Processing. IEEE. 2009, pp. 417–422.

[3] Adam Berthelot et al. “Handling uncertainties in criticality assess-ment”. In: 2011 IEEE Intelligent Vehicles Symposium (IV). IEEE.2011, pp. 571–576.

[4] Mattias Brannstrom, Erik Coelingh, and Jonas Sjoberg. “Model-based threat assessment for avoiding arbitrary vehicle collisions”.In: IEEE Transactions on Intelligent Transportation Systems 11.3(2010), pp. 658–669.

[5] John D’Errico. distance2curve. url: https://www.mathworks.com / matlabcentral / fileexchange / 34869 - distance2curve.(accessed: 19-February-2020).

[6] Vincenzo Gallelli, Rosolino Vaiana, and Teresa Iuele. “Compari-son between simulated and experimental crossing speed profiles onroundabout with different geometric features”. In: Procedia-socialand behavioral sciences 111.10 (2014), pp. 117–126.

[7] Tobias Gindele, Sebastian Brechtel, and Rudiger Dillmann. “Learn-ing context sensitive behavior models from observations for pre-dicting traffic situations”. In: 16th International IEEE Conferenceon Intelligent Transportation Systems (ITSC 2013). IEEE. 2013,pp. 1764–1771.

85

https://books.google.se/books?id=MWHBDwAAQBAJ

https://books.google.se/books?id=MWHBDwAAQBAJ

https://www.mathworks.com/matlabcentral/fileexchange/34869-distance2curve

https://www.mathworks.com/matlabcentral/fileexchange/34869-distance2curve

BIBLIOGRAPHY

[8] Yeping Hu, Wei Zhan, and Masayoshi Tomizuka. “A frameworkfor probabilistic generic traffic scene prediction”. In: 2018 21stInternational Conference on Intelligent Transportation Systems(ITSC). IEEE. 2018, pp. 2790–2796.

[9] Yeping Hu, Wei Zhan, and Masayoshi Tomizuka. “Probabilisticprediction of vehicle semantic intention and motion”. In: IEEE.2018, pp. 307–313.

[10] Marco Huber. Nonlinear Gaussian filtering: theory, algorithms,and applications. Vol. 19. KIT Scientific Publishing, 2015.

[11] Eugen Kafer et al. “Recognition of situation classes at road in-tersections”. In: 2010 IEEE International Conference on Roboticsand Automation. IEEE. 2010, pp. 3960–3965.

[12] Christian Laugier et al. “Collision Risk Assessment to ImproveDriving Safety”.

[13] Stephanie Lefevre, Christian Laugier, and Javier Ibanez-Guzman.“Risk assessment at road intersections: Comparing intention andexpectation”. In: 2012 IEEE Intelligent Vehicles Symposium. IEEE.2012, pp. 165–171.

[14] Stephanie Lefevre, Dizan Vasquez, and Christian Laugier. “A sur-vey on motion prediction and risk assessment for intelligent vehi-cles”. In: ROBOMECH journal 1.1 (2014), p. 1.

[15] Stephanie Lefevre et al. “Intention-aware risk estimation: Field re-sults”. In: 2015 IEEE International Workshop on Advanced Roboticsand its Social Impacts (ARSO). IEEE. 2015, pp. 1–8.

[16] Martin Liebner et al. “Driver intent inference at urban intersec-tions using the intelligent driver model”. In: 2012 IEEE IntelligentVehicles Symposium. IEEE. 2012, pp. 1162–1167.

[17] Matthias Schreier, Volker Willert, and Jurgen Adamy. “An In-tegrated Approach to Maneuver-Based Trajectory Prediction andCriticality Assessment in Arbitrary Road Environments”. In: IEEETransactions on Intelligent Transportation Systems 17.10 (2016),pp. 2751–2766.

[18] Jens Schulz et al. “Multiple Model Unscented Kalman Filteringin Dynamic Bayesian Networks for Intention Estimation and Tra-jectory Prediction”. In: 2018 21st International Conference on In-telligent Transportation Systems (ITSC). IEEE. 2018, pp. 1467–1474.

86

BIBLIOGRAPHY

[19] Ihab El-Shawarby et al. “Evaluation of driver deceleration be-havior at signalized intersections”. In: Transportation ResearchRecord: Journal of the Transportation Research Board 2018.1 (2007),pp. 29–35.

[20] Steven E Shladover et al. Gap acceptance for vehicles turning leftacross oncoming traffic: Implications for intersection decision sup-port. Tech. rep. 2006.

[21] Wikipedia contributors. Hyperplane separation theorem — Wikipedia,The Free Encyclopedia. url: https://en.wikipedia.org/w/index.php?title=Hyperplane_separation_theorem&oldid=

945175155. (accessed: 17-April-2020).

87

https://en.wikipedia.org/w/index.php?title=Hyperplane_separation_theorem&oldid=945175155



TRITA SCI-GRU 2020:070

www.kth.se

vehicle collision risk prediction using a dynamic bayesian ...1431653/fulltext02.pdf · jonas...

Documents