Estimation and Pre-Processing of Sensor Data in Heavy Duty Vehicle Platooning

(1)

Institutionen för systemteknik

Department of Electrical Engineering

Examensarbete

Estimation and Pre-Processing of Sensor Data in

Heavy Duty Vehicle Platooning

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

av

Hanna Pettersson LiTH-ISY-EX--12/4592--SE

Linköping 2012

Department of Electrical Engineering Linköpings tekniska högskola

Linköpings universitet Linköpings universitet

(2)

(3)

Estimation and Pre-Processing of Sensor Data in

Heavy Duty Vehicle Platooning

Examensarbete utfört i Reglerteknik

vid Tekniska högskolan i Linköping

av

Hanna Pettersson LiTH-ISY-EX--12/4592--SE

Handledare: Assad Alam

Scania CV AB

Patrik Axelsson

isy, Linköpings universitet

Henrik Pettersson

Scania CV AB

Examinator: Martin Enqvist

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 2012-06-15 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://www.ep.liu.se ISBN — ISRN LiTH-ISY-EX--12/4592--SE

Serietitel och serienummer

Title of series, numbering

ISSN

—

Titel

Title

Estimering och signalbehandling av sensordata i platooningsystem för tunga for-don

Estimation and Pre-Processing of Sensor Data in Heavy Duty Vehicle Platooning

Författare

Author

Hanna Pettersson

Sammanfattning

Abstract

Today, a rapid development towards fuel efficient technological aids for vehicles is in progress. One step towards this is the development of platooning systems. The main concept of platooning is to let several heavy duty vehicles (HDVs) drive in a convoy and share important information with each other via wireless communication. This thesis describes one out of three subsystems in a project developed to handle the process from raw sensor data to control signal. The goal of the project is to achieve a safe and smooth control with the main purpose of reduced fuel consumption.

This subsystem processes the raw sensor data received from the different HDVs. The purpose is to estimate the positions and velocities of the vehicles in a platoon, taking into account that packet-loss, out of sequence measurements and irrelevant information can occur. This is achieved by filtering the information from different sensors in an Extended Kalman Filter and converting it into a local coordinate system with the origin in the ego vehicle. Moreover, the estimates are sorted and categorized into classes with respect to the status of the vehicles. The result of the thesis is useful estimates that are independent of outer effects in a local reference system with origin in the host vehicle. This information can then be used for further sensor fusion and implementation of a Model Predictive Controller (MPC) in two other subsystems. These three subsystems result in a smooth and safe control with an average reduced fuel consumption of approximately 11.1% when the vehicles drive with a distance of 0.5 seconds in a simulated environment.

Nyckelord

(6)

(7)

Abstract

Today, a rapid development towards fuel efficient technological aids for vehicles is in progress. One step towards this is the development of platooning systems. The main concept of platooning is to let several heavy duty vehicles (HDVs) drive in a convoy and share important information with each other via wireless communica-tion. This thesis describes one out of three subsystems in a project developed to handle the process from raw sensor data to control signal. The goal of the project is to achieve a safe and smooth control with the main purpose of reduced fuel consumption.

This subsystem processes the raw sensor data received from the different HDVs. The purpose is to estimate the positions and velocities of the vehicles in a platoon, taking into account that packet-loss, out of sequence measurements and irrelevant information can occur. This is achieved by filtering the information from different sensors in an Extended Kalman Filter and converting it into a local coordinate system with the origin in the ego vehicle. Moreover, the estimates are sorted and categorized into classes with respect to the status of the vehicles.

The result of the thesis is useful estimates that are independent of outer effects in a local reference system with origin in the host vehicle. This information can then be used for further sensor fusion and implementation of a Model Predictive Controller (MPC) in two other subsystems. These three subsystems result in a smooth and safe control with an average reduced fuel consumption of approxi-mately 11.1% when the vehicles drive with a distance of 0.5 seconds in a simulated environment.

Sammanfattning

Dagens utveckling inom fordonsindustrin fokuserar mer och mer påutveckling av bränsleeffektiva hjälpmedel. Ett steg i denna riktning är utvecklingen av platoo-ningsystem. Huvudkonceptet med platooning är att låta flera tunga fordon köra i följd i en konvoj och dela viktig information med varandra via trådlös kommuni-kation och en automatiserad styrstrategi. Detta examensarbete beskriver ett utav tre delsystem i ett projekt som är utvecklat för att hantera en process från rå sensordata till styrsignaler för fordonen. Målet är att uppnå en säker och mjuk reglering med huvudsyftet att reducera bränsleförbrukningen.

Det här delsystemet behandlar mottagen sensordata från de olika fordonen. Målet v

(8)

vi

med delsystemet är att skatta positioner och hastigheter för fordonen i konvojen med hänsyn till att förlorad, försenad eller irrelevant information från det trådlösa nätverket kan förekomma. Detta uppnås genom filtrering i ett Extended Kalman Filter och konvertering till ett lokalt referenssystem med origo i det egna fordo-net. Utöver detta sorteras informationen och kategoriseras in i olika klasser efter fordonens status.

Examensarbetet resulterade i användbara skattningar oberoende av yttre om-ständigheter i ett lokalt referenssystem med origo i det egna fordonet. Denna information kan användas vidare för ytterligare sensorfusion och implementering av en modellbaserad prediktionsregulator (MPC) i två andra delsystem. De tre delsystemen resulterade i en mjuk och säker reglering och en reducerad bränsleför-brukning med i genomsnitt 11.1% då fordonen körde med 0.5 sekunders avstånd i en simulerad miljö.

(9)

Acknowledgments

With this thesis I complete the Master of Science degree in Applied Physics and Electrical Engineering at Linköping University, Sweden. The work in this master’s thesis has been conducted between January and June 2012 at the Pre-development of Intelligent Transportation Systems Department (REPI) at Scania CV AB in Södertälje, Sweden and was supervised at the Automatic Control Division at Linköping University.

First, I would like to thank Scania Student Intro for letting me complete my master’s thesis at Scania. My deepest gratitude goes to Sanna Nilsson and Josefin Kemppainen for excellent cooperation and all the help during this work. Sanna and Josefin has been a great inspiration and motivation throughout the work and brought a lot of laughter and joy. I also want to thank my supervisors at Sca-nia, Henrik Pettersson and Assad Alam, for engagement, outstanding help and a lot of support. Patrik Axelsson and Martin Enqvist at Linköping University are acknowledged for wise inputs and feedback. Lastly I want to thank all other colleagues at REPI and my family and friends.

(10)

(11)

2.1.2 WiFi . . . 7 2.2 Sensors . . . 8 2.2.1 GPS . . . 8 2.2.2 Radar . . . 8 2.2.3 Other Sensors . . . 9 2.3 Dynamic Model . . . 9 2.3.1 Longitudinal Forces . . . 9 2.3.2 Power Train . . . 10 2.3.3 Air Drag . . . 11 2.3.4 Roll Resistance . . . 12 2.3.5 Gravitational Force . . . 12 2.3.6 Combined Equations . . . 12 3 Filter Theory 15 3.1 Extended Kalman Filter . . . 15

3.1.1 Time Update . . . 16

3.1.2 Measurement Update . . . 16 ix

(12)

x Contents

4 Implementation 19

4.1 Overview . . . 19

4.2 Sensor Data . . . 20

