Path Prediction for a Night Vision System

(1)

Institutionen för systemteknik

Department of Electrical Engineering

Examensarbete

Path Prediction for a Night Vision System

Examensarbete utfört i Reglerteknik vid Tekniska högskolan vid Linköpings universitet

av

Johannes Fri

LiTH-ISY-EX--11/4483--SE

Linköping 2011

Department of Electrical Engineering Linköpings tekniska högskola

Linköpings universitet Linköpings universitet

(2)

(3)

Path Prediction for a Night Vision System

Examensarbete utfört i Reglerteknik

vid Tekniska högskolan i Linköping

av

Johannes Fri

LiTH-ISY-EX--11/4483--SE

Handledare: Tohid Ardeshiri

isy, Linköpings universitet Jonas Elbornsson Skogstjärna

Autoliv Electronics

Examinator: Thomas Schön

isy, Linköpings universitet

(4)

(5)

Avdelning, Institution

Division, Department

Division of Automatic Control Department of Electrical Engineering Linköpings universitet

SE-581 83 Linköping, Sweden

Datum Date 2011-06-13 Språk Language Svenska/Swedish Engelska/English Rapporttyp Report category Licentiatavhandling Examensarbete C-uppsats D-uppsats Övrig rapport

URL för elektronisk version

http://www.control.isy.liu.se http://urn.kb.se/resolve?urn=urn:nbn:se:liu:diva-68895 ISBN — ISRN LiTH-ISY-EX--11/4483--SE

Serietitel och serienummer

Title of series, numbering

ISSN

—

Titel

Title

Vägprediktering för ett mörkerseende system Path Prediction for a Night Vision System

Författare

Author

Johannes Fri

Sammanfattning

Abstract

In modern cars, advanced driver assistance systems are used to aid the driver and increase the automobile safety. An example of such a system is the night vision system designed to detect and warn for pedestrians in danger of being hit by the car. To determine if a warning should be given when a pedestrian is detected, the system requires a prediction of the future path of the car for up to four seconds ahead in time.

In this master’s thesis, a new path prediction algorithm based on satellite positioning and a digital map database has been developed. The algorithm uses an extended Kalman filter to get an accurate estimate of the current position and heading direction of the car. The estimate is then matched to a position in the map database and the possible future paths of the vehicle are predicted using the road network.

The performance of the path prediction algorithm has been evaluated on recorded night vision sequences corresponding to 15 hours of driving. The results show that map-based path prediction algorithms are superior to dead-reckoning methods for longer time horizons.

It has also been investigated whether vision-based lane detection and tracking can be used to improve the path prediction. A prediction method using lane markings has been implemented and evaluated on recorded sequences. Based on the results, the conclusion is that lane detection can be used to support a path prediction system when lane markings are clearly visible.

Nyckelord

Keywords Positioning, Vehicle Navigation, GPS, Map Matching, Extended Kalman Filter, Path Prediction, Lane Detection

(6)

(7)

Abstract

In modern cars, advanced driver assistance systems are used to aid the driver and increase the automobile safety. An example of such a system is the night vision system designed to detect and warn for pedestrians in danger of being hit by the car. To determine if a warning should be given when a pedestrian is detected, the system requires a prediction of the future path of the car for up to four seconds ahead in time.

In this master’s thesis, a new path prediction algorithm based on satellite posi-tioning and a digital map database has been developed. The algorithm uses an extended Kalman filter to get an accurate estimate of the current position and heading direction of the car. The estimate is then matched to a position in the map database and the possible future paths of the vehicle are predicted using the road network.

The performance of the path prediction algorithm has been evaluated on recorded night vision sequences corresponding to 15 hours of driving. The results show that map-based path prediction algorithms are superior to dead-reckoning methods for longer time horizons.

It has also been investigated whether vision-based lane detection and tracking can be used to improve the path prediction. A prediction method using lane markings has been implemented and evaluated on recorded sequences. Based on the results, the conclusion is that lane detection can be used to support a path prediction system when lane markings are clearly visible.

(8)

(9)

Acknowledgments

First I would like to thank Jonas Elbornsson Skogstjärna at Autoliv Electronics for all his help during this master’s thesis. Of great assistance have also Tohid Ardeshiri and my examiner Thomas Schön at Linköping University been, giving feedback and providing valuable comments on the report.

Furthermore, I want to thank Mattias Ragnehed for helping me with lane detec-tion, Stefan Ivehammar for help with framework problems and all the staff at Autoliv Electronics for making my time there enjoyable.

Linköping, June 2011 Johannes Fri

(10)

(11)

6 Lane Detection and Tracking 35 6.1 Lane Detection . . . 36 6.2 Line Tracking . . . 38 6.3 Lane Tracking . . . 39 6.4 Path Prediction . . . 40 6.5 Discussion . . . 41 7 Results 43 7.1 Performance Evaluation . . . 43 7.1.1 Sequence A . . . 49 7.1.2 Sequence B . . . 51 7.1.3 Sequence C . . . 53 7.1.4 Sequence D . . . 54 7.1.5 Sequence E . . . 55 7.1.6 Sequence F . . . 56 7.1.7 Sequence G . . . 57 7.1.8 Sequence H . . . 58 7.1.9 Sequence I . . . 59 7.1.10 Sequence J . . . 60 7.1.11 Sequence K . . . 61

7.2 Evaluation of Lane-based Prediction . . . 62

7.3 Visualizations . . . 64 8 Concluding Remarks 67 8.1 Conclusions . . . 67 8.2 Future work . . . 68 Bibliography 69 A Abbreviations 73

(13)

Contents xi

List of Figures

1.1 FIR Image . . . 2

1.2 NIR Image . . . 3

3.1 Speed Comparison Between CAN and GPS . . . 12

3.2 Course Comparison . . . 13

3.3 The Coordinate System . . . 14

3.4 EKF Trajectory . . . 18

3.5 EKF Innovations . . . 19

4.1 One Problem with Point-to-Point Matching . . . 22

4.2 One Problem with Point-to-Curve Matching . . . 23

4.3 One Problem with Curve-to-Curve Matching . . . 23

4.4 Result from Improved Point-to-Curve . . . 25

5.1 Path Prediction in a Curve . . . 28

5.2 Example of Dead-reckoning Methods . . . 29

5.3 Example of Map Based Path Predictions . . . 31

5.4 Weighted Path Prediction Example . . . 32

5.5 Path Prediction Sequence . . . 33

6.1 Gradient Image . . . 36

6.2 Lane Marking Extraction . . . 37

6.3 Result from Lane Detection . . . 38

6.4 Result from Line Tracker . . . 39

6.5 Result from Lane Tracking . . . 39

6.6 Lane-based Prediction . . . 40

6.7 Lane Keeping Assumption . . . 40

6.8 Detection Problem . . . 41

6.9 Detection Problem 2 . . . 41

7.1 Performance Evaluation: Mean Absolute Error . . . 44

7.2 Performance Evaluation: Root Mean Square Error . . . 45

7.3 Example of Prediction Errors . . . 46

7.4 Prediction Errors Plotted On Map . . . 47

