Probabilistic Collision Estimation System for Autonomous Vehicles: Evaluated in Intersection Scenarios Using a Velocity Planning Controller

(1)

IN

DEGREE PROJECT MECHANICAL ENGINEERING, SECOND CYCLE, 30 CREDITS

STOCKHOLM SWEDEN 2016,

Probabilistic Collision Estimation System for Autonomous Vehicles

Evaluated in Intersection Scenarios Using a Velocity Planning Controller

ALEXANDER GRATNER

STEFAN ANNELL

(2)

(3)

.

Master of Science Thesis MMK 2016:91 MDA 549 Probabilistic Collision Estimation System

for Autonomous Vehicles Alexander Gratner, Stefan Annell

Approved: Examiner: Supervisor:

June 12, 2016 De-Jiu Chen Lars Svensson

Commissioner: Contact Person:

ÅF, ITRL Tor Ericsson

.

Abstract

.Nearly 1.3 million people die each year in traffic-related accidents, whereas an addi- tional 20-50 million people are injured. Introducing autonomous vehicles into traffic would aim at reducing accidents and improving the transportation experience for humans. However, intersections are still difficult traffic scenarios for such systems due to their complex nature. Functions which could accurately foresee future events would improve autonomous systems when dealing with such complex traffic scenarios.

This thesis is carried out at ÅF AB, Stockholm, in collaboration with the Inte- grated Transport and Research Labs at KTH. The aim is to research how a collision prediction system could be implemented on the concept vehicle developed by the Transport Labs. This system would give the vehicle the ability to make decisions of estimated future events based on information gathered in real-time.

A conceptual collision prediction system is developed using the Robotics Op- erating System simulation environment. The simulations are conducted on T- intersection traffic scenarios where only the ego vehicle and another observed vehicle is present. The system has two outputs, 1) probability fields which indicates probable future positions of the observed car and 2) a summarized collision probability which shows how probable collisions are over the prediction horizon given a certain control input for the ego vehicle. The system performance is evaluated using a model predictive velocity controller which chooses the control input that gives the lowest value for the summarized collision probability. In the current state, both parts are too slow to work in real-time applications, but this is something that is achievable with the correct implementation. The results for system robustness and precision are promising and the vehicle successfully avoids collisions given that the sensor noise is low enough and it is shown that this is a promising system.

(4)

(5)

.

Examensarbete MMK 2016:91 MDA 549 Probabilistisk Kollisionestimeringssystem

för Autonoma Fordon Alexander Gratner, Stefan Annell

Godkänt: Examinator: Handledare:

June 12, 2016 De-Jiu Chen Lars Svensson

Uppdragsgivare: Kontaktperson:

ÅF, ITRL Tor Ericsson

.

Sammanfattning

.Nästan 1,3 miljoner människor dör varje år i trafikrelaterade olyckor, medan ytterli- gare 20-50 miljoner människor skadas. Införande av autonoma fordon i trafik skulle syfta till att minska antalet olyckor och förbättra körupplevelsen för människor.

Några av de mest problematiska trafiksituationerna för autonoma system är ko- rsningar, vilka skulle underlättas om förmågan att predikera framtida scenarion fanns i systemen.

Detta examensarbete utförs vid ÅF AB, Stockholm, i samarbete med det ITRL på KTH. Syftet är att forskning hur ett probabilistisk kollisionestimeringssystem skulle kunna genomföras på ett konceptfordon som finns i ITRL. Detta system skulle potentiellt kunna ge fordonet förmågan att fatta beslut om framtida händelser baserat på information som samlats in i realtid.

En probabilistisk kollisionestimeringssystem har utvecklats med hjälp av simu- leringsmiljön ROS. Simuleringarna genomförs på T-korsningssituationer endast ego fordonet och ett annan observerat fordon är närvarande. Systemet har två funk- tionaliteter, 1) skapandet utav sannolikhetsfält vilka indikerar framtida positionerna för den observerade bilen och 2) en summerad kollisionssannolikheten som visar hur sannolik en kollision är över prediktionshorisonten givet en viss styrsignal för ego fordonet. Systemets prestanda utvärderades med hjälp av en hastighetsplanerare som väljer styrinput som ger det lägsta värdet för den summerade kollisionssannolikheten. Som systemet fungerar just nu så är båda delarna för beräkningstunga för att fungera i en realtids applikation, men detta är något som går att uppnå med korrekt implementation. Resultaten gällande robusthet och presision är lovande och fordonet undviker kollisioner då sensorbruset är tillräckligt lågt och det påvisas att detta är ett lovande delsystem för autonoma fordon.

(6)

(7)

Acknowledgements

We would sincerely like to thank our KTH supervisor Lars Svensson for making this thesis possible and for his valuable feedback throughout the project. We would also like to thank our two supervisors from ÅF; Tor Ericson and Gunnar Loeb. Their experience in project management been invaluable when setting up and realizing this thesis. Lastly, we would like to thank our examiner De-Jiu Chen for his valuable help throughout the process of finalizing the thesis.

(8)

(9)

Work Division

The workload in this thesis has been evenly divided between the two authors. Chap- ter 4 and 5 was individually written, researched and implemented by Alexander and Stefan respectively. An equal amount of work effort was put in on the remaining chapters in order to create a cohesive thesis.

(10)

List of Figures

1.1 Conceptual view of the evaluated traffic situation. The ego vehicle is shown in green and the observed vehicle is shown in orange. Possible future states are shown more opaque with associated contours representing position uncertainties. . . 3 2.1 The ROS environment that runs the simulation, the ego vehicle is shown

in green and the observed vehicle is shown in orange. . . 8 2.2 Illustration of case 1. The path of the observed vehicle may vary within

the orange shaded area and its velocity may vary inside the interval shown to the right. . . 11 2.3 Illustration of case 2. The path of the observed vehicle may vary within

the orange shaded area and its velocity may vary inside the interval shown to the right. . . 11 3.1 An overview showing the system, its inputs and outputs. The system

gets inputs from sensor readings and the environment. It outputs either to a trajectory planner or a velocity controller. . . 15 4.1 T-intersection with intention paths. . . 23 4.2 A general representation of a cubic Hermite spline segment. . . 23 4.3 The minimum distance from an arbitrary point p to a cubic Hermite