4.3 Time Management . . . 22

4.3.1 Calculate Sample Time . . . 22

4.3.2 Out Of Sequence Measurements . . . 22

4.4 ID Verification . . . 23

4.5 Extended Kalman Filter . . . 23

4.5.1 States . . . 23

4.5.2 Prediction . . . 24

4.5.3 Measurement Update . . . 25

4.6 Radar Filtering . . . 25

4.7 Sorting and Categorization . . . 26

4.8 Local Reference System . . . 27

5 Tests and Results 31 5.1 Testing Environments . . . 31

5.1.1 Data Collection . . . 31

5.1.2 Simulated Environment . . . 31

5.2 Results . . . 32

5.2.1 Estimator Results . . . 32

5.2.2 Sensor Fusion Results . . . 34

5.2.3 Common Results . . . 36

6 Discussion and Conclusions 43 6.1 Discussion . . . 43 6.1.1 Sensors . . . 43 6.1.2 Models . . . 43 6.1.3 The Estimator . . . 44 6.1.4 Project . . . 45 6.2 Conclusions . . . 45 6.3 Future Work . . . 46 Bibliography 47

(13)

Chapter 1

Introduction

This chapter outlines the relevant background to this master thesis accomplished at Scania CV AB. A description of platooning as a concept is given as well as a definition, goal and approach of a common project consisting of three master’s theses. Furthermore, a definition of the individual subproject that is the main focus of this thesis and its goals, assumptions and limitations are described.

1.1 Background

Today, there is a rapid development towards autonomous vehicles. Several active safety and driver assistance functions are already implemented in vehicles, with the goal of autonomous control. One example is the Adaptive Cruise Control (ACC), which is an existing function on most trucks. The ACC is similar to a Cruise Control (CC), which is a speed control that maintains the velocity according to a reference speed set by the driver. The difference to CC is that the ACC additionally takes the preceding vehicle into account, i.e., if the preceding vehicle has a lower speed than the reference, the ACC will autonomously lower the speed as well.

1.2 Platooning – Intelligent Transport System

Future vehicle research is increasingly focusing on developing systems that enable and utilize wireless communication between vehicles. The purpose of the communi-cation is that the vehicles should be able to share their relevant information about vehicle parameters and driving strategies. This can be used to assist the driver in fuel efficient and secure behaviour. The main aim of platooning is to let sev-eral Heavy Duty Vehicles (HDVs) operate at a close intermediate spacing through an automated control strategy. Each vehicle will be controlled autonomously in longitudinal direction and communicate with other vehicles through a wireless network. As a result of this controller, the distance between the HDVs can be decreased significantly, which implies a reduction of the air drag. Consequently, the fuel consumption will be reduced for all HDVs in the platoon. The vehicles

(14)

2 Introduction

Figure 1.1. A platoon with three HDVs. Courtesy of Scania CV AB.

in the platoon will behave as one unit and any oscillatory behaviour in the traffic should be prevented. This leads to an increase of the traffic flow [11] and of the road capacity [18]. Figure 1.1 shows a platoon with three HDVs.

The platooning outcome will be comparable to cross country skiers or cyclists, who take advantage of each other to reduce the air drag. Due to reduced adverse rear aerodynamic effects, even the leading truck will take advantage of the platoon [5]. A basic model of the platoon is shown in Figure 1.2 and a further description of the air reduction will be given in Section 2.3.3.

A first step towards realization of a platooning system is to let several HDVs drive in a convoy with the technology of today, i.e., ACC. The ACC will keep the truck on the distance of 2-3 seconds which corresponds to 40-60 meters at the speed of approximately 70 km/h. With a functioning platooning system, with wireless communication, the distance will decrease to 0.5 seconds which corresponds to 10 meters at the same speed. With this distance, the fuel consumption will be reduced by more than 10 % according to simulations on compact platooning [31]. However, taking the platooning concept into a real traffic environment will ad-ditionally affect the surrounding traffic and the drivers behaviour. Section 6.3 describes the future possibilities and problems for the system.

1.3 Common Project

This master’s thesis is part of a platooning project with three master’s theses. The project is planned together and the general outline of the interface and the different blocks of the projects have been developed together. The interface and a deeper explanation of the subsystems can be found in Section 1.3.2. The two connected

(15)

1.3 Common Project 3

Figure 1.2. A general image of a platoon with N vehicles. Courtesy of Assad Alam [6].

master’s theses are titled Sensor Fusion for Heavy Duty Vehicle Platooning, [25] and Model Predictive Control of Heavy Duty Vehicle Platooning, [21].

1.3.1 Approach

This project has been developed, planned and implemented in cooperation between three students from Linköping University at Scania. The approach of the project was to develop and plan the interface and the goals of the project together. Certain assumptions and limitations were also defined. Primarily, a breakdown of the project into three subsystems was defined. Each subsystem had distinct goals and assignments. A clear interface between the subsystems was then developed. The main part of the project was individual development and testing, where each person had responsibility for one subsystem. Finally, the subsystems were integrated and tested together as one large system. A further description of the interface can be found in Section 1.3.2 and the goal of the project in Section 1.3.3.

1.3.2 Interface

This section describes the general outline of the common project and the different parts. Figure 1.3 gives an overview of the interface between the three subsystems.

Figure 1.3. An overview of the interface of the project containing the three master’s

(16)

4 Introduction

Estimator

The Estimator is the first subsystem in the project, see Figure 1.3, and therefore receives the raw sensor data. The data is obtained directly through WiFi from the surrounding vehicles and via CAN from the own vehicle, see Section 2.1.1. The task for the Estimator is to estimate the states of the vehicles in the platoon and to construct a local reference system with origin in the receiving vehicle. The inputs of the Estimator are an ID vector and measured data from WiFi and from CAN.

Every vehicle has a specific ID number directly connected to the unit to deduce from which vehicle the information is collected. The estimator estimates the states of all transmitting vehicles and vehicles in the platoon with packet loss, only with the help of information that can be directly connected to a certain ID number, e.g. GPS data. The radar information,R, will not be taken into account when estimating the state of the vehicles. The reason for that is that radar gives infor-mation about the object ahead and can therefore not be ID tagged. Instead, the radar data requires pre-processing before it is used for estimation in the Sensor Fusion block.

The output of the system will be a state vector, ˆx, given in a local reference

system, an input vector u and the covariance of the states P . In addition an ID vector with the ID numbers of the estimated vehicles as well as their status, S, (i.e., leader, platoon ID, etc.) will be given as an output. Moreover, a data status vector, DS, with information whether the estimates are based on measurements or not is also given as output. The Estimator block will receive all raw sensor data and guarantee that the data is reliable for further estimations before it enters the two following blocks.

Sensor Fusion

The Sensor Fusion block receives information from a known source but unknown target and fuses it with information from the Estimator. The task is to decide whether the vehicles are in the platoon and relevant for the controller. The out-come will be a more accurate overview of the total platoon. The feedback from the Sensor Fusion block to the Estimator contains the ID numbers of the relevant vehicles to assure that the vehicles with packet loss in WiFi will not be forgotten. The output of the Sensor Fusion block will be an improved state vector, ˆx, a

co-variance matrix, P , and a status vectors. To achieve this an Extended Kalman Filter is applied together with association algorithms. Further information about the Sensor Fusion can be found in [25].