7.5 Performance Evaluation in Turns: Mean Absolute Error . . . 48

7.6 Performance Evaluation in Turns: Root Mean Square Error . . . . 48

7.7 Sequence A: Trajectory Plot . . . 49

7.8 Sequence A: Average Error . . . 50

7.9 Sequence A: RMSE . . . 50

7.10 Sequence B: Trajectory Plot . . . 51

7.11 Sequence B: Average Errors . . . 52

7.12 Sequence B: Average Errors in Turns . . . 52

7.13 Sequence C: Trajectory Plot . . . 53

7.14 Sequence C: Average Errors . . . 53

(14)

xii Contents

7.16 Sequence D: Average Errors . . . 54

7.17 Sequence E: Trajectory Plot . . . 55

7.18 Sequence E: Average Errors . . . 55

7.19 Sequence F: Trajectory Plot . . . 56

7.20 Sequence F: Average Errors . . . 56

7.21 Sequence G: Trajectory Plot . . . 57

7.22 Sequence G: Average Errors . . . 57

7.23 Sequence H: Trajectory Plot . . . 58

7.24 Sequence H: Average Errors . . . 58

7.25 Sequence I: Trajectory Plot . . . 59

7.26 Sequence I: Average Errors . . . 59

7.27 Sequence J: Trajectory Plot . . . 60

7.28 Sequence J: Average Errors . . . 60

7.29 Sequence K: Trajectory Plot . . . 61

7.30 Sequence K: Average Errors . . . 61

7.31 Lane Evaluation Night . . . 62

7.32 Lane Evaluation Day . . . 63

7.33 Path Visualisation . . . 64 7.34 Path Visualisation 2 . . . 64 7.35 Predicting a Curve 1 . . . 65 7.36 Predicting a Curve 2 . . . 65 7.37 Predicting a Curve 3 . . . 65 7.38 Roundabout Example . . . 66 7.39 Multiple Paths . . . 66

List of Tables

7.1 Overview of Methods . . . 44 7.2 Relative Performance . . . 45

(15)

Chapter 1

Introduction

1.1 Background

Vehicle navigation systems which utilize the Global Positioning System (GPS) and digital map databases have become popular in recent years. Most such systems are used to aid the driver in navigating but with the increasing accuracy of digital maps they can also be used for estimating the future path of the vehicle. For active safety systems such as lane departure warning and collision avoidance, a prediction of the future path is very useful in order to take preemptive actions. This thesis focuses on a prediction algorithm for a night vision system with pedes-trian detection. The purpose of the system is to increase the visibility in darkness, but also to detect and warn for pedestrians in danger of being hit by the car. To determine if the system should give a warning when a pedestrian is detected, the system must know where the car will be up to four seconds ahead in time. Given the long prediction horizon, it is difficult to predict the vehicle position only based on vehicle dynamics. Therefore, a prediction of the road ahead is of interest since that is the most probable path of the vehicle.

In this thesis, a map-based method for predicting the future path of a vehicle is presented. The method is aimed for use in a night vision system but could be used in any system requiring path predictions for 1-10 seconds ahead in time.

1.2 Night Vision

The traffic risk at night is higher than during daylight hours, especially for vul-nerable road users such as pedestrians and cyclists. In recent years, night vision systems have therefore been introduced for vehicles to improve the safety of night time driving [26].

(16)

2 Introduction

Night vision systems use infrared cameras to capture images which are presented for the driver, thus increasing the driver’s range of sight in darkness. Newer night vision systems are also capable of detecting pedestrians and warn the driver if they are in danger of being hit by the car.

There are currently two main types of night vision technologies: far infrared (FIR) and near infrared (NIR) systems.

FIR systems are passive systems which detect thermal radiation and turn it into an image, see Figure 1.1. Humans and animals are clearly visible in the FIR camera since the radiation from body heat lies within the far infrared spectrum (8-12 µm).

Figure 1.1. An image example from a FIR system. Bright objects are warmer than

dark objects.

NIR systems are active systems which illuminate the scene ahead of the car in the near infrared spectrum (0.7-1 µm) and then detect the reflected radiation. The image provided by a NIR system has higher resolution and looks more realistic than the FIR image, see Figure 1.2.

Other benefits of the NIR camera is that it has a lower cost than the FIR camera and it can also be mounted behind the windshield of the car (the windshield is not transparent for the FIR wavelengths). The NIR camera can therefore be better protected and also have a better angle towards the road.

The main advantage of the FIR system is that pedestrians are easier to detect due to the contrast between warm and cold objects. Furthermore, the FIR system have much longer range than the NIR system and requires no illumination.

(17)

1.3 Problem Statement and Objectives 3

Figure 1.2. An image example from a NIR system. The NIR image looks more realistic

than the FIR image but pedestrians are harder to spot.

1.3 Problem Statement and Objectives

Autoliv has developed a night vision system with the purpose to issue a warning for a pedestrian in danger of being hit by the car. The system consists of a far-infrared (FIR) camera, an electronic control unit (ECU) with algorithms for detection of pedestrians and a display where the infrared image is shown.

The system detects objects (possible pedestrians) in every frame. The detected objects are sent to a tracker that connects the objects between frames and esti-mates world information, e.g. time to impact (TTI). After that, a decision is made whether the object should issue a warning or not.

The system should warn for objects that are on the road or close to the road and moving towards the road. It should not warn for objects on the side of the road that are not moving towards the road, e.g. pedestrians walking on the sidewalk along the road.

In order to find out if the system should give a warning or not, the system must know where the car will be up to four seconds ahead in time, i.e. a prediction of the road geometry is needed for the road ahead of the car on a distance corresponding to four seconds of driving.

The purpose of the master’s thesis is the following:

• To investigate the accuracy of path prediction based on GPS position to-gether with a map database and data from the Controller Area Network (CAN) bus.

• To investigate if lane markings can be used to improve the prediction of the path.

• To make a test-implementation in Matlab based on the investigations above and evaluate the performance on recorded night vision sequences.

(18)

4 Introduction

1.4 Limitations

The map-based path prediction method will be used as a supporting system to an already existing system. Therefore it only has to perform well when a GPS signal exists and the map database has sufficient precision. An uncertainty measure is implemented to determine if the map should be used for the path prediction. Only a two-dimensional path will be predicted.

The method will be evaluated offline on recorded data sequences.

1.5 Related Work

Much research is currently being pursued in the area of vehicle navigation, espe-cially regarding systems utilizing GPS and map databases. An overview of vehicle positioning and navigation is given in [25]. Other common applications for the combination of digital maps and GPS are monitoring and tracking vehicle move-ment and location-based services [8].

Map databases are also very useful for estimating the future path of a vehicle. With the increasing accuracy of digital maps, many safety-related systems have started to use map-based predictions in order to take preemptive actions. Advanced driver assistance systems (ADAS) use GPS receivers, vehicle motion sensors, cameras and road maps for applications such as lane departure warning [2], collision avoidance [10], adaptive light control etc. Digital maps can also be used to help determine the upcoming road curvature [19].

To get an accurate estimate of the current position and heading of the car, the standard approach is to use an extended Kalman filter (EKF). An overview of the Kalman Filter can be found in [4] and positioning problems similar to what is studied in this thesis have been solved in e.g. [1, 3, 5, 16, 18].