spline is defined as D. . . 24 4.4 Illustration of how the states (shown as circles) of two trajectories are

fused together using a weight function. The states of the fused vector may lie anywhere on the gray lines depending on the weight function design. . . 25 4.5 The design of the weight function which is used to fuse the motion model

trajectory with the intention trajectory. The function is design so that the intention trajectory is considered the absolute truth after tvir = 2 seconds. . . 27 4.6 The resulting trajectory Ωres is shown in red. It is obtained by fusing

the model trajectory Ωmodel and the intention trajectory Ωintention. . . . 28

(13)

4.7 The turning intention path Ωintentionis shown in green, the motion model is shown in red Ωmodel and the resulting trajectory Ωintention is shown in orange. The turning maneuver has in this case been chosen as most probable since the vehicle’s velocity is low. . . 28 5.1 An example of a two variable Multivariate Normal Distribution with

µ₁ = 1, σ1= 0.5, µ2 = 2 and σ2 = 1 . . . 31 5.2 Example of particle filter from t = 0 to t = 1, where the particles are

excited by a control input ut. . . 34 5.3 The prediction (the ellipses) from the Kalman Filter (KF) of a vehicle

moving on the xy-plane (the dashed line). . . 38 5.4 Displays the probability estimation using the KF as estimator, along

two different possible trajectories where the trajectories for the detected vehicle. . . 38 5.5 A normal distribution for the control input of the particle filter, the ac-

celeration and yaw rate is both zero mean distributed with two different variances. . . 39 5.6 A decay function used to set the weight parameters that is used in

Tdistance, Tvelocity, alongitudinal and alateral. The parameters A, B, C and D is defined to get the desired shape for the purpose, and will be differ- ent for each weight parameter, and in this case A = 0.2037, B = −1.109, C = 0.7937 D = 0.7937. . . 40 5.7 Displays the probability estimation using the particle filter as estimator,

along two different possible trajectories where the trajectories for the detected vehicle. . . 41 6.1 Two objects described by Gaussian distributions occupying the same space 44 6.2 Estimation of collision using the collision probability estimation, along

a specific trajectory for the ego vehicle at a time instant . . . 48 6.3 Shows the points where a possible collision has been detected the green

area is the set of Gaussian distribution along a given trajectory and the red, green and orange dots indicates the position of a possible collision.

Where the green is the ego position, red is the collision point and the orange is the observed vehicles position in the collision. . . 48 7.1 A sequential image of the system, showing the potential collision zones

as dots, of a vehicle controlling the speed based on the controller input with lowest collision chance, thus avoiding a collision. The associated collision predictions given different control inputs are shown beneath each sequential image. . . 50 7.2 A sequential image of the system, showing the potential collision zones as

dots, of a vehicle not controlling the speed, thus resulting in a crash. The associated collision predictions given different control inputs are shown beneath each sequential image. . . 50

(14)

7.3 The estimated probability of a collision for the ego vehicle along a trajectory. The velocity controller for the ego picks the control input which it predicts has lowest collision probability, thus avoiding crash. . . 51 7.4 The collision percentage as a function of samples and sensor noise when

applying the system to case 1 using the Extended Kalman Filter (EKF).

The sensor noise is zero mean normal distributed with σ. . . 52 7.5 The collision percentage as a function of samples and sensor noise when

applying the system to case 1 using the particle filter. The sensor noise is zero mean normal distributed with σ. . . 52 7.6 The collision percentage as a function of samples and sensor noise when

applying the system to case 2 using the EKF. The sensor noise is zero mean normal distributed with σ. . . 53 7.7 The collision percentage as a function of samples and sensor noise when

applying the system to case 2 using the particle filter. The sensor noise is zero mean normal distributed with σ. . . 53 7.8 Computation time for the probability field, the two different cases with

particle filter and KF estimating the probability field at different number of samples. . . 54 7.9 The computation time estimating collision along a ego trajectory given

a probability field given from a KF or a particle filter. . . 55 B.1 A vehicle which is simplified to two circles, that is used to check for a

collision . . . 67 B.2 Testing for collision between two vehicles, a collision occurs since the

distance between two of the circles are less then their radius (they overlap) 67

(15)

List of Tables

2.1 Values for a typical car. Obtained from [1]. . . 9 2.2 The different control inputs tested and evaluated in the Velocity Plan-

ning Controller (VPC) controller. . . 10 3.1 Description of the input and output symbols that are used in the system. 15 4.1 Description and visualization of the most commonly used motion models. 22

(16)

(17)

Abbreviations

ADAS Advanced Driver Assistance Systems. 1, 13, 43 CA Constant Acceleration. 20, 21, 23, 26

CCA Constant Curvature and Acceleration. 20 CSAV Constant Steering Angle and Velocity. 20

CTRA Constant Turning Rate and Acceleration. 20, 21, 25, 26, 38 CTRV Constant Turning Rate and Velocity. 20, 21

CV Constant Velocity. 20, 21, 23

DSRC Dedicated Short-Range Communications. 2 EKF Extended Kalman Filter. 29, 30, 33, 37, 52, 53, 59 GPS Global Positioning System. 9

GPU Graphic Processing Unit. 58–60 HIL Hardware In Loop. 59, 60

IMU Internal Measurement Unit. 9

ITRL Integrated Transport and Research Labs. 1 ITS Intelligent Transportation Systems. 2, 57

KF Kalman Filter. 16, 29, 30, 32, 33, 37, 38, 46, 47, 51, 54, 55, 58 KTH Royal Institute of Technology. 1

LIDAR Light Detection And Ranging. 8, 9, 16 MC Monte Carlo. 29, 44, 58, 59

(18)

MPC Model Predictive Controller. 1, 4, 9, 10, 17, 59 PDF Probability Distribution Function. 44

RCV Research Concept Vehicle. 1, 3 ROS Robot Operating System. 8, 59 TTC Time To Collision. 43

TTX Time To X. 43

UKF Uncented Kalman Filter. 29, 30 V2I Vehicle to Infrastructure. 2 V2V Vehicle to Vehicle. 2

(19)

Nomenclature

G A Gaussian distribution. 15 H Vehicle height. 8, 9, 69 L Vehicle length. 9, 69