MPC

The last subsystem, the Model Predictive Controller (MPC), having received pro-cessed data, focuses on providing a smooth and safe control for the vehicles in the platoon. The MPC is briefly an optimization problem that is solved online and

(17)

1.4 Assumptions and Limitations 5

can easily handle multiple in- and outputs and constraints. A control signal to the Cruise Control is the output of the system. More information about the MPC can be found in [21].

1.3.3 Project Goal

The goal with the total platooning project containing the three master’s theses is to:

• Develop and implement a system that handles the process from raw sensor data to control signals.

• Achieve a smooth and safe control of the vehicles in a platoon where the main purpose is to reduce the fuel consumption.

1.3.4 Individual Goal

The goal of the Estimator is to process and estimate the raw sensor data coming from the vehicles’ CAN buses so that the output states are

• synchronized,

• estimated for vehicles with WiFi connections problems, i.e., also if no mea-sured data is received from the relevant vehicles,

• categorized in relevant status classes,

• sorted in a relevant order to ease the further processing, • converted to a local reference system,

and the radar data is • synchronized,

• preprocessed to reduce noise and handle loss in the WiFi connection, • sorted in relevant order.

1.4 Assumptions and Limitations

Some assumptions have been made during the work of this project. One example is that the mass and engine parameters of the different vehicles are assumed to be known. To begin with, the system will not take into account changes of speed limits, traffic lights or altitude changes.

(18)

6 Introduction

1.5 Related Work

The development and research in the field of platooning is relatively new. However, several master’s thesis and articles has been produced previously in this area. At Scania, a platooning project called IQ-fleet is in progress, where these master’s theses is a part. The main inspiration for this thesis has been [2] and [23]. Further previous master’s thesis produced at Scania within this area is [1], [22] and [20]. Studies of the potential fuel reduction, safety aspect of platooning and control strategy can also be found in [5], [7] and [6]. There has also been an international competition, called The Grand Cooperative Driving Challenge (GCDC), where several solutions to similar problems have been tested and studied [4, 8, 9, 13]. Concerning the estimation and interface of the different subsystems in this project, [28] and [30] have been used.

1.6 Thesis Outline

Chapter 1 gives an introduction to the field of the thesis and a description of the project of which this master’s thesis is a part. To proceed with, Chapter 2 gives a further explanation about the current system which follows by a description of the theory behind the solution to the problem in Chapter 3. Chapter 4 outlines an overview and then the implementation of the subsystem of the solution to the thesis. The result, the discussion and examples of future work are described in Chapter 5 and 6.

(19)

Chapter 2

System Description

This chapter gives an explanation of the sensors and networks used for this thesis and the dynamic model for an HDV.

2.1 Networks

This section describes two communication networks that are important for the project. The Controller Area Network (CAN) is used for the communication be-tween the electrical control units in the HDVs and a WiFi network is used for the vehicle to vehicle communication between the HDVs.

2.1.1 Controller Area Network

All communication in the vehicles is transferred via a CAN. This network is used for the communication between the electrical control units (ECUs) in the vehicle. The network was initially developed to enable robust serial communication in automotive applications. The transmission of the CAN protocol is message-based instead of address-based. Each node in the network receives all the messages and it is up to the node to decide whether it should be discarded or kept in process. A CAN message contains data and a priority flag for the message [27]. For this project, a protocol has been developed for the CAN transmission of to the signals that are necessary for this application. An overview of the signals is given in Section 4.2.

2.1.2 WiFi

For the Vehicle to Vehicle (V2V) communication, a wireless network, WiFi, is used. The used WiFi standard is called IEEE802.11, which is a network designed especially for the V2V communication. In this network, each vehicle broadcasts all the necessary signals [16]. The nodes, i.e., the other vehicles, collect all the information sent via the network. It is therefore crucial for the system to select and evaluate which data to use for the implementation of the controller, since data

(20)

8 System Description

from other sources than vehicles in the platoon, might also be collected [23]. The frequency of the WiFi is set to 5 Hz, since that is the frequency of the GPS.

2.2 Sensors

This section describes the sensors that are used for the data collection in this project.

2.2.1 GPS

The GPS used in the vehicles is called GPS 18 from the manufacturer Garmin. The GPS has a 12 channel receiver and uses up to 12 satellites to compute and update the position. The GPS computes information about the position and velocity of the object as well as a time stamp and the heading of the vehicle. The frequency of the GPS is 5 Hz. The accuracy of the absolute position is less than 15 meter and of the velocity less than 0.1 knot which is 0.05 m/s [17]. In the end, the control will be based on the relative velocity and position of the vehicles, which will imply a better accuracy of the GPS compared to the accuracy of the absolute values. The GPS is placed on the top of the cabin.

2.2.2 Radar

A radar (Radio Detection and Ranging) measures relative position and velocity to objects by transmitting radio waves. The waves will be reflected by the object and analyzed when they return. The functionality is independent of the weather. The radar is placed at the front of the vehicle, see Figure 2.1, and computes four different messages. Figure 2.2 shows the labelling of the four messages. A message contains sensor information from the radar. RT1 contains information about the object in front, RT4 about the vehicle to the right, RT3 the vehicle to the left and RT2 the vehicle two vehicles ahead. Naturally, since the second vehicle ahead will be more difficult to detect, RT2 will be an insecure source of information.

Figure 2.1. The radar is placed in the middle of the front of the vehicle and looks like

(21)

2.3 Dynamic Model 9

Figure 2.2. An illustration of the meaning of the different messages from the radar.

Furthermore, the radar will only detect objects that move in the same direction, i.e., oncoming vehicles will not be detected. Each message (RT1 - RT4) contains three different signals, relative position, relative velocity and relative acceleration. More information about the radar signals will be given in Chapter 4. The maxi-mum detection distance, frequency range, visual field, velocity resolution and angle resolution are limited [26].

2.2.3 Other Sensors

In addition to the GPS and the radar, some other sensors on the vehicle are used for the estimation. The yaw rate is measured by a gyro placed in the middle of the vehicle. The engine torque, used for the calculation of the acceleration in the motion model, is an estimated signal based on the fuel injection and the engine speed in the engine management system (EMS). Finally, the velocity of the vehicle is measured by the tachometer, which is using the rotational speed of the rear wheels of the vehicle.

2.3 Dynamic Model

In order to perform a model predictive control of the vehicles and to estimate their states with sufficient accuracy, a dynamic model of each vehicle in the platoon is required. The outcome of this section is the dynamics used for the estimation [12].

2.3.1 Longitudinal Forces

The longitudinal forces that have an impact on the system are shown in Figure 2.3. By applying Newton’s second law of motion the dynamic equation

(22)

Figure 2.3. The forces acting on a HDV. Courtesy of Assad Alam [2].

is obtained, where mt is the accelerated mass of the vehicle, v is the velocity,

Fengine is the power train force, Fbrake is the brake force, Fairdrag is the air drag

force, Froll is the roll resistance and Fgravity is the gravity force component. The

total accelerated mass is calculated from the mass of the vehicle and some specific constants, according to mt= m + Jw+ i2ti2fηtηfJe r2 w (2.2)

where Jw is the wheel inertia, it is the conversion ratio and ηt is the efficiency

constant of the gear in the transmission, if is the conversion ratio and ηf is the

efficiency constant in the final drive. Moreover, Je is the mass moment of inertia