In order to use the map for prediction, the current position must also be matched to a road in the map database. An introduction to the general problem of map matching is provided in [7].

One method for matching a position to a road is point-to-curve map matching [7, 22, 23]. This is a simple minimum distance method which can easily be extended using additional information such as vehicle heading, road directions and previous matches.

Map matching is generally a complicated problem and there are many advanced algorithms that can be used [8, 17]. In [11], belief theory is used to find the probabilities of different matches. Most map matching methods update the vehicle position to the road’s centerline but if precise positioning is required it can be better to calculate the offset to the map, see [6]. Map matching can also be used instead of GPS to find the absolute position. An example of this is shown in [15],

(19)

1.6 Autoliv 5

where a navigation system based on dead-reckoning, a digital map and a particle filter is described.

After localization and map matching, the future path can be predicted assuming the car continues on the current road. If multiple paths are possible, for example when approaching an intersection, all of them should be calculated. Other infor-mation (e.g. turn signals and steering angle) can then later be used to identify the correct path.

An algorithm that uses the visual information from the camera is also investigated in this thesis. The idea is that lane markings on the road can be used to get infor-mation about the upcoming path. Vision-based lane detection using a standard camera is described in [9] and [20]. Visual odometry using FIR images is used in [21] and [24].

1.6 Autoliv

This master’s thesis project has been performed at Autoliv Electronics in Linköping. Autoliv is the worldwide leader in automotive safety, known for developing and manufacturing standard safety products such as airbags and seatbelts. The pur-pose of these passive safety systems is to reduce the consequences of an accident. Autoliv is also developing active safety systems which aim to prevent accidents or mitigate their effects before they occur. Examples of active safety systems are night vision, vision and radar systems.

More information about Autoliv can be found on www.autoliv.com.

1.7 Thesis Outline

Chapter 2 discusses digital maps and GPS while Chapter 3 defines the models and algorithms used for estimating the position of the car. Chapter 4 gives an introduction to map matching and describes the selected map matching method. Different path prediction methods are described in Chapter 5 and lane detection and tracking is investigated in Chapter 6. Chapter 7 shows the results from the performance evaluation and Chapter 8 gives conclusions and directions for future research.

(20)

(21)

Chapter 2

GPS and Digital Maps

2.1 GPS

The Global Positioning System (GPS) is a satellite-based navigation system that provides location and time information. GPS is designed for global coverage at all times, using around 24 satellites in six orbital planes around the earth. Each satellite carries a high-precision atomic clock and broadcasts messages containing its location at known times. A receiver on the ground can therefore use the signal propagation time to estimate a so called pseudorange. Pseudoranges from four different satellites are needed to determine the 3D-coordinates of the receiver [1, 25].

The position obtained from a GPS receiver is accurate down to a few meters. The accuracy is affected by satellite positions, radio signal noise and atmospheric delays.

Since the most common errors are the same for all GPS receivers in a local area, a correction can be calculated by having a stationary receiver at a known location. This method can improve the accuracy of the system significantly and is commonly referred to as differential GPS (DGPS).

One problem with GPS is that it suffers from disturbances and the signal may occasionally be lost, especially in dense urban areas and tunnels. Therefore it is often not possible to only rely on GPS for positioning when high update frequency is required.

2.2 Digital Maps

Digital maps used in navigation systems, i.e. vector encoded maps, differ sig-nificantly from classical maps in their representation. While classical maps are

(22)

8 GPS and Digital Maps

represented by images, vector encoded maps are built up as databases containing only the most important features of the area. This makes vector encoded maps much easier and faster to interpret by a computer [15, 25].

A common approach for navigation systems is to use a vector encoded map for calculations and a digitalized classical map for display purposes.

In vector encoded maps, the road network is generally represented by a set of arcs (two-dimensional piecewise linear curves). Each arc represents the centerline of a road and can be described with a finite number of points. The first and last points of an arc are referred to as nodes, points where it is possible to go from one arc to another. The remaining points are called shape points.

All maps contain errors and digital maps are no exceptions. The position errors in the map database typically come from three sources:

• Error in the digitization when the map is created. • Error because physical roads are represented by arcs. • Error because the road network is not up to date.

The two largest digital map providers are Tele Atlas (www.teleatlas.com) and Navteq (www.navteq.com). They both provide maps covering most of the world with sufficient precision for vehicle navigation systems. Another digital map-ping company is Intermap Technologies (www.intermap.com), which is devel-oping 3D-maps for ADAS applications. There also exist many local map sup-pliers, usually with coverage of a single country. One example is Lantmäteriet (www.lantmateriet.se), which provides map data of Sweden only.

In this work, the map data is collected from OpenStreetMap (www.openstreetmap. org), which is described in the following section.

2.2.1 OpenStreetMap

The OpenStreetMap (OSM) project is a collaborative project developing an online open source map of the world. The maps are created using GPS tracks, out-of-copyright maps and aerial photography.

OSM maps are available for free and can in some locations compete with the com-mercially available maps. However, the accuracy can differ significantly between areas and there is no guarantee of precision.

OpenStreetMap uses a data structure containing nodes, ways and relations. • Nodes are all points with a geographic position. This means that shape

points as defined in Section 2.2 are also referred to as nodes in OSM. • Ways are lists of nodes, thereby representing piecewise linear arcs. • Relations are groups of nodes, ways and other relations.

(23)

2.3 Map Projections 9

Nodes, ways and relations may also contain describing tags. Tag information can be used to select certain data from a map, e.g. to find roads of a specific type. The path prediction method in this thesis uses maps from OpenStreetMap and a parser for OSM maps in xml format has been implemented in Matlab. To improve performance the parser only selects roads with certain tags, e.g. highways, while other roads are not used at all, e.g. bicycle routes and pedestrian walkways. Roads tagged as service ways, typically roads in parking areas, are also removed from the map used in this thesis.

2.3 Map Projections

Both GPS and OpenStreetMap use the reference coordinate system World Geode-tic System 1984 (WGS-84), which utilizes an ellipsoidal model to approximate the Earth. A position on the surface of the earth can then be specified by two coordinates, i.e. latitude and longitude.

To simplify later calculations, the WGS-84 coordinates from the GPS and the map database are projected to the surface of the Earth. A simple approach is to use the cylindrical projection given by Algorithm 2.1.

Algorithm 2.1 Cylindrical Projection

A cylindrical projection from WGS-84 latitude ϕ and longitude λ in radians to planar coordinates (x, y) in meters relative a reference point (ϕ0, λ0) is given by

x = R(λ − λ0) cos(ϕ0) (2.1)

y = R(ϕ − ϕ0) (2.2)

where R is the earth equatorial radius (6378.137 km).

While the simple cylindrical projection seems to work adequately for small maps, it may affect the performance negatively far away from the reference point. Com-monly, a more precise map projection such as the Transverse Mercator map pro-jection is therefore employed in national and international mapping systems. The Universal Transverse Mercator (UTM) coordinate system is a widely used system of map projections designed to be used worldwide [14]. The system di-vides the surface of Earth into 60 different zones and uses an angle-preserving map projection for each zone. A position on the Earth is then specified by the UTM zone and a Cartesian coordinate pair (E, N ) known as easting and northing. The coordinates are commonly measured in meters and refer to the eastward and northward distance from a zone-specific origin.