N Number of particles/samples. 16, 45 P Probability. 44–47

W Vehicle width. 8, 9, 69

∆t The length of a time step. 21 Ω A trajectory. 21, 25–27, 29, 46, 47

G¯ A set of Gaussian distributions. 16, 17, 40 M¯ The map of the scene. 15, 16, 22

P¯ A probability distribution along a trajectory. 15

¯Ω A set of trajectories. 15–17, 43

ψ¯ A set of probabilities related to intention paths. 16, 17, 45

Pˆ A probability field of future possible positions along a set of trajectories. 15, 17 ψ Probability related to an intention path. 15, 17, 45

ξ A path. 20

t_horizion Prediction horizion. 17, 26, 40 t_real Real time. 26, 46, 47, 50

tvir Virtual time in a prediction. 17, 26, 40, 47, 49 ut Control input at time-step t. 9, 10, 32, 34, 49 x_t State of an object. 9, 15, 16, 21, 26, 29–32, 46

(20)

(21)

Chapter 1

Introduction

Nearly 1.3 million people die each year in traffic-related accidents, whereas an ad- ditional 20-50 million people are injured or disabled [2]. These numbers serve as motivation for initiatives such as the Swedish approach of Zero Vision, which states that no loss in life is acceptable [3], or the no fatal accidents by 2020 goal set by Volvo Cars’[4].

Consequently, these numbers and types of initiatives have inspired a lot research within the field of Advanced Driver Assistance Systems (ADAS) and fully autonomous vehicular systems. Research which either aims to aid the driver, e.g.

automatic brake, or remove the driver from the control loop entirely. A lot of recent progress of fully autonomous cars has been made on relatively simple traffic scenarios, such as high-way scenarios. However, intersections still pose a major challenge to these kinds of systems, due to the complex nature of the situations.

These scenarios will require autonomous systems with the capability of making long term predictions in order to actuate the vehicle in the appropriate manner to avoid potential collisions.

This thesis proposes an approach to handle such intersection scenarios by making long-term state predictions of cars in the scene. These predictions form a basis for a feedback through a Velocity Planning Controller (VPC) that aims at actuating the driver’s vehicle along a predetermined path so that no future collisions are probable.

1.1 Background

In autumn 2014, the Royal Institute of Technology (KTH) started a collaboration with the Swedish truck manufacturer Scania and Ericsson in a attempt develop transport systems for the future. The collaboration resulted in the Integrated Trans- port and Research Labs (ITRL). One of the main products developed at ITRL has been the Research Concept Vehicle (RCV) which is a steer-by-wire car that serves as a platform for applying new technology related to autonomous systems. One initiative is to implement a probabilistic system which assesses future situations by predicting the states of other vehicles in the scene to avoid collisions.

(22)

CHAPTER 1. INTRODUCTION Collision avoidance systems can be divided into two methodologies; sensor-based and Intelligent Transportation Systems (ITS)-based. Sensor based approaches rely on on-board sensors and processors to retrieve information of the vehicles surround- ings while ITS-based systems utilize either Vehicle to Vehicle (V2V) or Vehicle to Infrastructure (V2I) communication to retrieve information. According to [5], both methodologies face several challenges in order for them to be viable as alternatives in future intelligent vehicles. Sensor based systems are limited to field-of-view and are still prone to unfavorable weather conditions. ITS systems are on the other hand prone to errors when sending information and this also opens up a channel that brings security issues. This is an underlying reason why automotive manufactur- ers currently are developing the Dedicated Short-Range Communications (DSRC) standards for communication in such systems [6]. The information collected using either of the two methodologies described above is then used to predict/assume future behaviors of vehicles present in the scene. From now on, only prediction approaches using sensor-based methodologies will be discussed.

One way of determining the future behavior of other vehicles is by pattern recognition based on previously recorded data. Such approaches require large training sets in order to classify behaviors depending on their previous and current states (see e.g. [7]). Moreover, they are challenged by situations where erratic behavior, e.g. drunk drivers, are present. Other approaches relies on more physical models to predict future states of vehicles. Simple examples of such models can be found in [8, 9] where bicycle models are implemented.

1.2 Purpose

The purpose of this thesis is to provide an initial step in a probabilistic collision prediction system which aims at producing a risk field for the vehicle that predicts upcoming risks. This step will include creating a probability field, which estimates what areas the surrounding objects are going to occupy that increases with the level of uncertainty, which is shown in Figure 1.1. The overall research question is defined as:

– How can probabilistic collision prediction using a dynamic risk field approach with future estimations be applied for an autonomous vehicle?

This research question is then subdivided into smaller questions. Three of these questions, which are the focus of this thesis, are defined as

– How can future trajectory estimations be done using dynamic models and road information?

– How can future prediction algorithms support a probabilistic collision estimation system by creating a probability field?

2

(23)

1.3. SCOPE

– How feasible is a probabilistic collision estimation system, in terms of accuracy and computational time, for real-time applications?

Figure 1.1. Conceptual view of the evaluated traffic situation. The ego vehicle is shown in green and the observed vehicle is shown in orange. Possible future states are shown more opaque with associated contours representing position uncertainties.

1.3 Scope

Autonomous systems are complex by nature and it is therefore natural make sub- stantial delimitations to obtain a reasonable scope for a master’s thesis. This thesis has been delimited to only include simulations of the system since the implementation step on the intended application platform (i.e. the RCV) is going to be too time consuming. Additionally, the evaluated scenarios have been chosen to be T- intersections with a known environment, limited to only include a single vehicle in addition to the ego vehicle. This choice has been made due to the system complexity, concerning interactions, that multiple cars would introduce. The vehicles in these scenarios are considered to be cars.

Furthermore, some research areas which include image processing, object tracking, mapping and trajectory planning will not be addressed since these areas con- stitute as research areas single-handedly. Incorporating any of these areas would make this thesis even more complex and remove attention from what should be the main focus of the thesis; the probabilistic collision estimation.

1.4 Method

This thesis will apply a sequential mixed method described by Creswell [10] and is carried out according to the time-plan in Appendix A. The system is designed in a qualitatively manner and use a specific case to quantitatively evaluate how well the system performs by comparing the different combinations of dynamic models

(24)