of the engine and rwis the wheel radius. A complete derivation of the accelerated

mass and the power train force can be found in [2] and [23].

2.3.2 Power Train

The complete force from the power train is obtained by combining the forces from the engine, the clutch, the transmission, the propeller shaft, the final drive, the drive shafts and the wheels. A total derivation of the force can be found in [29]. Figure 2.4 shows a basic model of the power train.

The combined force from the power train is

Fengine(Te) =

itifηtηf

rw

Te (2.3)

where itis the conversion ratio and ηt is the efficiency constant of the gear in the

transmission, ifis the conversion ratio and ηfis the efficiency constant in the final

(23)

2.3 Dynamic Model 11

Figure 2.4. A basic model of the powertrain. Courtesy of Assad Alam [2].

2.3.3 Air Drag

The air drag force is given by

Fairdrag(v, d) =

1

2cDAaρav

2 _(2.4)

where Aais the front area of the vehicle, ρais the air density, v the velocity of the

vehicle and cDis the air drag coefficient. In a platoon the air drag coefficient will

be reduced due to the decrease of the distance between the vehicles. Figure 2.5 shows the mapping of the reduction of the air drag coefficient with respect to the relative distance between two vehicles.

The reduction is derived empirically from a platoon with three vehicles and is given by cD= cd 1 −fi(d) 100 (2.5)

where cd is the drag coefficient and the function fi(d), the reduction of the ith

vehicle with relative distance to the vehicle in the front, d, is a non-linear function which has been obtained using a first order least-squares approximation. For the first vehicle, d is the distance to the first vehicle behind.

f1(d) = −0.9379d + 12.8966 0 ≤ d ≤ 15

f2(d) = −0.4502d + 43.0046 0 ≤ d ≤ 80

f3(d) = −0.4735d + 51.5027 0 ≤ d ≤ 80

fi(d) = 0 otherwise

(2.6)

It is assumed that the reduction does not increase after the third vehicle in the platoon.

(24)

Figure 2.5. The air reduction for three HDVs in a platoon. Courtesy of Assad Alam [3].

2.3.4 Roll Resistance

The roll resistance is modeled by

Froll(α) = crmg cos(α) (2.7)

where cr is the roll coefficient, g is the gravitational constant, m the mass of the

vehicle and α the road slope.

2.3.5 Gravitational Force

The gravitational force is given by

Fgravity(α) = mg sin(α) (2.8)

where m is the mass, g the gravitational constant and α the road slope.

2.3.6 Combined Equations

By combining (2.1) to (2.8), a mathematical expression for the acceleration of the vehicle is obtained according to

(25)

2.3 Dynamic Model 13 ˙vi= r2 w Jw+ mr2w+ i2ti2fηtηfJe itifηtηf rw Te− cdAaρav2 2 +cdAaρav 2 2 · fi(d) 100 − crmg cos(α) − mg sin(α) ! (2.9)

To simplify the equation, the following constants are introduced

κ1= rwitifηtηf Jw+ mrw2 + i2ti2fηtηfJe κ2= 1 2r 2 wAaρacd Jw+ mrw2 + i2ti2fηtηfJe κ3= crr2wmg Jw+ mrw2 + i2ti2fηtηfJe κ4= r2 wmg Jw+ mrw2 + i2ti2fηtηfJe (2.10)

and the function

φi(d) = 1 − fi(d) 100 (2.11) which results in ˙vi= κ1Te− κ2φi(d)v2− κ3cos(α) − κ4sin(α) (2.12)

This dynamic model in (2.12) is then converted to discrete time and used for the estimates described in Chapter 4.

(26)

(27)

Chapter 3

Filter Theory

This chapter provides a theoretical background to the filter applied in this thesis. The filter is used to estimate non-stationary states, xk, that move according to a

motion model. A general non-linear motion model in discrete time can be written as

xk+1= f (xk, uk, θ, vk) (3.1)

where xk is the estimated state vector, uk the input signals, θ the model

parame-ters and vk the process noise.

A general model of sensor measurements is

yk = h(xk, uk, θ, ek) (3.2)

where ek is the measurement noise [15].

3.1 Extended Kalman Filter

In this thesis, an Extended Kalman Filter (EKF) is used for the estimation of the states. The EKF is an extension of an ordinary Kalman Filter (KF). Unlike the KF, the EKF also has the possibility to handle nonlinearities in the models and is therefore preferred in this case. The EKF applies a Taylor expansion to approximate the nonlinear functions f and h in (3.1) and (3.2). Unlike the KF, the EKF does not always converge since it is affected by approximation errors. This possible divergence depends on how inaccurate the initial values are and how non-linear the model is. Another drawback with the EKF is that the covariance matrix,

P , tends to be under-estimated compared to the real covariance matrix. The EKF

estimates the states with help of a motion model and measured sensor data. The algorithm first applies a time update (prediction) and then a measurement update.

(28)

16 Filter Theory

3.1.1 Time Update

The first step in the EKF algorithm is the time update. The EKF applies a motion model to predict the states using the previous information about the states. A general formula for the motion model is shown in (3.1) and the predicted states are

ˆ

xk+1|k= f (ˆxk|k, uk, θ, 0). (3.3)

In addition, a predicted covariance matrix, P is computed according to

Pk+1|k= FkPk|kFkT + GkQkGTk (3.4) where Fk= ∂f ∂x|xˆk|k,uk, (3.5) Gk= ∂f ∂v|ˆxk|k,uk (3.6)

and Qk is the covariance matrix for vk. Here, Qk can be viewed as a measure of

the uncertainty of the model and can be chosen according to how accurate the model is.

3.1.2 Measurement Update

In the measurement update a model for the measurements is utilized to compare the sensor values with the state estimates from the previous sample. A general function for this model is given in (3.2). The measurement residual is then calcu-lated according to

zk= yk− h(ˆxk|k−1, uk, θ, 0) (3.7)

where yk is the measurement and h the measurement function. The covariance

matrix, Sk, for the measurement residual, zk, is calculated according to

Sk = HkPk|k−1HkT+ Rk (3.8)

where Pk|k−1is the state covariance and Hk is calculated according to

Hk=

∂h

(29)

3.1 Extended Kalman Filter 17

Furthermore, Rk is the covariance matrix for the measurement noise, ek. This

matrix can be adjusted to reflect the uncertainty of the sensor measurements. The state update estimate, ˆxk|k, is then calculated as

ˆ

xk|k= ˆxk|k−1+ Kkzk (3.10)

where ˆxk|k−1 is derived from (3.3) and the Kalman gain, Kk, is

Kk= Pk|k−1HkTS

−1

k (3.11)

The covariance matrix, Pk, for the estimated states, is updated according to

Pk|k= (I − KkHk)Pk|k−1 (3.12)

This filter is used for estimations of the positions and velocities of the HDVs in a platoon, as will be described in Chapter 4.

(30)

(31)

Chapter 4

Implementation

This chapter explains the method and implementation of the solution that satisfy the goals defined in Section 1.3.4. In Section 4.1 an overview of the functions and subsystems of the estimator will be given. Section 4.2 to 4.7 give a more thorough description of each subsystem.

4.1 Overview