(24)

10 GPS and Digital Maps

In this thesis a simplified variation of UTM is used, see Algorithm 2.2, where the reference longitude is calculated from the map instead of from the UTM zones [14]. Furthermore, the easting and northing coordinates are translated in order to focus on the interesting area.

Algorithm 2.2 UTM Projection

A simplified formula for converting the WGS-84 coordinates latitude, ϕ, and longitude, λ, in radians into UTM coordinates (E, N ) in meters is

E = E0+ k0Rv(A + (1 − T + C)A3/6 + (5 − 18T + T2)A5/120) N = N0+ k0R(s(ϕ) + v tan(ϕ)(A2/2 + (5 − T + 9C + 4C2)A4/24 + (61 − 58T + T2)A6/720)) where R = 6378137 e = 0.0878192 v = 1/p(1 − e2sin2(ϕ)) A = (λ − λ0) cos(ϕ)

s(ϕ) = (1 − e2/4 − 3e4/4 − 5e6/256)ϕ − (3e2/8 + 3e4/32 + 45e6/1024) sin(2ϕ)

+ (15e4/256 + 45e6/1024) sin(4ϕ) − 35e6/3072 sin(6ϕ) T = tan2(ϕ)

C = e2/(1 − e2) cos2(ϕ)

E0= 500000

N0= 0

(25)

Chapter 3

Vehicle Positioning

Positioning is the process of determining the global coordinates of an object. Vehi-cle positioning systems (see [25] for an overview) typically use GPS technology as their main information source. However, as described in Section 2.1, GPS suffers from signal masking and may not be reliable as a stand-alone positioning system. A common approach is therefore to use a dead-reckoning system supported by GPS measurements for localization.

Dead-reckoning systems, i.e. navigation systems measuring only relative position, typically employ sensors with much higher sample frequency than GPS. However, the error in a position estimate computed using only dead-reckoning grows without bound.

The advantage of using reckoning together with GPS is that the dead-reckoning system smooths out the short-term GPS errors while the absolute posi-tion measurements from GPS calibrate the sensor drift over longer time periods. The fusion of dead-reckoning and GPS can therefore provide positioning perfor-mance that is superior to that of either system used alone.

3.1 Data Acquisition

The data used in this thesis have been collected from several driving sessions with a Mercedes S500 test vehicle. The car is equipped with a night vision system and a GPS receiver. Longitudinal acceleration, longitudinal velocity (speed) and yaw rate measurements are available through the vehicle’s CAN bus. The GPS receiver is situated on the right hand side of the car and provides position measurements with a sample period of one second and a delay of up to one sample period. In order to use the data, both the GPS data and the CAN data first need to be parsed from their raw format (NMEA text format and binary format respectively). The CAN data is sampled with a frequency of 25 Hz.

(26)

12 Vehicle Positioning

The messages used for NMEA data are GPGGA and GPRMC. The GPGGA mes-sage contains the position fix in WGS-84 latitude and longitude and the GPRMC message provides measurements of course and speed over ground.

The speed and course measurements from the GPS as well as the acceleration from CAN data are in this thesis used for comparison purposes only. When added to the model these measurements did not improve the estimated positions. One reason for this is that the information provided by the speed and course from the GPS is already indirectly given by other measurements, i.e. CAN speed and GPS positions. A comparison between the speed measurements from CAN and GPS can be seen in Figure 3.1 and a comparison of estimated course and GPS course is shown in Figure 3.2.

The longitudinal acceleration measurements may provide some new information but with a sample rate of 25 Hz, speed variations are already sufficiently accurate. Furthermore, the road inclination together with the assumption of a flat world seems to affect the acceleration measurements too much for them to be very useful.

0 500 1000 1500 2000 2500 3000 3500 4000 4500 2 4 6 8 10 12 14 16 18 20 22 Sample Nr Speed (m/s) CAN Speed GPS Speed

(27)

3.2 Sensor Models 13 0 500 1000 1500 2000 2500 3000 3500 −1 −0.5 0 0.5 1 1.5 2 Sample Nr Course (rad) Estimated course GPS course

Figure 3.2. A comparison between estimated course and GPS course.

3.2 Sensor Models

The GPS measurements are modeled as

EGPS_k = Ek−d+ e1k (3.1)

NkGPS= Nk−d+ e2k (3.2)

where EGPS_{and N}GPS_{are the UTM transformed coordinates corresponding to the} latitude and longitude from the GPS receiver and d is the GPS delay. The subscript

k is used to indicate the value at time tk. The measurement noise given by e1kand

e2_kis assumed to be uncorrelated and Gaussian, with Cov(e1_k) = Cov(e2_k) = 100 m2. The speed and yaw rate measurements from the CAN bus, vCAN

k and ˙ψkCAN, are

modeled as signals with a time-varying offset.

vCANk = vk+ vkoffset+ e v k (3.3) ˙ ψCANk = ˙ψk+ ˙ψkoffset+ e ˙ ψ k (3.4)

The offset values in (3.3) and (3.4) can be computed and removed from the signals beforehand. Furthermore, the measurements show that the noise ev_khas a standard deviation of 0.04 m/s and eψ_k˙ a standard deviation of 10−4 rad/s.

(28)

3.3 Vehicle Model

The dynamic model for the vehicle is given in equations (3.5)-(3.7) below.

Ek+1= Ek+ T vkcos(ψk) (3.5)

Nk+1= Nk+ T vksin(ψk) (3.6)

ψk+1= ψk+ T ˙ψk (3.7)

Here, T is the sample time, E and N the east and north position coordinates,

v is the speed, ψ is the yaw angle and ˙ψ is the yaw rate. The coordinates and

orientation of the vehicle are defined as in Figure 3.3.

-East 6 North uψ N E

Figure 3.3. The coordinate system consists of east and north components, (E, N ),

relative to a map reference point. The yaw angle ψ is measured from the east axis to the direction of travel.

The dynamic model introduced above is obviously a very simplified model of a vehicle. More complicated models can be used for example if additional sensor data is available or if the three-dimensional position is of interest.

As stated in the introduction to this chapter, a dead-reckoning system consisting only of the vehicle model could be used alone to estimate the position. However, sensor inaccuracy and the assumptions of constant speed, yaw rate and yaw angle over a sample period will lead to errors. These errors will then accumulate over time and the position estimates will eventually be completely incorrect.

To solve these problems the vehicle model and the sensor models are fused together using an extended Kalman filter. An introduction to the Kalman filter is given in Section 3.4 and the extended Kalman filter employed in this thesis is presented in Section 3.5.

(29)

3.4 Kalman Filter 15

3.4 Kalman Filter

The Kalman filter [4, 12, 13, 16] is an optimal estimation method that is widely used in many technical applications. A Kalman filter estimates the states of a system by determining how to combine measurements with a dynamical model. The state estimates provided by the filter are optimal in the sense that they minimize the expected square-error between the estimates and the true system states [4].