CHAPTER 1. INTRODUCTION together with prediction algorithms in terms of precision, robustness to noise, computational time and feasibility in terms of what is needed from the system. The data will be gathered from simulations, and the physical parameters that define the system will be discussed upon and how selection of different parameters affects the results.

To be able to evaluate how good the prediction is, a VPC is implemented on the ego vehicle which regulates the velocity according to a predictive approach. This controller tests different scenarios and predicts which one is safest and then chooses the control input based on this information.

To test the precision, the system is tested quantitatively so that results can be acquired on how often the system crashes when varying two different parameters, noise level and number of samples. This is tested for two different cases, as defined in Section 2.4, and two different estimation methods, as defined in Chapter 5, which gives four different situations that indicates the precision and the robustness of the system.

The computation time is measured quantitatively by running the system and measuring the time spent calculating the relevant parts, the number of samples variable is varied to show scalability. The KF, particle filter and risk evaluation will all be measured independently because depending on how the system is used, different parts affects the computation time.

The system feasibility for embedded real time applications will be discussed in a qualitative manner and suggestions for continuing the work will be presented in future work.

1.5 Outline

The thesis is structured as follows:

• Preliminaries discusses the assumptions for the thesis, shows the simulation environment and discusses the cases that are tested.

• Probabilistic Collision Avoidance System shows the structure of the presented system and defines inputs and outputs from the subsystems.

• Long-term Trajectory Predictions presents the theory behind the long- term trajectory prediction and shows the implementation in the presented system.

• Probabilistic Estimation Methods presents the theory behind the esti- mation along a given trajectory, using two different methods of estimation.

The system implementation using these two methods is also explained.

• Collision Probability from Distributions shows the theory and imple- mentation on how the collision calculations are done using a Monte Carlo type of method.

4

(25)

1.6. ETHICS

• Results shows the acquired results, as defined in the method.

• Conclusion discusses the results from the perspective defined in the method.

The future work and the contribution is also concertized.

1.6 Ethics

This thesis focuses on how a collision detection system could work for autonomous vehicles, the work is developed and tested in a simulation environment. Therefore there is not much ethical consideration one needs to have at this point of the project, no real data is recorded, no one is put in any dangerous situations and we do not conduct any interviews. However, if one would take this project forward and test it in real traffic situations using an array of sensors or communication. There could be much more ethical considerations that are needed depending on how the future research would be conducted. One would for example need to consider how the recorded data is handled, and what information there is in the data. One example is if the recorded data is video footage. Then it should be clear for the persons using the system that there is a camera and the other vehicles/pedestrians should be made anonymous if the data is made public.

Since this kind of system could potentially be dangerous if it malfunctions it is very important to clarify who bares responsibility if accidents happen.

(26)

(27)

Chapter 2

Preliminaries

This chapter describes the required preliminaries which were made in order to com- plete the thesis. It elaborates on necessary assumptions which were made, environment that was used, vehicle models that have been implemented and predetermined test cases.

2.1 Assumptions

In order to reduce complexity and delimit the scope of the thesis, some assumptions has been made. The following assumptions have therefore been made.

• Two vehicles are considered in each scenario; the ego vehicle, which implements the system, and one observed vehicle.

• The ego vehicle is considered to be fully autonomous which implies the following.

– The ego position is known in the coordinate system specified by a pre- set map. It is assumed that the error and delay of these systems are negligible.

– The vehicle is mounted with sensors such as cameras and laser sensors, in order to detect other vehicles.

– The vehicle is equipped with controllers so that the system has the ability to actuate yaw and speed.

– The vehicle is equipped with a trajectory controller which provides a predetermined trajectory for the ego vehicle.

• The weather conditions are considered optimal for the sensors.

• The condition of the road is considered to be perfect.

• No objects obstruct the vision in the intersection.

(28)

CHAPTER 2. PRELIMINARIES

• The ego and the observed vehicle are assumed to be cars with the same kind of dynamics and dimensions. No other dynamic objects are considered.

• The surrounding observed vehicles intentions are not known.

• Ego vehicle can only observe the other vehicle, no communication is done between the vehicles or between vehicles and infrastructure.

2.2 Environmental Setup

The simulation environment that is used is the Robot Operating System (ROS) with the code developed in C++. ROS is a framework that is easy to develop and test code in as well as integrate sensors with. The visualized environment can be seen in Figure 2.1.

For this project the software is developed to test two specific test cases, varying a few variables within a range at specific number of times. During each run the data is logged to be able to analyze how varying the parameters affects the system performance. The parameters are varied independently from each other and tested several times to test the entire space of the specified parameters and to be able to draw conclusions on trends of the system from the given results.

Figure 2.1. The ROS environment that runs the simulation, the ego vehicle is shown in green and the observed vehicle is shown in orange.

The ego vehicle is tracked in the global frame while the observed vehicle is tracked using sensors, like for example a Light Detection And Ranging (LIDAR) sensor, which gives a local reference point in polar coordinates relative to the ego, which is then transformed to the global reference frame. Moreover, the simulation environment is set up to run at a frequency of 10Hz and will only consider T- intersection scenarios.

2.3 Vehicle Model

The system proposed in this paper is restricted to handling cars, at this moment in time. Cars are geometrically described as rectangles with a width W , height H and a length L. They have a turning radius φ as well as a maximum acceleration

8

(29)

2.3. VEHICLE MODEL

and deceleration ¨x. Values from a typical car have been obtained from [1] and are shown in Table 2.1. Friction is neglected.

Table 2.1. Values for a typical car. Obtained from [1].

Property Value

Width (W ) 4.8m

Height (H) 1.8m

Length (L) 2.4m

Turning radius (φ) 0.5 rad/s Acceleration (¨xacc) 4.2m/s² Braking (¨xret) −9.1m/s²

The states of the ego car are determined using on-board sensors, such as Global Positioning System (GPS) and Internal Measurement Unit (IMU), whereas its position is defined in the global coordinate system. On the other hand, the states of the observed cars are captured relative to the ego car in a polar coordinate system using e.g. LIDAR and cameras. The position is then converted into the global coordinate system. The state vector of the ego and observed vehicles are defined as

xtego= xtobs =





 x y θ

˙x˙θ







(2.1)