The Estimator is the first of three main blocks in the total project. The goal is to estimate states of each sending vehicle in the platoon and other interesting vehicles despite temporary packet losses. Furthermore, the goal is to handle the issue of time regarding the data sorting and categorize the output before it will be sent to the next block. As can be seen in Figure 4.1, raw sensor data is received to the system as an input directly from the HDVs. This data contains both information regarding the ego vehicle as well as the other sending vehicles. Primarily, two questions will be asked: when was the data sent and which vehicles are relevant to monitor? The sensor data is then used to estimate the states in an EKF. The estimator also provides a filter for the radar data. Finally an algorithm for sorting and categorizing the vehicles and a conversion to a local reference system is performed on the estimates.

Figure 4.1. An overview of the estimator.

(32)

20 Implementation

4.2 Sensor Data

The communication in each HDV is going through the CAN. All signals will be received via the CAN bus in different messages. Each message contains different signals with information about the vehicle. Table 4.1 and 4.2 show some of the different messages and signals.

Table 4.1 shows the relevant signals used to estimate each vehicle’s states in the platoon. In Section 2.2.1 more information about the GPS were given. The time stamp has a range between 0 and 5980 seconds. When the clock reaches its max-imum value, it starts over from zero again. The Vehicle ID is a specific unique ID number for each vehicle. This number is connected to an individual HDV and will not change if the vehicle leaves or enters a new platoon. The platoon ID is the ID of the leader vehicle in the platoon. This is automatically sent to each vehicle that connects to a platoon. The signal from the torque is given as a percentage of a reference value of the torque, which is approximately 3000 Nm for the HDVs considered in this thesis.

Each vehicle sends a set of messages out on WiFi and uses the same set for the estimation of the own vehicle. Therefore, the information from the own vehicle is the same as for the other ones. Figure 4.2 shows a simplified image of the WiFi and CAN connection. The messages in Table 4.1 are the messages used in the EKF for estimation of the vehicle states. The messages and signals in Table 4.2 are messages sent from the vehicles that need to be preprocessed before further estimation and processing. Further information regarding the radar was given in Section 2.2.2.

Table 4.1. An overview of the messages and signals that are relevant for the Estimator.

SensorData

Message Signals Unit Range Source

msg00

Speed m/s GPS

heading degrees 0 - 360 GPS

time stamp s 0 - 5980 GPS

msg01 longitude position degrees -180 - + 180 GPS latitude position degrees -90 - +90 GPS

msg02 Platoon ID integer

Vehicle ID integer

msg03

Torque % 0 - 100 EMS

Yaw rate degrees/s 0 - 360 Gyro

(33)

4.2 Sensor Data 21

Table 4.2. An overview of the messages and signals sent from the radar.

SensorData

Message Signals Unit from msg05

relative distance m radar RT1 relative velocity m/s radar RT1 relative accelaration m/s2 radar RT1 msg06

relative distance m radar RT3 relative velocity m/s radar RT3 relative accelaration m/s2 _{radar RT3}

msg07

relative distance m radar RT4 relative velocity m/s radar RT4 relative accelaration m/s2 _{radar RT4}

Figure 4.2. A model of the communications in and between the vehicles. The vehicles

(34)

4.3 Time Management

In real-time applications it is of importance to know at which time a message was sent. All the messages from one vehicle are transmitted at the same time on WiFi and as can be seen in Table 4.1, the time stamp is received from the GPS. The time management block mainly has two goals.

• Calculate the sample time,

• Handle out of sequence measurements (OOSM).

4.3.1 Calculate Sample Time

The calculation of the sample time is based on the time stamp from the GPS. The problems that can occur when the sample time is derived is that the GPS has a lower frequency than the CAN bus and the sensors. To solve this, the sensor data will only be measured and used in the filter with the frequency of the GPS, i.e., 5 Hz. The MPC in the last subsystem has a slower sample time of 0.4 seconds and therefore, 5 Hz, i.e., 0.2 seconds will be a sufficient sample time for the estimations. Previous experiments at Scania have shown that the time delay of the transmitting is neglectable.

4.3.2 Out Of Sequence Measurements

Out of sequence processing is necessary when the connection suffers from packet loss, duplication or reordering. A measurement is out of sequence if it has a num-bering that is smaller than the previous package. Out of sequence says a lot about the ’health’ of the connection. Figure 4.3 shows a basic image of delayed data. OOSM can be caused by three different events [19]:

1. Retransmission: If the sender retransmits the message.

2. Network duplication: If the receiver creates a duplicate of the packet. 3. Reordering: If the connection devices have reordered the information.

There are some different approaches to how to handle the OOSM problem [24]. • Neglecting. Ignore and discard the message. If the time delay is small

and the information value and accuracy of the package are high, this is not desirable.

• Data Reprocessing, Rollback. Store the sensor reports in a memory and reorder the measurements, then filter the ordered result. Could be dangerous in real-time applications and requires significant computer resources and data storage for a large number of sensors.

(35)

4.4 ID Verification 23

Figure 4.3. A basic image of an out of sequence measurement where zk−1 has been

delayed.

In this particular case, it is crucial that the information reaches the controller in time. Due to the real-time application, the rollback method is not to be considered. Therefore, a simple neglecting method will be implemented that ignores all the data with a time stamp less than the current time. If data is missing, the EKF will be run as usual but without the measurement update.

4.4 ID Verification

The sensor data from the different vehicles in the platoon are tagged with a specific ID number for each vehicle, see Section 4.2. However, in case of packet loss it is important to estimate the states of some vehicles that do not send any data. These relevant vehicles are derived in the Sensor Fusion block and sent back to the Estimator, as can be seen in Figure 1.3. The task of the Estimator is to estimate all the vehicles and let the sensor fusion block decide whether the vehicles are of importance or not. Therefore, all the sending vehicles will be estimated in the EKF regardless of the feedback from the Sensor Fusion block. The reason for this is to keep track of all the sending vehicles in case of a change in position or if they decide to enter the platoon. The ID vector will therefore consist of all the sending vehicles ID numbers and the non-sending vehicles that are still of importance according to the Sensor Fusion block.

4.5 Extended Kalman Filter

An EKF is used for the estimation of the states. The EKF estimates states with help of a motion model and measured sensor data. A theoretical explanation of the filter can be found in Chapter 3.

4.5.1 States

The states that will be estimated in the filter are

x =X1_{· · · X}NT

(4.1) where

(36)

and i is the specific vehicle, p1and p2are the longitude and latitude global position

coordinates, v the velocity in m/s and Φ the heading in degrees. Since the number of vehicles will change over time depending on how many vehicles there are in the platoon, the state vector x will have a variable length. The input to the EKF is

u =U1· · · UNT (4.3) Ui =Ti e αi Ψi (4.4)

where Te is the engine torque, α the slope of the road and Ψ the yaw rate of the

vehicle. Other parameters necessary for the EKF are

θ =Θ1_{· · · Θ}NT (4.5) Θi=mi _ii t iif ηti ηti rwi (4.6)

where m is the mass of the vehicle, itis the conversion ratio and ηtis the efficiency

constant of the gear in the transmission. if is the conversion ratio and ηf is the

efficiency constant in the final drive and rwis the wheel radius.

4.5.2 Prediction

To begin with the EKF makes a prediction of the states in the filter. This is done using the motion model for the vehicles. The discrete prediction in the EKF is derived from the continuous motion model shown in Section 2.3 and has also been used for earlier research in this area. The predicted positions, p1 and p2, are