Consider the following state space model

xk+1= Fkxk+ wk (3.8)

yk = Hkxk+ ek (3.9)

where Fk and Hk may be time-varying matrices, x is the state vector and y is the

measurement vector.

If the process noise wkand the measurement noise ek can be represented as

uncor-related Gaussian noise with Cov(wk) = Qk and Cov(ek) = Rk, the Kalman filter

is given by Algorithm 3.1. The filter is often divided in two steps, referred to as the time update and the measurement update.

Algorithm 3.1 Kalman Filter

where the Kalman gain K is given by

(30)

3.5 Extended Kalman Filter

The sensor models from Section 3.2 and the vehicle model from Section 3.3 can be combined, resulting in

xk+1= f (xk) + wk (3.15)

yGPS_k = hGPS(xk) + eGPSk (3.16)

yCAN_k = hCAN(xk) + eCANk (3.17)

with the state vector

xk=       Ek Nk ψk vk ˙ ψk       (3.18)

and the functions f , hGPS _{and h}CAN _{given by}

f (xk) =       Ek+ T vkcos(ψk) Nk+ T vksin(ψk) ψk+ T ˙ψk vk ˙ ψk       (3.19) hGPS(xk) = Ek Nk (3.20) hCAN(xk) =  vk ˙ ψk (3.21)

This is a nonlinear model, so the Kalman filter cannot be used directly. The most commonly used approximation is provided by the extended Kalman filter (EKF). The idea of the EKF is to linearize the nonlinear functions around the current state estimate and then apply the standard Kalman filter [4, 12, 13]. The method is described in Algorithm 3.2.

Unfortunately, the linearization in the EKF can affect the accuracy of the solution and the filter is therefore suboptimal. In systems of very nonlinear nature and with non-Gaussian noise, more refined nonlinear filtering methods may be needed to improve system performance.

(31)

3.5 Extended Kalman Filter 17

Algorithm 3.2 Extended Kalman Filter

For the dynamic system (3.15)-(3.21), the EKF can be applied with

Fk = ∂f ∂x _ˆ_x k|k−1 =       1 0 −T ˆvk|k−1sin( ˆψk|k−1) T cos( ˆψk|k−1) 0 0 1 T ˆvk|k−1cos( ˆψk|k−1) T sin( ˆψk|k−1) 0 0 0 1 0 T 0 0 0 1 0 0 0 0 0 1       (3.29) H_kGPS= ∂h GPS ∂x _ˆ_x k|k−1 =       1 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0       (3.30) H_kCAN= ∂h CAN ∂x _x_ˆ k|k−1 =       0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 1       (3.31)

Note that the GPS measurement update is only done when new GPS measure-ments occur (1 Hz), while the time update and the CAN measurement update are executed at every time step (25 Hz).

(32)

An illustration of the result from the EKF can be seen in Figure 3.4. The trajectory is smooth with small corrections when a new GPS measurement arrives. It is also clear that the GPS positions have an offset compared to the map. The main reason for this is that the map contains only the centerline of the roads, when in reality they have a width. As it is quite common for the car to travel in a lane on the side of the road, an offset is therefore expected. Furthermore, the GPS receiver is situated on the right hand side of the car, which also results in a small offset.

2880 2900 2920 2940 2960 2980 3000 3020 3040 3060 1840 1860 1880 1900 1920 1940 1960 1980 2000 2020 2040 E (m) N (m)

Figure 3.4. The resulting trajectory from the EKF (cyan) plotted together with the

map (grey) and GPS position fixes (blue crosses)

3.6 Discussion

Evaluating the accuracy of the estimated trajectory can be quite hard since there is no ground truth available. Comparison with the digital map may not be the best approach, since there are errors both in the map and the estimation. Also, since there are different road widths in different locations the road width offset makes a comparison even more difficult.

Some sequences have been evaluated together with aerial photographs. The con-clusion that can be drawn is that both the EKF trajectory and the map contain errors due to GPS inaccuracy. However, in general the EKF trajectory seems to fit well with the photographs.

(33)

3.6 Discussion 19

The residuals from the extended Kalman filter can also be used as a performance indicator. As seen in Figure 3.5, they show that the difference between an esti-mated position and a GPS measurement in general is less than 1 meter.

0 500 1000 1500 2000 2500 3000 3500 4000 4500 −1.5 −1 −0.5 0 0.5 1 1.5 Sample Nr Innovation (m) E N

(34)

(35)

Chapter 4

Map Matching

Map matching is the method to determine which road the car is traveling on and where on the road the car is situated. This is done by comparing the estimated trajectory from the positioning method with the road network from the digital map. If the map quality is sufficiently good, the result of the map matching can also be used to improve the state estimates as well as correct sensor errors. An example of this can be seen in [15] where a vehicle navigation system using only wheel speed sensors and map matching is described.

In this chapter, some existing map matching methods are investigated in an ap-proach similar to the one used in [7] and [23]. Possible improvements are then discussed before the improved map matching algorithm used in this thesis is de-scribed and evaluated.

4.1 Point-to-Point

One technique to match the position estimate to a road is to simply match it to the closest node or shape point in the road network. This algorithm is called point-to-point matching.

The closest node is usually defined as the one with the shortest Euclidean distance to the position estimate. The Euclidean distance between two points X and Y in R2is given by

kX − Y k2= p

(X1− Y1)2+ (X2− Y2)2 (4.1)

Point-to-point matching is a fast method which is easy to implement, but the performance can be quite bad in practice. One problem can be seen in Figure 4.1, where the estimated position is matched to the road with higher node resolution.

(36)

22 Map Matching r r r r r b A1 _A2 B1 _B2 _B3 X

Figure 4.1. One problem with point-to-point matching. The estimated position X is

matched to the closest point B2_{, even though it is clear that the road from A}1 _{to A}2 _is

a more likely candidate.

4.2 Point-to-Curve

A more sophisticated method is to instead match the position to the closest arc in the road network, i.e. point-to-curve matching. Since the arcs in this thesis are assumed to be piecewise linear, the closest arc is found after calculating the minimum distance to each line segment of each arc. Algorithm 4.1 is used to project a point to a line and the complete point-to-curve method is described by Algorithm 4.2.

Algorithm 4.1 Projection to Line

The point P on a line from A to B closest to the point X is

P =    A, K < 0 A + K(B − A), 0 ≤ K ≤ 1 B, K > 1 (4.2) where K =(X − A) T_{(B − A)} (B − A)T_{(B − A)} (4.3) Algorithm 4.2 Point-to-Curve

For each segment in curve.

Project the estimated position to the line segment using Algorithm 4.1. Calculate the distance between the projection and the estimated position. Select the projection with the minimum distance.

The main disadvantage of point-to-curve matching is that it only uses momen-tary measurements. This can be seen in Figure 4.2 where the lack of historical information leads to a bad match.

(37)

4.3 Curve-to-Curve 23 r r r b b b A1 A2 B1 B2 X0 X1 X2

Figure 4.2. One problem with point-to-curve matching. The estimated position X2 _is