where x and y determines the position in the global coordinate system, θ determines the relative yaw angle, ˙x the longitudinal velocity and ˙θ the yaw rate over time.

A VPC is used to control the velocity of the ego vehicle. It uses the prediction of how the scene will develop, described in Chapter 5, and by testing different control inputs, described in Chapter 6, the input that has the lowest chance of collision will be chosen. Only a small subset of all possible control inputs are available and the values are defined as ut and shown in Table 2.2.

When the collision probability is calculated for ut, and if two would have equally low probability of collision, the input with highest priority will be chosen. The priority is defined using a superscript where lower number means higher priority, thus the VPC chooses the best control input in a top-down fashion as described in Table 2.2. As the table shows, constant velocity have the highest priority followed by braking and lastly accelerating.

(30)

CHAPTER 2. PRELIMINARIES

Table 2.2. The different control inputs tested and evaluated in the VPC controller.

Control input Value [m/s²]

u_t¹ 0

u_t² −1

ut3 −4

u_t⁴ −9

u_t⁵ 1

ut6 2

u_t⁷ 4

2.4 Evaluated Cases

The system is evaluated by two different scenarios. Both scenarios are set up by having the ego car approach a T-intersection from the connecting lane while another car approaches the intersection from the left, relative to the ego vehicle. The speed limit of each road is 70km/h. The objective of the ego car is to perform a safe left turn in the intersection with respect to the predicted behavior of the other observed vehicle. The ego car follows a predetermined path and uses a model predictive velocity controller to actuate its velocity.

2.4.1 Case 1 - Normal

The first case depicts a scenario where the observed vehicle drives straight through the intersection at a constant speed while the ego vehicle aims to do a left turn. The speed for the observed vehicle is, for each run, randomized within in the interval depicted in Figure 2.2 to the right. The path of this vehicle is, for each run, randomized within the shaded area in Figure 2.2 to the left. The path is thus ensured to be within the observed vehicles designated lane. The ego vehicle implements the VPC which aims to decrease or increase the speed to avoid potential future collisions.

This case should be seen as a relatively simple case which is bare minimum of what the presented system should be able to handle without experiencing any collisions.

10

(31)

2.4. EVALUATED CASES

0 20 40 60 80 100 120

40 60 80 100

Distance[m]

Velocity[km/h]

Boundaries Mean

Figure 2.2. Illustration of case 1. The path of the observed vehicle may vary within the orange shaded area and its velocity may vary inside the interval shown to the right.

2.4.2 Case 2 - Erratic

The second case depicts a scenario where the observed vehicle drives in a more erratic manner. Its velocity profile and path are chosen randomly, for each run, in the intervals defined in Figure 2.3. This case enables the observed vehicle to perform a braking motion in the middle of the intersection. Moreover, it enables the vehicle to use paths that moves over to the opposite lane. We assume that this case will provide a more complex case for our system, although this could be investigated further.

0 20 40 60 80 100 120

0 50 100

Distance[m]

Velocity[km h]

Bounderies Mean

Figure 2.3. Illustration of case 2. The path of the observed vehicle may vary within the orange shaded area and its velocity may vary inside the interval shown to the right.

(32)

(33)

Chapter 3

Probabilistic Collision Avoidance System

This chapter focuses on the system developed in this thesis. It first introduces some related work on systems for autonomous vehicles and ADAS systems. This is followed up by a description of the system structure adopted in this thesis.

3.1 Related Work

ADAS and fully autonomous vehicles have developed a lot over the last couple of years. Recent standards suggest that ADAS system can be divided into eight domains of interests; lateral control, longitudinal control & avoidance, reversing/- parking aids, vision enhancements, intelligent speed adaption, driver monitoring, pre-crash systems and road surface/low friction warning [11]. On the other hand, fully autonomous systems aim to reduce the drivers influence to zero whereas the prototype vehicle from Stanford Junior is a case in point [12]. All systems that incorporate any level of autonomy need to be subjected to extensive safety analysis looking at both predetermined risks and active risks. One of biggest challenges within this area is to assert risks associated with particular situations by predicting how the situation will evolve in the future [13].

Various system structures related to future prediction algorithms have been proposed by numerous authors. Some examples of such systems are presented in the following text. In [14], the system obtains vehicle and environmental evidence, such as vehicle states and lane lines, in order to detect probable maneuvers for each vehicle in the scene using Bayesian inference. These results are then fed forward to a maneuver specific prediction algorithm which estimates future motions in order to later on calculate collision probability estimations and critically metric calculations.

A similar system is presented by [15] whereas target tracking and road marking is provided to a trajectory prediction system which combines a kinematic vehicular model with a maneuver recognition module. In [16], a situation hypothesis selection framework is presented which selects the ego maneuver based on situation recognition and risk minimization. A more abstract system is proposed by [17] whereas a standardized architecture language is used to present a model based approach for

(34)

CHAPTER 3. PROBABILISTIC COLLISION AVOIDANCE SYSTEM

risk assessment and safety requirements.

3.2 Requirements

The requirements are kept at a high level intentionally, since this project is still at an early concept stage and all the requirements are not defined. These are the system requirements:

• The system shall support the trajectory planner by providing information about upcoming dangerous areas.

• The system shall be able to evaluate a given trajectory and estimate how likely it is that a collision will occur.

• The system shall be able to handle sensor noise.

• The system shall have a modular design, where big components can be changed when improved methods are found.

• The system shall be designed in a way so that the method for detecting the position of target objects can be changed and mixed.

• The probability estimation should be able to run at real time, where real time is defined as 10Hz on a embedded platform.

• The system needs to be able to successfully predict any collision from a detected object.

• The system needs to scale well with bigger and more complex traffic situations.

3.3 System Overview

The system is composed by multiple subsystems and itself is a part in a more complex vehicular system, aiming to aid other systems such as trajectory planners.

A summary of the variables passed internally through the system, as well as inputs and outputs, are described in Table 3.1, a system overview is shown in Figure 3.1.

14

(35)

3.3. SYSTEM OVERVIEW

Table 3.1. Description of the input and output symbols that are used in the system.

Symbol Description

xt Vehicle state (x,y,θ, ˙x, ˙θ) M¯ Map data

ψ Intention path probabilities