derived by adding the distance the vehicles have moved to the previous position of the vehicle. The positions are estimated in global coordinates and the heading of the vehicle, Φ, is defined as zero when pointing towards the north, i.e. the same direction as the longitude. This will imply a multiplication with cos(Φ) and sin(Φ) for the velocity, v, and acceleration, a, in the time update for the states p1 and

p2 respectively, see (4.7). The estimated heading, Φ, is derived from the previous

heading and the yaw rate. The states are initialized with the values of the first measurements. pi1(k + 1) = pi1(k) + Tsv(k) cos(Φi(k))_2πR360 + T2 s 2 a i_{(k) cos(Φ}i_(k))360 2πR pi₂(k + 1) = pi₂(k) + Tsv(k) sin(Φi(k))_2πR360 + T2 s 2 a i_{(k) sin(Φ}i_(k))360 2πR vi(k + 1) = vi(k) + Ts· ai(k) Φi(k + 1) = Φi(k) + TsΨi(k) (4.7) where ai(k) = κi₁T_ei− κi 2(v i₎2_{− κ}i 3cos(α i_{) − κ}i 4sin(α i_), _(4.8)

(37)

4.6 Radar Filtering 25

see Section 2.3, and Tsis the sample time. The factor _2πR360, where R = 6, 371, 000 m

is the radius of the earth, is used to convert the velocity to the right coordinate system so that it can be used to predict the position.

4.5.3 Measurement Update

The measurements in the EKF are gathered from WiFi and CAN from different vehicles and the measurement vector will look like

y =Y1_{· · · Y}NT

(4.9)

where

Yi=pilong,GPS pilat,GP S viGP S vtachoi ΦiGPS

(4.10)

and i represents the vehicle. The measurement equation will be calculated as follows: hi=       pi 1 pi 2 3.6vi vi 2π 360Φ i       (4.11)

where the factor 3.6 multiplied with the velocity, v, is a conversion from m/s to km/h and the factor ₃₆₀2π multiplied with the heading, Φ, is a conversion from degrees to radians.

4.6 Radar Filtering

Each transmitting vehicle will, in addition to the measurements used in the EKF, also send radar information. More information about the radar was given in Sec-tion 2.2.2. The radar gives informaSec-tion about what the corresponding vehicle has in the closest environment in front of the vehicle. As was explained in Section 1.3.2, this is handled by the Sensor Fusion block and not by the estimator. However, the raw sensor data from the radar can contain outliers and needs to be prepro-cessed. A filter has therefore been developed to eliminate errors in the raw signals from the radar. A simple algorithm is applied, where the radar value is set to the previous value if there is a missing data sample due to the WiFi connection. The case when the radar cannot detect anything due to a missing target will result in a measurement of the maximum value and will not be handled in this filter.

(38)

4.7 Sorting and Categorization

Before making association decisions, the vehicles in the platoon will be categorized in different classes. Table 4.3 shows the first classification of the vehicles. The estimation algorithm will also be provided with a data status vector. This vector will give information about how reliable the estimates are by indicating whether the estimate is calculated with or without measured data from CAN. This is shown in Table 4.4. Figure 4.4 illustrates the different categories.

Table 4.3. Categories.

Vehicle Class

Leader 1

Ego 2

Vehicle in same platoon 3

Miscellaneous 4

Table 4.4. Data status.

Estimation Data Status.

With measurements 1

Without measurements 0

The sorting and categorizing algorithm is shown below.

1. Set status on ego vehicle and verify platoon ID on the ego vehicle. 2. Select the vehicle where P latoonID = V ehicleID and set as leader. 3. Select all vehicles, i, where P latoonIDi= P latoonIDegoand set status to 3. 4. For all vehicles i, calculate the distance to the leader.

5. Sort the vehicles i with respect to the distance from the leader.

The distances, disti, between the vehicles in point 4 are calculated with Haversines

formula [10] according to disti= 2R · arctan( √ a √ 1 − a) (4.12) a = sin2∆lat 2

+ cos(lati) sin(lati+1) sin2

∆long 2

(4.13)

where R is the radius of the earth, ∆lat and ∆long the difference between the latitude and longitude position of the vehicles respectively and lati the latitude

(39)

4.8 Local Reference System 27

Figure 4.4. The categories of the vehicles in a platoon.

4.8 Local Reference System

The final processing step for the estimates before being transmitted to the Sen-sor Fusion block is a coordinate transformation. In the EKF, the positions of the vehicles are estimated in GPS units, longitude and latitude, see Section 4.2. Since the Sensor Fusion block should receive the distance between the vehicles in meters, the estimates will be converted to a local coordinate system with origin in the ego vehicle. Figure 4.5 and 4.6 shows how the local reference system is defined, where Figure 4.6 illustrates the earth from above. Note that the radius of the earth seen from above depends on the latitude where the vehicle is, as seen in Figure 4.6. Closer to the equator gives a larger radius and closer to the north pole give a smaller radius. The surface of the earth will be assumed to be flat in the surroundings of the vehicles. This leads to the following equations

y = R sin(∆φ) (4.14)

x = R cos(φ) sin(∆λ) (4.15)

where λ is the longitude position, φ the latitude position and R the radius of the earth.

Figure 4.5. A model of how the local reference system is defined. Here, R is the radius

of the earth, φ the latitude of the ego vehicle and y the position of a vehicle in the coordinate system of the ego vehicle.

(40)

Figure 4.6. A model how the x - position of a vehicle is calculated in a local coordinate

system. The figure shows the earth from above. Here, R is the radius of the earth, φ the latitude of the ego vehicle and ∆λ the difference between the longitude position of the vehicle and the ego vehicle.

(a) The local reference system of a platoon with origin in the ego vehicle given by (4.14) and (4.15).

(b) The rotated local reference system of a platoon with origin in the ego vehicle and the ˜

y-axis in the direction

of the heading. Figure 4.7. Local reference system.

By applying these calculations for all vehicles in the platoon, a local coordinate system will be defined, with the origin in the ego vehicle and the y-axis pointing towards north, see Figure 4.7(a). In addition to the translation of the coordi-nate system, a rotation of the axis is performed with the result that the y-axis always points in the same direction as the heading of the ego vehicle, Φego, see

(41)

4.8 Local Reference System 29

Figure 4.8. A model over calculation of the distance between two vehicles in a curve.

Figure 4.7(b). The rotation of the coordinates is calculated using

˜

x = x · cos(Φego) − y · sin(Φego) (4.16)

˜

y = x · sin(Φego) + y · cos(Φego) (4.17)

With the assumption that the vehicles always drive on a straight road, the distance,

d, between the vehicles can easily be obtained as

d =px˜2_{+ ˜}_y2 _(4.18)

However, if the heading of the vehicles differ, the distance is approximated more accurately as a curve between the vehicles as shown in Figure 4.8. To derive the curve distance, dr, the angle α and the radius r of the curve are calculated

according to β = Φi− Φego (4.19) r = 1 sinβ₂ d 2

where Φi is the heading of the vehicle. The distance, dr, is calculated as

dr= rβ = (Φi− Φego)

1 sinβ₂

d

(42)

(43)

Chapter 5

Tests and Results

This chapter describes the different environments in which the solution to the problem has been implemented and outlines the different results of the tests.

5.1 Testing Environments

One of the goals described in Section 1.3.3 is to implement and integrate the three separate subsystems into one. This section describes the environments and tools applied in the testing and verification process.

5.1.1 Data Collection

Since the Estimator is the primary subsystem and processes the raw sensor data, it is crucial to use collected sensor data for testing and validation of the system. For the data collection, two HDVs were used on the test track shown in Figure 5.1. The test track is approximately 3 km long and the measurements were performed with different velocities and different masses on the vehicles. The track contains a small uphill section before the curve A, see Figure 5.1, which is also reflected in the results. During the tests, the HDVs were provided with all sensors mentioned in Section 4.2 and a WiFi connection. The HDVs were driving in a convoy and were automatically controlled with adaptive cruise control. All the tests contain data from one lap around the test track starting and ending in point B in Figure 5.1. All together approximately 20 tests were performed with velocities between 40 km/h to 90 km/h, with and without a trailer connected. The figures in Section 5.2.1 will only show the result of one of the tests without a trailer.

5.1.2 Simulated Environment

For the testing of the complete system, including the Sensor Fusion and the MPC subsystem, a feedback to the HDVs is necessary. To test this, a simulated envi-ronment in Matlab Simulink has been used. The model simulates two HDVs in a platoon and calculates simulated sensor values with help of the control signals

(44)

32 Tests and Results

Figure 5.1. A map over the test track used for data collection [14].

from the MPC. The environment has been developed by Scania and simulates the cruise control, brakes, gears and the vehicle surroundings. The tests in the simulated environment are performed with the assumption that the HDVs drive in northern direction on a straight road with no curves, i.e., the heading and the longitude position are always constant.

5.2 Results

This section describes the results for the algorithms developed in this thesis. Sec-tions 5.2.1 gives the results for the Estimator and Section 5.2.3 describes the simulated results of the complete project.

5.2.1 Estimator Results

The most important goals of the Estimator are to estimate the states of the ve-hicles with and without WiFi loss, using the ID tagged sensor data described in Section 4.5. In Figure 5.2, the four states of the EKF for one HDV are plotted together with the raw sensor values. The data was collected at the test track as described in Section 5.1.1. This estimation was done for the ego vehicle which implies that no WiFi loss took place, since each vehicle receives information about its own states via the CAN from on-board sensors. This result will also corre-spond to the estimation of surrounding vehicles when there are no WiFi losses is in question since the same set of sensors will be used for all vehicles.

(45)

5.2 Results 33 0 50 100 150 200 17.62 17.625 17.63 17.635 17.64 Longitude Position Time [s] Longitude [degrees] GPS data estimate 0 50 100 150 200 59.162 59.164 59.166 59.168 59.17 59.172 59.174 59.176 Latitude Position Time [s] Latitude [degrees] GPS data estimate 0 50 100 150 200 0 5 10 15 20 25 30 Velocity Time [s] Velocity [m/s] tachometer data GPS data estimate 0 50 100 150 200 1 2 3 4 5 6 7 8 Heading Time [s] Heading [degrees] GPS data estimate

Figure 5.2. The estimated position, velocity and heading of the ego vehicle in global

coordinates.

As can be seen in the figure, the sensor data do not contain a lot of noise. The least accurate signals come from the velocity measurements. Even though a lot of trust can be set to the motion model in the result shown in Figure 5.2, the estimates follow the measurements, which is desirable.

Figure 5.3 shows the result of the estimator with WiFi loss. Here, there are short time periods of WiFi losses throughout the whole measurement sequence. However, as can be seen in the figure, the longest time interval without WiFi connection between approximately the time 60 and 80 seconds, gives relatively insecure estimates of the states, especially the velocity. The increase in velocity during this time period is due to the motion model which calculates the value with help of the last received velocity and torque measurements. This uncertainty will be known by the other subsystems and the control signals of the MPC will adapt. Further information about the control logic can be found in [21]. The WiFi loss was obtained by a physical disconnection of the WiFi antenna in the HDV while performing the test. It is not likely that a vehicle looses connection for such a long time period in an actual traffic situation.

The estimated states from the EKF in Figure 5.2 and Figure 5.3 correspond to a global reference system. In Figure 5.4 the estimated position of two vehicles in a platoon during one test is shown. This result can be compared with the test track in Section 5.1.1. In this specific test, two vehicles have driven one lap on the test

(46)

34 Tests and Results 0 50 100 150 200 17.62 17.625 17.63 17.635 17.64 Longitude Position Time [s] Longitude [degrees] GPS data estimate 0 50 100 150 200 59.162 59.164 59.166 59.168 59.17 59.172 59.174 59.176 Latitude Position Time [s] Latitude [degrees] GPS data estimate 0 50 100 150 200 0 5 10 15 20 25 30 Velocity Time [s] Velocity [m/s] tachometer data GPS data estimate 0 50 100 150 200 1 2 3 4 5 6 7 8 Heading Time [s] Heading [degrees] GPS data estimate

Figure 5.3. The estimated position, velocity and heading of a vehicle with WiFi-loss in

global coordinates.

track. The leading vehicle is sending information on WiFi and the ego vehicle is following. The result of the same experiment converted to a local reference system, which also was one of the goals, is shown in Figure 5.5. The arrows in the plot correspond to the velocity of the vehicles. The local reference system has its origin in the ego vehicle, which is pointed out in the figure. The other arrows correspond to the leading vehicle. As can be seen in the figures, the leader mainly drives in the front of and approximately 5 meters to the right of the ego vehicle. This distance in x-position is mainly due to the drivers but also to the fact that the GPS sensor can be placed in different positions on individual vehicles. The arrows to the left of the origin correspond to the curve at point A, in the north-eastern end of the test track, see Figure 5.1.

5.2.2 Sensor Fusion Results

The positions and velocities computed by the Estimator are recalculated into distances and relative velocities in the Sensor Fusion block and compared to the corresponding radar values. Figure 5.6 shows the distance between two vehicles compared to the radar value and the result of the sensor fusion between these two. The vertical lines of the radar are the results of target losses. In these cases, the output from the Sensor Fusion relies only on the output from the Estimator. As illustrated in the figure, the result of the Estimator is mostly consistent with the

(47)

5.2 Results 35 1.9238 1.924 1.9242 1.9244 1.9246 1.9248 1.925 1.9252 1.9254 1.9256 x 106 5.4582 5.4583 5.4584 5.4584 5.4585 5.4585 5.4585 5.4586 5.4586 5.4587 5.4588x 10 6 x−position [m] y−position [m] Global Position HDV leader HDV follower

Figure 5.4. The position of two vehicles during one measurement on the test track in

global coordinates. −200 −15 −10 −5 0 5 10 15 5 10 15 20 25 30 35 40 45 x−position [m] y−position [m]

Position in a local reference system

HDV leader HDV follower

Ego vehicle

Figure 5.5. The position of two vehicles in the local reference system. The circle and

black arrow point out the ego vehicle in the figure, which always is placed in the origin of the coordinate system.

(48)

36 Tests and Results 40 60 80 100 120 140 160 10 20 30 40 50 60 70 Relative distance Time [s] Distance [m]

Measurement from radar Measurement from Estimator Fused Data 40 60 80 100 120 140 160 −5 0 5 10 Relative velocity Time [s] Velocity [m/s]

Measurement from radar Measurement from Estimator Fused Data

Figure 5.6. The output of the Estimator recalculated into distance between two HDVs

in a platoon compared with the radar data and the final estimate from the Sensor Fusion.

radar values, which is desirable. However, by combining the information from the Estimator and the radar, the the Sensor Fusion block improves the estimates.

5.2.3 Common Results

This section describes the results of the total project consisting of all three subsys-tems integrated together with feedback. The results are divided into some different test cases simulated in the environment described in Section 5.1.2. The first one shows a reference tracking with added white noise, the second one a simulation with WiFi loss and the last one a case of an unknown vehicle entering the platoon. Finally, the reduction of the fuel consumption is analyzed.

Noisy Reference Tracking

Figure 5.7 and 5.8 show the result after adding white noise to the signals in the simulated environment after 50 seconds. Figure 5.7 shows the output of the Sensor Fusion block and the output of the MPC is shown in Figure 5.8. Initially, the sim-ulation has the reference signal 70 km/h (19.44 m/s), after 60 seconds it changes to 80 km/h (22.22 m/s ), by 180 seconds to 75 km/h (20.83 m/s) and finally back to 70 km/h at the time 300.

In the Estimator, the estimates rely more on the motion model than the mea-surements. This implies a noise reduction in the estimates. It is worth to notice that the noise in the GPS signals in the simulated environment is large compared to the measurements, where the signals are smoother. Thus, the result becomes noisier in the simulation. The result of the reference tracking is shown in Fig-ure 5.7. The Sensor Fusion block takes the estimates from the Estimator and the radar into account and improves the signal. In Figure 5.8, reference tracking of the vehicles in the platoon can be identified. The upper plot shows the velocity of the two HDVs and the lower, the distance between them. For a safe control, the MPC aims to keep the time slot between the vehicles constant. Thus the distance between the vehicles in the platoon increases with the increase of the velocity. At the time 200 seconds, when the reference value of the velocity decreases, an

(49)

5.2 Results 37 0 50 100 150 200 250 300 350 400 15 16 17 18 19 20 21 22 23 24 25 Relative distance Time [s] Distance [m]

Measurement from radar Measurement from estimator Fused data

Figure 5.7. The relative distance computed by the Sensor Fusion block from simulated

data with white noise.

undershoot is obtained and this is enhanced due to the noise in the signal.

WiFi Loss

Figure 5.9 and 5.10 show the result of a simulation with WiFi loss. The vehicles loose connection for 30 seconds at the time 25 - 55 seconds. However, the Estimator continues to estimate the states of the vehicles, as can be seen in Figure 5.9. In this test case the reference signal keeps the constant velocity 70 km/h. Figure 5.9 shows the relative distance and the velocity of the two vehicles in the platoon. Due to the low accuracy in the information from the Estimator at the period of WiFi loss, the estimates from the Sensor Fusion block rely more on the radar measurements. When the connection is restored, a discontinuity occurs in the estimated distance at the time of 55 seconds. This can be handled by tuning and adapting the weight matrices of the EKF and does not, as can be seen in Figure 5.10, have a big impact on the controller [25]. To receive a secure control of the system, which is one of the goals described in Section 1.3.3, the distance between the vehicles immediately increases when the WiFi connection is lost, as can be seen in Figure 5.10. This is achieved by taking the covariance and data status into account [21]. The data status is described in Section 4.4. The increase of distance is followed by a smooth decrease of the distance by the time the WiFi connection returns by a velocity increase of the last vehicle. A further explanation about the control logic and interface between the Sensor Fusion and MPC can be found in [25] and [21].

(50)

38 Tests and Results 0 50 100 150 200 250 300 350 400 65 70 75 80 85 Time [s] Velocity [km/h]

Velocity profile for centralized MPC and the common system v last v_leader 0 50 100 150 200 250 300 350 400 17 18 19 20 21 22 23 24 25 Time [s] Distance [m]

Relative distance for centralized MPC and the common system

d_{leader−last}

Figure 5.8. The upper plot shows the velocities of the two HDVs and the lower, the

distance between them with simulated data with white noise from the MPC block.

0 10 20 30 40 50 60 70 80 90 100 15 20 25 30 35 Relative distance Time [s] Distance [m]

Measurement from radar Measurement from estimator Fused data 0 10 20 30 40 50 60 70 80 90 100 −0.4 −0.2 0 0.2 0.4 0.6 0.8 1 Relative velocity Time [s] Velocity [m/s]

Measurement from radar Measurement from estimator Fused data

Figure 5.9. The estimated position and velocity of the Sensor Fusion with WiFi loss

(51)

5.2 Results 39 0 10 20 30 40 50 60 70 80 90 100 65 70 75 Time [s] Velocity [km/h]

Velocity profile for centralized MPC and a non linear model

0 10 20 30 40 50 60 70 80 90 100 20 25 30 35 Time [s] Distance [m]

Relative distance for centralized MPC and a non linear model v

last v_leader

d_{leader−last}

Figure 5.10. The behavior of the vehicles after being controlled by the MPC with WiFi

loss between the time 25 and 55 seconds.

Unknown Vehicle

This section shows results of the system handling the entrance of an unknown vehicle into the platoon. In this case, an unknown vehicle is referred to as a vehicle which is not equipped with WiFi i.e. a vehicle that does not broadcast any WiFi information e.g. a car. This implies that an unknown vehicle can only be detected by the radar. To simulate that an unknown vehicle enters the platoon, a temporary decrease of the velocity of the second HDV is performed, but the radar value stays constant. This will imply an increase of the estimated length of the leading vehicle [25]. In the system, such an increase corresponds to an unknown vehicle which will be taken into account in the controller by an increase of the distance [21]. Figure 5.11 shows the resulting estimated length of the leading vehicle and Figure 5.12 the behavior of the MPC. The last vehicle decreases the velocity, to let the unknown vehicle enter, at the time 20 seconds. When the estimated length reaches a value of 22 meters, the system concludes that an unknown vehicle is in the platoon. Hence, the MPC increases the distance. Fuel Consumption

The main purpose of the total project is to reduce the fuel consumption of the ve-hicles, see Section 1.3.3. Table 5.1 shows the fuel consumption during a simulated test of the total system. The platoon simulation was performed by two vehicles following the same reference as in Section 5.2.3. The energy consumption of the vehicles in the platoon was then compared to two single vehicles with the same parameters driving the same distance with the same velocities. The consumed energy was calculated by integrating the power of the vehicles. The fuel consump-tion decreases for both vehicles in the platoon due to the air drag reducconsump-tion, as

(52)

40 Tests and Results 0 10 20 30 40 50 60 70 80 90 100 6 8 10 12 14 16 18 20 22 24 Estimated length Time [s] Length [m]

Figure 5.11. Estimated length of the front HDV when a unknown vehicle enters the

platoon. 0 10 20 30 40 50 60 70 80 90 100 62 64 66 68 70 72 74 76 78 Time [s] Ve lo ci ty [km/ h ]

Velocity profile for centralized MPC and the common system

0 10 20 30 40 50 60 70 80 90 100 5 10 15 20 25 30 35 40

Relative distance for centralized MPC and the common system

Time [s] D ist a n ce [ m] v last v leader last v leader v last v leader

Figure 5.12. Velocity and distance between the two vehicles when a unknown vehicle