matched to the road B. However, given the previous estimates X0 and X1 it seems it would be better to match X2 _{to the road A.}

4.3 Curve-to-Curve

One approach to improve the point-to-curve method is to also use historical in-formation. The idea with curve-to-curve matching is to first construct a piecewise linear curve from the current position estimate together with previous estimates. This curve can then be matched to the curves in the map database.

While this approach solves some of the problems from point-to-curve, it also intro-duces some difficulties in how to define the distance between curves. Furthermore, curve-to-curve matching is quite unpredictable and can give unexpected results, see e.g. Figure 4.3.

r r r r b b b b b A1 _A2 _A3 B1 B2 X1 _X5

Figure 4.3. One problem with curve-to-curve matching. The sequence given by

(38)

24 Map Matching

4.4 Improving the Map Matching

Instead of relying only on geometrical information (e.g. distance and arc shapes), it is also possible to use the connectivity of arcs in the network for map matching, see e.g. [17]. As an example, a position previously matched to an arc A should not be matched to a new arc B unless A and B are connected.

Map matching methods using connectivity or other topological information gen-erally perform better than pure geometrical methods. They can also reduce the computational load since arcs that are unreachable from the previous match within one sample period can be eliminated from the candidate set. However, methods using connectivity can be quite complex to implement and they also have the drawback that one bad match may lead to an entire sequence of errors.

If the confidence region of the position estimate or the precision of the map is known, they can also be used to improve the map matching. Probabilistic map matching algorithms use these confidence values to find the most probable road segment and estimate the vehicle location. This approach is less sensitive to out-liers and can give better performance than deterministic methods. A probabilistic map matching method based on a particle filter is described in [15] and a method using an extended Kalman filter is presented in [23].

Another approach to improve the map matching is to use information from other sensors in combination with the map. In this thesis the estimated vehicle heading is used together with a point-to-curve approach. The method is described in the following section.

4.5 Improved Point-to-Curve

Knowledge of the heading of the vehicle can be used to solve most of the problem from the ordinary point-to-curve method. The idea is that the best match should have a short distance to the estimated position but also be a part of a road with a direction similar to the estimated heading. The method is described in Algorithm 4.3.

Algorithm 4.3 Point-to-Curve with Heading

Select all possible roads (i.e. roads that are in the surrounding area of the posi-tion estimate).

Use point-to-curve matching to find candidate positions from each road. Calculate a matchvalue for each candidate.

Match the estimated position to the candidate with the best (lowest) match-value.

(39)

The matchvalue for a candidate is calculated using the distance to the estimated position, d, and the angular difference between the direction of the candidate road and the heading of the vehicle, φ. The resulting formula is

matchValue = k1d + k2φ (4.4)

where

k1= 1 m−1 (4.5)

k2= 30 rad−1 (4.6)

As seen in Figure 4.4, the improved point-to-curve algorithm matches the es-timated position to the correct road even if another road is closer, due to the angular difference between the road direction and the vehicle heading.

4305 4310 4315 4320 4325 4980 4982 4984 4986 4988 4990 4992 4994 4996 4998 5000 E (m) N (m)

Figure 4.4. Result from the improved point-to-curve map matching. The estimated

positions (blue crosses) are matched to the correct positions (red circles) on the road using the heading of the vehicle.

4.6 Discussion

Even though the improved point-to-curve algorithm is conceptually simple, the performance of the method is generally good. Many problems of map matching methods are due to low sample rate, which in this case is solved with the EKF-filtering of the input. The availability of an accurate vehicle heading estimate is also a big advantage for this method.

(40)

26 Map Matching

However, there are some situations where the improved point-to-curve method has problems. One example is that a position estimate between two parallel roads is hard to match correctly with this method. Roundabouts are also troublesome, as the direction of the road changes rapidly and there are many possible paths. Furthermore, the error due to piece-wise linear representation of roads is usually larger in roundabouts and it is also more common for drivers to cut across lanes. Inspired by [11], an uncertainty value has therefore been added to the map match-ing algorithm. The uncertainty value is calculated from the current matchvalue as well as the matchvalues of other candidate positions and can be used to determine if a map-based path prediction algorithm should be applied. A large uncertainty value is an indication that the estimated position does not fit anywhere in the map database. The cause of this is typically a bad localization (e.g. due to GPS signal masking) or an error in the road network (e.g. missing road, parking area).

(41)

Chapter 5

Path Prediction

Predicting the future path of a vehicle can be quite difficult, especially with a long prediction horizon. Consider for example that it is not uncommon for a car to drive straight ahead for a long time and then suddenly make a sharp turn. Using only the current measurements from the in-car sensors, these types of actions are impossible to predict beforehand. However, assuming that the path of the vehicle is restricted by the road network, a map based path prediction method can foresee such events a long time before they happen.

This chapter first investigates some dead-reckoning based path prediction meth-ods which do not use any map information. Thereafter, the map based methmeth-ods implemented for this thesis are described and evaluated.

5.1 Dead-reckoning Methods

A simple approach to estimate the future path of the vehicle is to integrate the position ahead in time using only the current state. This is a fairly common method which does not require any extra information from GPS or maps. While stand-alone dead-reckoning methods are not the best approach for positioning (due to error growth), they can perform well for path prediction with a limited time horizon.

5.1.1 Linear Path Prediction

Perhaps the simplest idea for predicting the future path is to assume that the car continues straight ahead. The linear path prediction approach corresponds to not using any path prediction at all and is used as a reference in this thesis.

(42)

28 Path Prediction

5.1.2 Extrapolated Curve

Another natural approach is to predict the path according to a turn with constant yaw rate. This method is also very easy to implement and it performs well in general. However, there are two common situations when the performance of the extrapolated curve prediction is poor: At the start of a curve and at the end of a curve (see Figure 5.1).

5.1.3 Statistical Path Prediction

It is also possible to add statistical information about curves under different con-ditions to the previous methods. The statistical path prediction method uses curvature data collected from real driving to improve the performance of a yaw rate based prediction [26]. Curve symmetry assumptions are also employed to improve the performance at the end of a curve, see e.g. Figure 5.1.

1.794 1.795 1.796 1.797 1.798 1.799 x 104 4490 4495 4500 4505 4510 4515 4520 4525 4530 4535 4540 E (m) N (m)

Figure 5.1. At the end of a curve, the statistical path prediction (red curve) is much

better than the extrapolated curve (black) and the linear path prediction (yellow) when compared to the estimated trajectory (cyan).

It is also possible to collect statistical data while the car is driving, thus adapting after different road conditions. An example of the dead-reckoning path predictions can be seen in Figure 5.2.

(43)

5.2 Map Based Methods 29 2880 2900 2920 2940 2960 2980 1930 1940 1950 1960 1970 1980 1990 2000 2010 2020 2030 E (m) N (m)

Figure 5.2. Example of dead-reckoning methods. Linear path prediction (yellow), extrapolated curve (black), statistical path prediction (red) and estimated trajectory (cyan).

5.2 Map Based Methods

With the information from the localization and map matching steps, the future path of the vehicle can be predicted using the map database. This gives the map based methods the possibility to predict future changes in the trajectory which cannot be known by the dead-reckoning methods.