¯Ω Vehicle trajectories G Gaussian distributions Pˆ Probability fields

P¯ Collision probability along given ego trajectory

Figure 3.1. An overview showing the system, its inputs and outputs. The system gets inputs from sensor readings and the environment. It outputs either to a trajectory planner or a velocity controller.

(36)

CHAPTER 3. PROBABILISTIC COLLISION AVOIDANCE SYSTEM The different parts of the system, illustrated as boxes in Figure 3.1, are described in the following subsections.

Inputs

The system inputs originates from the environment, sensors and possibly a trajectory planner. The environmental aspect provides the system with map data ¯M, such as lane markings, traffic signs and speed limits which are used to predict a future path for objects in the scene. Sensors provide the system with both the current ego state xtego(using for example GPS and IMU sensors) and the observed state of other vehicles xtobj (using for example LIDAR/camera sensors or communication).

The last input is obtained from a trajectory planner ¯Ωegoproviding the system with the future trajectory of the ego vehicle.

Intention Estimation

The intention estimation obtains the states of the observed vehicle xtobj and map data ¯M as inputs. The map data is used to define intention paths. These paths are placed in the middle of each lane and represents ideal maneuvers (such as turning or going straight) for the vehicles in the intersection. The intention estimation then compares xtobjwith the closest state on each reference path and assigns a probability value ψ to each. This block outputs this as a vector of probability values ¯ψ. The function of this block is further elaborated in Section 4.4.

Path Prediction

The path prediction block gets the object state xtobj, map data ¯M and the probability vector ¯ψderived by intention estimation as inputs. Its goal is to calculate the predicted trajectories ¯Ωobj for the observed object given the intention trajectories with highest probability values. This part is further explained in Section 4.4.

Kalman Filter

The KF obtains the predicted trajectories ¯Ωobj from the path prediction along with the object state xobj and intention path probability vector ¯ψ. It uses these to implement Gaussian uncertainties along the predicted trajectories to introduce an uncertainty in the objects position which are called Gaussian distributions ¯G. This part is further explained in Section 5.4.

Particle Filter

The particle filter box does, much like the KF, produce a set Gaussian uncertainties describing the state of the observed object. However, it propagates N number of particles forward in time acting as the observed object. These particles are then used to estimate the distribution object. This process is described more in-depth in Section 5.4.

16

(37)

3.3. SYSTEM OVERVIEW

Probability Fields

The generated Gaussian uncertainties ¯G are combined with the probability vector ψ¯related to each intention path in order to produce a probability field. To make a single probability field for one set of estimations, the following is done

Pˆ(tvir) = ψ · ¯G, t_vir = [0, thorizion] (3.1) which holds estimations of the target objects future position. This produced field could either be outputted to a trajectory planner, which in turn could use this to predict safer trajectories, or as input to the collision probability algorithm.

Collision Probability

The needed input to this part is the probability field ˆP and the ego trajectory ¯Ωego. It then calculates potential collision points by drawing samples from the probability field and comparing these with the ego states along the trajectory. This process is further described in Section 6.4.

Outputs

Two main outputs can be obtained by the system. Firstly, a trajectory planner may use the system to make safe trajectory predictions by obtaining the probability fields from the system and feeding back the predicted trajectory, thus closing the loop. The other output is the probability of collision given a certain ego trajectory.

This could be an input to a longitudinal velocity controller used when the ego has a fixed future trajectory. This second operation is used to validate the system in this thesis. A velocity controller is used to control the speed of the vehicle by taking a VPC approach which at each time step, tests a range of input through the collision probability to see which one gives the best results. The control input with the lowest collision probability is then chosen.

(38)

(39)

Chapter 4

Long-term Trajectory Predictions

Predicting long-term future trajectories for dynamic objects in traffic scenes is es- sential in order to accurately assess risks associated with these scenarios. Long-term is here defined as having a sufficiently large time horizon to avoid collisions by actuating the vehicle. This chapter will firstly introduce relevant research related to the area of trajectory prediction, secondly a section on theory related to the presented approach and lastly how this is implemented in the suggested system.

4.1 Related Work

There exists numerous of methods for predicting long-term trajectories of target vehicles in a scene. One way of classifying these has been suggested by [13] in a survey on motion prediction and risk assessment. It categorizes approaches in three levels with increased degree of flexibility; physical-based, maneuver-based and interaction aware approaches. Physical-based approaches obtain future trajectory predictions by propagating the vehicles using kinematic and dynamic models independent of other objects and traffic rules. Maneuver-based approaches aim to identify/classify the current maneuver that the driver intents to perform based on previously observed movement patterns to obtain a more accurate prediction. Intention-aware approaches consider interactions between entities as well as traffic rules which makes for complex models which are not often suitable for real time applications. Another more general classification is mentioned in [14] whereas long-term trajectory prediction approaches are divided up into either machine learning methods using motion pattern databases or approaches which fuse dynamic models with environmental and behavior information. The article suggests that the machine learning approaches suffer from generating, saving and fitting large motion pattern data records as well as having the disadvantage that only trajectories included in the database can be predicted. According to the author, this last issue makes these approaches unsuit- able for critical assessment due to their inability to capture extraordinary events.

The remained of this section is focused on physical based approaches and approaches which fuse motion models with environmental information.

(40)

CHAPTER 4. LONG-TERM TRAJECTORY PREDICTIONS Motion models, within the context of vehicle tracking, have been thoroughly analyzed by [18]. The comparative research introduces six different motion models; Constant Velocity (CV), Constant Acceleration (CA), Constant Turning Rate and Velocity (CTRV), Constant Turning Rate and Acceleration (CTRA), Constant Steering Angle and Velocity (CSAV) as well as Constant Curvature and Acceleration (CCA). Only CV, CTRV, CTRA and CCA are included in an performance analysis stage which concludes that the CTRA model performs best in terms of higher prediction accuracy and lower calculation effort. Although, utilizing any of the motion models presented above alone for long-term predictions would be insufficient since motion models only are reliable for short term predictions (thorizion < 1second) [13, 19, 20].

Due to this, numerous researchers have focused on methods which fuse the dynamic short-term models with environmental or behavioral information. A selection of these are explained in the following. [14] presents a Bayesian Network (BN) which determines which maneuver that is most likely to occur depending on certain evidence based on the state of the observed vehicle and environmental information.