The drawback with map based path prediction methods is that they are much more difficult to implement than the dead-reckoning methods. First, a map has to be found and parsed to a suitable format (Chapter 2). Then the global position of the vehicle has to be determined (Chapter 3) and matched to the map (Chapter 4). Finally, the prediction step itself is not trivial due to the following problems:

• The position is estimated with errors.

• The coordinates of the arcs may contain error due to inaccurate map con-struction.

• The road network may not be up to date.

(44)

5.2.1 Map Aided Path Prediction

In order to predict the possible paths, a map aided path prediction algorithm has been implemented in Matlab. The algorithm takes the latest map matched position as a starting point and predicts all possible paths in a distance d given by

d = vTpred (5.1) where v is the current speed and Tpred is the prediction horizon.

For the night vision system in this thesis, four seconds prediction time is needed in order to warn the driver in time. However, as the car may accelerate and the predicted path may take a longer route than the actual path, a prediction time of six seconds is used, i.e. Tpred= 6.

The implemented method is described by Algorithm 5.1. The idea is to, from the starting position, go through all possible paths in the map network until the traveled distance is larger than the distance to predict.

Algorithm 5.1 Map Aided Path Prediction (MAPP)

1. Get the possible next nodes from the current node. 2. For each possible node.

3. Set node as current node.

4. Check traveled distance and compensate for overshoot. 5. Recursively go to 1.

The connectivity between roads is determined from the data structure (Section 2.2.1) and is used to find the possible next nodes. Possible nodes should also belong to roads with a direction difference less than 135◦ to the current road. Special cases such as roundabouts, which are connected to themselves, are also dealt with to ensure the robustness of the method. The algorithm uses an offset to keep the distance to the road centerline.

(45)

5.2 Map Based Methods 31

5.2.2 Smoothed Map Aided Path Prediction

Even though the MAPP algorithm performs well in minimizing the average dis-tance to the actual path, the shape of the resulting curve differs a lot from the actual path. This is mostly due to the fact that in reality the car does not travel exactly in the direction of the road, especially not in sharp crossings.

To solve this problem a smoothed version of the map aided prediction is imple-mented using Bézier curves. Bézier curves are parametric smoothing functions widely used in computer graphics. The Bézier Curve of degree n is given by

B(t) = n X i=0 n i (1 − t)n−itiPi, t ∈ [0, 1] (5.2)

where P0− Pn are the points representing the predicted path.

The Smoothed Map Aided Path Prediction is the resulting Bézier curve con-structed from the MAPP points. The minimum distance performance of the smoothed prediction is similar to the original, but the shape of the predicted trajectory is more accurate (see Figure 5.3).

2880 2900 2920 2940 2960 2980 1930 1940 1950 1960 1970 1980 1990 2000 2010 2020 2030 E (m) N (m)

Figure 5.3. Example of map based path predictions. Map aided (purple, dashed curve),

smoothed (green) and estimated trajectory (cyan).

As seen in Figure 5.3, the map aided methods predicts multiple paths. This is useful if e.g. warnings should be given for pedestrians in all possible paths. Otherwise, yaw rate and/or steering angle measurements can be used to only select the most probable path.

(46)

5.2.3 Weighted Path Prediction

When evaluating the performance of the different path predictors it is clear that the dead-reckoning methods perform well for short prediction horizons while the map based predictors are better further into the future. The idea behind the weighted prediction is therefore to use the extrapolated curve in the beginning of the prediction and the smoothed map aided path prediction at more distant times. The weighted prediction in this thesis is constructed using linear weight functions, thus coinciding with the extrapolated curve at the start and with smoothed MAPP at the prediction horizon. An example of the weighted path prediction can be seen in Figure 5.4 and a sequence comparing different predictions is shown in Figure 5.5. 2880 2900 2920 2940 2960 2980 1930 1940 1950 1960 1970 1980 1990 2000 2010 2020 2030 E (m) N (m)

Figure 5.4. Example of the weighted path prediction (blue curve) compared with the

(47)

5.2 Map Based Methods 33 2260 2280 2300 2320 2340 2360 2380 1500 1520 1540 1560 E (m) N (m) 2260 2280 2300 2320 2340 2360 2380 1500 1520 1540 1560 E (m) N (m) 2260 2280 2300 2320 2340 2360 2380 1500 1520 1540 1560 E (m) N (m) 2260 2280 2300 2320 2340 2360 2380 1500 1520 1540 1560 E (m) N (m)

Figure 5.5. Example sequence showing the weighted path prediction (blue curve), the

extrapolated curve (black), the statistical path prediction (red) and the estimated tra-jectory (cyan). The map-based method can predict future turns before they are noticed by the in-car sensors.

(48)

(49)

Chapter 6

Lane Detection and Tracking

Given that the path prediction algorithm is intended to be used with a night vision system, it is interesting to see if the visual information from the camera can be used to aid in predicting the future path. The approach investigated in this chapter is to use the lane markings on the road to determine where the vehicle is heading.

One problem is that for a passive night vision system with a FIR camera it is gen-erally not possible to detect lane markings, since there is no temperature difference between the markings and the road. However, the test-vehicle used in this thesis is also equipped with an active night vision camera operating in the near-infrared (NIR) region. The NIR camera is able to detect lane markings on the road and can also be placed behind the windshield of the car, thereby getting a better angle towards the road.

The vision-based path prediction method consists of four different steps.

• Lane Detection: Lane marking candidates are detected, grouped and fitted to a curved lane model.

• Line Tracker: A Kalman filter based tracker to make measurements more stable.

• Lane Tracker: A Kalman filter based tracker to build lane estimates from the tracked lines.

• Path Prediction: The future path is predicted using the estimated lanes. This approach is similar to that of lane departure warning (LDW) systems, which are designed to warn the driver when the vehicle starts to move out of its lane. To predict a possible lane departure, LDW systems detect the lane markings on the road and then determine the vehicle’s position and orientation relative to the detected lanes. Note that instead of the path prediction step above, LDW

(50)

36 Lane Detection and Tracking

methods include a step where lane departures are detected and warnings issued, see e.g. [9, 20].

6.1 Lane Detection

One approach to detect lane marking candidates is to find the edges of the lane markings. As the lane markings have a large contrast change, this can be done by calculating the gradient image [20]. The edges are then found using the local maximums and minimums of the gradient, see Figure 6.1. To improve performance, only a part of the image is used for lane detection.

Figure 6.1. The selected part of the image (top), the gradient image (middle) and the

local extremes (bottom).

To get the lane marking candidates, the gradient extremal points are grouped together, first horizontally and then vertically. The candidates are then merged and fitted to a curve model. This method is illustrated in Figure 6.2 where each group is represented by a different color.

The full method is summarized in Algorithm 6.1 and a visualization of the results from the lane detection can be seen in Figure 6.3.

(51)

6.1 Lane Detection 37

Figure 6.2. Lane marking extraction. The marking candidates are grouped horizontally

and vertically before they are merged and finally fitted to a curve model.

Algorithm 6.1 Lane Detection

1. Calculate the horizontal gradient image. Find local max/min.

2. Extract lane marking candidates by locating positive gradient followed by negative gradient.

3. Group marking candidates. 4. Fit groups to curve model.

(52)

Figure 6.3. Example result from the lane detection (green curves). The correct lane

markings are found, but also some false candidates.

6.2 Line Tracking

In order to make the lane detection measurements more stable, a Kalman filter based tracker is used. The filter updates the lines from the lane detection depend-ing on vehicle speed and yaw rate, keepdepend-ing only lines which are consistent durdepend-ing several frames.

A line is represented by the state vector x = (d, ˙d, φ, c, w)T where d is the lateral distance in meters, φ is the angle in radians between the heading direction of the vehicle and the line, c is the curvature of the line in 1/m and w is the width of the line in meters. All states are measured from the lane detection result and by using the motion model

d = d + T ˙d (6.1)

φ = φ + T ˙ψ (6.2) where ˙ψ is the vehicle yaw rate, the standard Kalman filter (Algorithm 3.1) can be

applied. Lines are tracked if they have been updated for 10 consecutive frames and dropped if they have not been updated for 5 frames. The resulting line tracking method is described in Algorithm 6.2.

Algorithm 6.2 Line Tracking

1. Time update based on the motion model 6.1-6.2.

2. Associate tracked lines with the results from lane detection. 3. Measurement update.

4. Create new tracks. 5. Drop inactive tracks.

Comparing the result after line tracking (Figure 6.4) with the lane detection result, it is clear that only stable measurements remain.

(53)

6.3 Lane Tracking 39

Figure 6.4. Example result from the line tracker (red curves). Only consistent

candi-dates remain.

6.3 Lane Tracking

The lane tracker module builds lanes from the tracked lines. A lane is constructed by two lines having similar curvature and with an acceptable distance (2.5 − 5 m) between them. The lane tracker module also uses a Kalman filter to track the lanes and drop inactive objects. The result from the lane tracker is seen in Figure 6.5.

Figure 6.5. Example result from the lane tracker (blue curves). With clearly visible

(54)

6.4 Path Prediction

Using the result from the lane tracker and assuming that the car continues in its current lane, the predicted path can easily be calculated by taking the average of the closest lane lines. To make the method more robust, predictions are only made if lane lines are available both to the left and to the right. An example of the lane-based prediction is shown in Figure 6.6.

Figure 6.6. Example of path prediction based on lane detection (green curve) and

tracked lanes (blue curves).

The lane-based path prediction is very dependent on correct lane detections and only performs well on larger roads with visible road markings. Similarly to the map-based methods, this method can also predict turns ahead in time. One prob-lem with this prediction is that the lane-keeping assumption may not always be correct, see e.g. Figure 6.7.

Figure 6.7. Example of a car exiting a highway. The lane-based method predicts a

(55)

6.5 Discussion

Even though the two Kalman filters sort out most of the unstable detections, the system may still give false lane detections due to contrast changes caused by e.g. traffic signs, street lights and other cars. An example of this can be seen in Figure 6.8 where a fence on the side of the road causes a false lane detection.

Figure 6.8. Output from line tracker (red curves) and lane tracker (blue curves). The

road fence on the left causes a false lane detection.

There are also problems due to the fact that lane markings are not always visible with dirt or snow covering the road. A typical example is shown in Figure 6.9.

Figure 6.9. Example where no lane markings are visible.

To summarize, the lane detection used in this thesis is easily disturbed, but works well on roads with clearly visible markings. Parameter tuning is extremely impor-tant and it is hard to find values that work well for all possible cases.

(56)

(57)

Chapter 7

Results

The performance of the path prediction algorithms is in this chapter evaluated on recorded night vision sequences. In order to adequately measure the effectiveness of the different methods a common performance metric is needed. A natural choice would be to use the orthogonal distance between the predicted path and the true path. However, this is not possible since the evaluation is made on recorded sequences where there are no ground truths available.

Instead, the estimated trajectory from the EKF is used as an approximated true path and the performance is measured as the shortest distance between the EKF trajectory and the predicted path. The errors caused by this approximation should be quite small considering the large number of evaluations (about 108 measure-ments for each method).

This chapter first presents the main results from the performance evaluation, fol-lowed by more detailed assessments for each data sequence and an extra evaluation for path predictions based on lane detection. To provide information on the per-formance in specific situations, the predicted paths and the estimated trajectory have also been visualized from the camera’s field of view.

7.1 Performance Evaluation

The path predictions described in this thesis has been evaluated on data from 15 hours of driving in different areas in Sweden, Germany and France. The perfor-mance has been measured as the orthogonal distance, d, between the predicted path and the approximated true path up to four seconds ahead in time. The data is then evaluated using the root mean square error (RMSE) and the mean absolute error (MAE) given by equations (7.1) and (7.2), where N is the total number of predictions (∼ 106_).

(58)

44 Results RMSE = v u u t 1 N N X i=1 d2 i (7.1) MAE = 1 N N X i=1 |di| (7.2)

Six different path prediction methods are evaluated in this section. The methods are described in Chapter 5 and are presented with colors and labels according to Table 7.1. The total results are shown in Figures 7.1 and 7.2.

Method Section Label Color

Linear Path Prediction 5.1.1 Straight Ahead Yellow

Extrapolated Curve 5.1.2 Extrapolated Curve Black Statistical Path Prediction 5.1.3 Statistical Red

Map Aided Path Prediction 5.2.1 Map Aided Purple

Smoothed MAPP 5.2.2 Smoothed Green

Weighted Path Prediction 5.2.3 Weighted Blue

Table 7.1. Overview of the evaluated methods.

0 0.5 1 1.5 2 2.5 3 3.5 4 0 0.5 1 1.5 2 2.5 Time (s)

Mean Absolute Error (m)

Straight Ahead Extrapolated Curve Statistical Map Aided Smoothed Weighted

(59)

7.1 Performance Evaluation 45 0 0.5 1 1.5 2 2.5 3 3.5 4 0 0.5 1 1.5 2 2.5 3 3.5 4 Time (s) RMSE (m) Straight Ahead Extrapolated Curve Statistical Map Aided Smoothed Weighted

Figure 7.2. The root mean square errors of the orthogonal distances between the predicted paths and the approximated true path.

The results show that the weighted path prediction is superior to the other methods at all prediction times. It can also be seen that the pure map-based methods perform worse than the dead-reckoning methods for short time horizons, but the map-based methods scale much better with the prediction horizon.

The relative performance has also been evaluated at specific times, i.e. at 1 s, 2 s, 3 s and 4 s into the future. The results are presented in Table 7.2, where the values represents the percentage of improved performance compared to if no prediction was used. For example, at four seconds prediction horizon the weighted path prediction is 51% better than predicting straight ahead, while the statistical path prediction is 18% better.

Method / Time 1s 2s 3s 4s

Extrapolated Curve 21 43 44 41

Statistical Path Prediction 14 24 21 18 Map Aided Path Prediction -151 -50 3 29

Smoothed MAPP -168 -64 -6 23

Weighted Path Prediction 21 44 49 51

Table 7.2. Relative performance calculated from the RMSE. The values show how many