The maneuvers also include a trash class which captures irrational behaviors. In the research presented by [19, 15], they argue that the trajectory gained from a kinematic model is accurate for a short time predictions while the trajectory obtained from a maneuver identification, such as change lane, is more accurate for long time predictions. They utilize this by calculating the predicted trajectory as a weighted sum of these two trajectories using a cubic spline weight function. Another similar approach is presented by [20]. In their work, the dynamic model is also used for short term predictions but the road geometry is explicitly used for long term predictions.

4.2 Proposed Approach

The chosen approach for making long-term trajectory predictions in the system is based of the studies performed by [19, 20]. The method utilizes map information coupled with speed gradients to obtain reference trajectories describing an the center-line of all different maneuvers in an intersection, such as going straight through or turning. The state of the observed vehicle is compared to the state of all reference trajectories at the point closest to the car. This comparison is done by taking the Euclidean norm of the difference between these states and the vehicle state individually. The most probable reference trajectories obtained from this comparison are then individually weighted with the dynamic model for the vehicle using a weight function to obtain a predicted path.

4.3 Theory

This chapter focuses on the theory behind the adopted long term planning approach.

An important differentiation is made between paths and trajectories in this section.

20

(41)

4.3. THEORY

Paths, ξ, are defined as continuous functions parameterized between two endpoints.

Trajectories, Ω, are on the other hand defined as time-ordered sets of states xtalong a path. These states contain information regarding position, yaw angle and velocity of the vehicle.

4.3.1 Motion Models

Physical based motion models aim to predict future trajectories based on the measured vehicle state by imposing different rules. These motion models provide fairly good future predictions given a sufficiently small prediction horizon [18]. Some of the most commonly used motion models are CV, CA, CTRV and CTRA. These motion models are described and depicted in Table 4.3.1. The state transition form, when using ∆t as time step size, of the motion models are as follows.

CV ¯x(t + ∆t) = x(t)

˙x(t)

!

+ ∆t ˙x 0

!

, (4.1)

CA ¯x(t + ∆t) =





 x(t)

˙x(t)

¨a(t)





+ ∆t







˙x∆t + ^x∆t^¨₂

¨x∆t0





, (4.2)

CTRV ¯x(t + ∆t) =





 x(t) y(t) θ(t)

˙x(t)

˙θ(t)





 +







˙ x

θ˙sin( ˙θ∆t + θ) − ^xθ^˙˙sin(θ)

−^x^˙_˙

θcos( ˙θ∆t + θ) + ^x_θ^˙_˙sin(θ)

˙θ∆t0 0







, (4.3)

CTRA ¯x(t + ∆t) =





 x(t) y(t) θ(t)

˙x(t)

¨x(t)

˙θ(t)





 +







∆x(t, ∆t)

∆y(t, ∆t)

¨x∆t˙θ∆t 00







(4.4)

with

∆x = 1

˙θ²[( ˙x(t) ˙θ + ¨x ˙θ∆t)sin(θ(t) + ˙θ∆t)

+ ¨xcos(θ(t) + ˙θ∆t) − ˙x(t) ˙θsin(θ(t)) − ¨xcos(θ(t))]

and

∆y = 1

˙θ²[−( ˙x(t) ˙θ − ¨x ˙θ∆t)cos(θ(t) + ˙θ∆t)

+ ¨xsin(θ(t) + ˙θ∆t) + ˙x(t) ˙thetacos(θ(t)) − ¨xsin(θ(t))].

(42)

CHAPTER 4. LONG-TERM TRAJECTORY PREDICTIONS Here, x and y are positions in R², θ is the yaw angle with respect to the horizontal axis in the global coordinate system, ˙x is the velocity, ¨x is the acceleration and ˙θ is the angular velocity. The motion models are further described and visualized in Table 4.3.1.

Table 4.1. Description and visualization of the most commonly used motion models.

Motion Model Visualization

Constant Velocity (CV) and Constant Acceler- ation (CA) propagates the vehicle forward in a straight trajectory using either the velocity or acceleration/braking measured at t = 0. These motion models will not be able to capture turning motions and are therefore not reliable.

Constant Turning Rate and Velocity (CTRV) assumes that the vehicle will keep the same ve- locity and turning rate as measured at t = 0.

The model does not take the acceleration into account and may therefore not calculate future trajectories, in an accurate way, in scenarios where the vehicle is subjected to a big acceleration or retardation.

Constant Turning Rate and Acceleration (CTRA) combines the ability to predict turning motions from CTRA with the ability to capture accelerating or braking motions.

4.3.2 Intention Paths

Sideline markings and center-lines of lanes can be obtained from the map data ¯M. This data can be further utilized to create idealized paths, which are shown in Figure 4.3.2. Such paths have been used in by other authors, e.g. [21].

22

(43)

4.3. THEORY

Figure 4.1. T-intersection with intention paths.

Cubic Hermite Splines

Paths that represents the geometry of the road, such as the intention paths, are commonly represented as cubic Hermite splines. Cubic Hermite splines are defined using piece-wise third order polynomials. Each spline segment is defined between two points. These points are in turn defined in Hermite form, which specifies the position ~pi and the first derivative ~mi of each endpoint. An general spline segment, using the endpoints p0 and p1, is shown in Figure 4.2.

(p0,x,p0,y) (m0,x,m0,y)

(p1,x,p1,y) (m1,x,m1,y)

Figure 4.2. A general representation of a cubic Hermite spline segment.

The cubic Hermite spline can furthermore be represented as a polynomial. On the unit interval [0,1], using the starting point p0 at s = 0, ending point p1 at s = 1, starting tangent m0 at s = 0 and ending tangent m1 at s = 1, the polynomial can be written as

~p(t) = (2s³−3s²+ 1)

| {z }

h00

~

p0+ (s³−2s²+ s)

| {z }

h10

~

m0+ (−2s²+ s)

| {z }

h01

~

p1+ (s³− s²)

| {z }

h11

~

m1 (4.5)

where s ∈ [0 1]. The functions h00, h10, h01, h11 are commonly referred to as the Hermite basis functions.

(44)

CHAPTER 4. LONG-TERM TRAJECTORY PREDICTIONS

Closest Point to Spline

A key component when using splines in driving simulators is the ability to find the closest point on it relative to the vehicles position. The closest point on a cubic Hermite spline relative to an arbitrary point can be found by minimizing the distance function D shown in Figure 4.3.

p¯0

D p¯c

(p0,x,p0,y) (m0,x,m0,y)

(p1,x,p1,y) (m1,x,m1,y)

Figure 4.3. The minimum distance from an arbitrary point p to a cubic Hermite spline is defined as D.

Let ~p0= (x0, y₀) be an arbitrary point in R². The cubic distance d² between ~p0 and the cubic Hermite spline ~p(t) is then denoted as

D²(t) = (x(t) − x0)²+ (y(t) − y0)². (4.6) Insertion of equation (4.5) in (4.6) and setting its derivative to zero would result in the minimum distance. However, this substitution and derivation results in a fifth order polynomial which is unsolvable analytically. It must be solved numerically.

This thesis uses Newton’s formula as a numerical method since this method has been proven to converge fast when applied to these kind of problems [22]. The iterative formula for finding the minimum point for the distance formulas derivative, and thus minimizing (4.6) is

s∗,n+1= s∗,n−D⁰_∗,n

D⁰⁰_∗,n, n ∈ N (4.7)

where the first and second derivatives can be obtained by derivation of equation (4.5) and (4.6). This iterative process is shown in Algorithm 1 and will converge to minimum point ¯p(t^∗) given a sufficiently good starting guess. The Algorithm inputs with the arbitrary point and the acceptable error tolerance .

4.3.3 Fusing Trajectories

Fusing two trajectories can be done by comparing states at the same time instances and fusing them [20, 19]. One way, proposed by [19], is to utilize a weighting function to control the impact of each trajectory at every time instance t. The final path can therefore be calculated as

Ωres= wΩ1+ (1 − w)Ω2 (4.8)

24

(45)

4.4. IMPLEMENTATION

Algorithm 1:Finding the closest point on spline to an arbitrary point.

1 Closest Point on Spline (¯p0, )

2 s∗ = 0.5;

3 ¯pold= ¯p(0)

4 ¯pnew = ¯p(s∗)

5 while |¯pnew−¯pold| > do

6 s∗ = s∗,n+1(¯p0)

7 ¯pold= ¯pnew 8 ¯pnew = ¯p(s∗)

9 end

10 return ¯pnew

where w is the weight factor obtained by the weighting function. This process of comparing state-pairs is illustrated in Figure (4.4) whereas the vertical lines indicate potential states of the resulting path Ωresdepending on design of the weight function.

Trajectory 2

Trajectory 1

Figure 4.4. Illustration of how the states (shown as circles) of two trajectories are fused together using a weight function. The states of the fused vector may lie anywhere on the gray lines depending on the weight function design.

4.4 Implementation

As stated before, motion models are fairly accurate for short time predictions while it is more probable that a vehicle heads towards paths defined by road geometry on

(46)

CHAPTER 4. LONG-TERM TRAJECTORY PREDICTIONS the long term. The motion model CTRA has been selected in this thesis due to it being acceleration enchanted and able to capture turning motions which combined provide more accurate estimations [18].

Each intention path is constructed by combining multiple cubic Hermite splines.

The different segments are then assigned a velocity profile. The intention paths that are going straight are assigned a constant velocity. The paths that are turning are on the other hand assigned a velocity based on [23] which is set up as a deceleration followed by an acceleration.

At each time instance treal, Algorithm 1 is used to find the closest point to the observed vehicle on the intention path, xîntentioni . The similarity of xîntentioni and the observed vehicles state xôbs_i is then assessed according to

ψ_i= e⁻ q

PI

i=0|x^obs_t −x^intention_i |²

(4.9) where I is the number of intention paths. The intention paths which similar are chosen. These resulting intention paths are then converted to trajectories by propagating the closest point forward along the path using CA. The CA model has been chosen for this to complement the CTRA chosen for the motion model trajectory.

This choice ensures that both trajectories have equal distances between states which otherwise could result a deformed resulting path with sharp edges.

The weight function used in (4.8) for fusing together the motion model and the intention trajectory has been chosen as

w(tvir) =

(_1+cos(πt_vir 2 )

2 , t_vir≤ t_horizion

0 , t_vir> t_horizion (4.10) and is depicted in Figure (4.5). The depicted function has a scaling of ^π₂ but this is arbitrarily chosen. It is design so that the motion model is the absolute truth at t_vir= 0. The function then decreases in a sinusoidal fashion until intention trajectory is considered the absolute truth after the time treal= 2s.

26

(47)

4.4. IMPLEMENTATION

0 0.5 1 1.5 2 2.5 3

tvirtual 0

0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1

Weight

w(t) = ^(1+cos(

π tvir 2 ) 2

Figure 4.5. The design of the weight function which is used to fuse the motion model trajectory with the intention trajectory. The function is design so that the intention trajectory is considered the absolute truth after tvir= 2 seconds.

Applying this weight function to the fusion stage, as shown in Figure 4.4, results in the resulting trajectory Ωres as depicted in red along its related states xt as circles in Figure 4.6.

Probabilistic Collision Estimation System for Autonomous Vehicles: Evaluated in Intersection Scenarios Using a Velocity Planning Controller

Probabilistic Collision Estimation System for Autonomous Vehicles

Evaluated in Intersection Scenarios Using a Velocity Planning Controller

ALEXANDER GRATNER

STEFAN ANNELL

Abstract

Sammanfattning

Acknowledgements

Work Division

Contents

List of Figures

List of Tables

Abbreviations

Nomenclature

Chapter 1

Introduction

1.1 Background

1.2 Purpose

1.3 Scope

1.4 Method

1.5 Outline

1.6 Ethics

Chapter 2

Preliminaries

2.1 Assumptions

2.2 Environmental Setup

2.3 Vehicle Model

2.4 Evaluated Cases

Chapter 3

Probabilistic Collision Avoidance System

3.1 Related Work

3.2 Requirements

3.3 System Overview

Chapter 4

Long-term Trajectory Predictions

4.1 Related Work

4.2 Proposed Approach

4.3 Theory

4.4 Implementation