Sensor Fusion for Heavy Duty Vehicle Platooning

(1)

Institutionen för systemteknik

Department of Electrical Engineering

Examensarbete

Sensor Fusion for Heavy Duty Vehicle Platooning

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

av

Sanna Nilsson

LiTH-ISY-EX--12/4593--SE

Linköping 2012

Department of Electrical Engineering Linköpings tekniska högskola

Linköpings universitet Linköpings universitet

(2)

(3)

Sensor Fusion for Heavy Duty Vehicle Platooning

Examensarbete utfört i Reglerteknik

vid Tekniska högskolan i Linköping

av

Sanna Nilsson

LiTH-ISY-EX--12/4593--SE

Handledare: Assad Al Alam

Scania CV AB

Henrik Pettersson

Scania CV AB

Zoran Sjanic

isy, Linköpings universitet

Examinator: Fredrik Gustafsson

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/4593--SE

Serietitel och serienummer

Title of series, numbering

ISSN

—

Titel

Title

Sensorfusion för tunga fordon i fordonståg Sensor Fusion for Heavy Duty Vehicle Platooning

Författare

Author

Sanna Nilsson

Sammanfattning

Abstract

The aim of platooning is to enable several Heavy Duty Vehicles (HDVs) to drive in a convoy and act as one unit to decrease the fuel consumption. By introducing wireless communication and tight control, the distance between the HDVs can be decreased significantly. This implies a reduction of the air drag and consequently the fuel consumption for all the HDVs in the platoon.

The challenge in platooning is to keep the HDVs as close as possible to each other without endangering safety. Therefore, sensor fusion is necessary to get an accurate estimate of the relative distance and velocity, which is a pre-requisite for the controller.

This master thesis aims at developing a sensor fusion framework from on-board sensor information as well as other vehicles’ sensor information communicated over a WiFi link. The most important sensors are GPS, that gives a rough position of each HDV, and radar that provides relative distance for each pair of HDV’s in the platoon. A distributed solution is developed, where an Extended Kalman Filter (EKF) estimates the state of the whole platoon. The state vector includes position, velocity and length of each HDV, which is used in a Model Predictive Control (MPC). Furthermore, a method is discussed on how to handle vehicles outside the platoon and how various road surfaces can be managed.

This master thesis is a part of a project consisting of three parallel master’s the-ses. The other two master’s theses investigate and implement rough pre-processing of data, time synchronization and MPC associated with platooning.

It was found that the three implemented systems could reduce the average fuel consumption by 11.1 %.

Nyckelord

(6)

(7)

Abstract

The aim of platooning is to enable several Heavy Duty Vehicles (HDVs) to drive in a convoy and act as one unit to decrease the fuel consumption. By introducing wireless communication and tight control, the distance between the HDVs can be decreased significantly. This implies a reduction of the air drag and consequently the fuel consumption for all the HDVs in the platoon.

The challenge in platooning is to keep the HDVs as close as possible to each other without endangering safety. Therefore, sensor fusion is necessary to get an accurate estimate of the relative distance and velocity, which is a pre-requisite for the controller.

This master thesis aims at developing a sensor fusion framework from on-board sensor information as well as other vehicles’ sensor information communicated over a WiFi link. The most important sensors are GPS, that gives a rough position of each HDV, and radar that provides relative distance for each pair of HDV’s in the platoon. A distributed solution is developed, where an Extended Kalman Filter (EKF) estimates the state of the whole platoon. The state vector includes position, velocity and length of each HDV, which is used in a Model Predictive Control (MPC). Furthermore, a method is discussed on how to handle vehicles outside the platoon and how various road surfaces can be managed.

This master thesis is a part of a project consisting of three parallel master’s the-ses. The other two master’s theses investigate and implement rough pre-processing of data, time synchronization and MPC associated with platooning.

It was found that the three implemented systems could reduce the average fuel consumption by 11.1 %.

Sammanfattning

Målet med platooning är att ett antal tunga fordon kör i en fordonskonvoj och agerar som en enhet för att minska bränsleförbrukningen. Genom att introduce-ra trådlös kommunikation mellan lastbilarna, och snabb reglering, kan avståndet mellan dem minskas avsevärt. Ett minskat avstånd mellan lastbilarna leder till minskat luftmotstånd som i sin tur minskar bränsleförbrukningen.

Utmaningen är att låta lastbilarna köra så nära varandra som möjligt utan att äventyra säkerheten. Sensorfusion är nödvändigt för att uppnå detta eftersom re-gulatorn behöver så trovärdiga skattningar av position och hastighet som möjligt. Detta examensarbete har därmed syftet att utveckla ett ramverk för sensor-fusion, där information från sensorer på lastbilen tillsammans med information från andra lastbilar via WiFi används. De viktigaste sensorerna är i första hand

(8)

vi

GPS, som ger en ungefärlig position för varje HDV, samt radar som ger informa-tion om det relativa avståndet och hastigheten för varje HDV-par i platoonen. En distribuerad lösning är utvecklad där ett Extended Kalman Filter (EKF) skat-tar tillstånden för hela platoonen. Tillståndsvektorn innehåller position, hastighet och längd för varje HDV tillsammans med en osäkerhet, denna används sedan i en modellbaserad prediktionsreglering. Dessutom har olika metoder för att upptäcka och hantera fordon som inte är medlemmar i platoonen diskuterats samt hur olika vägprofiler kan hanteras.

Detta examensarbete är en del av ett projekt som består av tre parallella ex-amensarbeten. De två andra examensarbetena utreder och implementerar, i stora drag, förbehandling av data, tidshantering och modellbaserad prediktionsreglering relaterat till platooning.

Det har visat sig att de tre implementerade systemen minskar den genomsnitt-liga bränsleförbrukningen med 11.1 %.

(9)

Acknowledgments

This thesis is the final part of the Master of Science degree in Applied Physics and Electrical Engineering at The Institute of Thechnology at Linköpings Uni-versity (LiTH). This master thesis has been carried out at the pre-development of Intelligent Transport Systems (REPI) which is a part of Scania CV’s Research and Development.

I would like to thank Hanna Pettersson and Josefin Kemppinen, for good co-operation and many laughs together during this project. Many thanks to my supervisors at Scania CV, Henrik Pettersson and Assad Alam, for support and knowledge. I also want to express my gratitude to my supervisor at LiTH, Zoran Sjanic, and examiner, Fredrik Gustafsson, for useful inputs and guidance. The people within the division REP should have thanks for a very enjoyable time at Scania. I would also like to thank Scania Student Intro program, who gave me the opportunity to this master thesis and introducing me to wonderful people.

Finally, many thanks to my boyfriend, family and friends, without your support during my studies this would not been possible.

Sanna Nilsson Linköping, June 2012

(10)

(11)

Introduction

This chapter covers an introduction to the concept of platooning. It also contains a description of the master thesis structure and important terms.

1.1 Background

The vehicle industry today is at the forefront of various solutions to fuel consump-tion issues. With time, diesel engines have been optimized to a vast extent with respect to fuel efficiency and therefore other actions must be taken. It has been shown that the air drag has a major impact on fuel consumption, [4]. Therefore, the air drag can be reduced by letting several vehicles maintain a suitable distance to each other, illustrated in Figure 1.1. However, direct communication between the vehicles is a necessary complement due to safety aspects. One way to achieve this is by using a comprehensive system called platooning.

Figure 1.1. An illustration of a platoon consisting of three vehicles. This is provided at the courtesy of Scania CV AB.

(14)

2 Introduction

Platooning is today one of the services in progress within the vehicle industry and it is an extension of the Adaptive intelligent Cruise Control (ACC). The ACC only considers the vehicle in front via the information from radar. A safe distance to this vehicle is held by having the driver set a pre-defined time gap. This means that no direct communication between the vehicles is used.

1.1.1 Platooning

The aim of platooning is to enable several HDVs to drive in a convoy using an automated control strategy and thereby act as one unit. Each vehicle will be controlled autonomously in a longitudinal direction and communicate with other vehicles through a wireless network. As a result of this automatic control the dis-tance between the HDVs can be decreased significantly, which implies a reduction of the air drag. Consequently, the fuel consumption will be reduced for all the HDVs in the convoy. The vehicles in the platoon will behave as one unit and the ”accordion effect”, oscillations in the chain, is expected to be prevented.

For the understanding of the fuel reduction possibilities in platooning, tests have been performed. When two identical trucks with a velocity of 70 km/h and a time gap of one second were driving, a maximum fuel reduction of 4.7% - 7.7% was obtained, [4].

Decreasing the distance between vehicles contributes to an increased risk for the driver and the traffic. For example collision risk will increase and must there-fore be prevented when platooning is developed. However, an automated control strategy with vehicle-to-vehicle communication (V2V) might improve safety. Nor-mally, it takes seconds for a human to react and act in critical situations. Using V2V, this time delay will be reduced. V2V can therefore serve as one of the pos-sible solutions to avoid this risk.

1.1.2 Vehicle-To-Vehicle Communication

The ACC uses the information from the radar in order to get information about the preceding vehicle, this is illustrated in layer I in Figure 1.2. By introducing of V2V communication, layer II in Figure 1.2, information from the surrounding vehicles can be obtained. This enables a more robust platooning since the information from the vehicles nearby is broadcasted and it is also possible to spread the host vehicle information. Vehicle-to-infrastructure (V2I), e.g. traffic lights, can be seen as an additional layer, layer III in Figure 1.2, for platooning. V2I will not be included in this master’s thesis.

(15)

1.2 Goal 3

Figure 1.2. System architecture for platooning, [1]. In layer I the vehicles use the information from the radar in order to get information about the preceding vehicle and in layer II the vehicles get information from the surrounding vehicles via V2V. In layer III information via V2I reaches the vehicles.

1.2 Goal

The main challenge in platooning is to keep the HDVs as close as possible to each other, while reducing the fuel consumption simultaneously. In order to manage this, signal processing, noise modeling and sensor fusion are necessary to control the distance between the HDVs in a suitable way.

The goal of the project, see Section 1.3, is to develop and implement a system that handles the process from raw sensor data to control signals to enable a smooth and safe control of the vehicles in a platoon where the main purpose is reduced fuel consumption.

To get as reliable information as possible regarding the states of the HDVs, sensor fusion is needed. Therefore the aim of this master’s thesis is to investigate how sensor fusion can be used in platooning and implement it. The major part is to relate ID tagged information with radar data. ID tagged information is information received through a known object via WiFi. The radar detects objects in front but the object is unknown in the sense that the radar does not identify the object. Furthermore, there are also discussions on how to handle vehicles outside the platoon and on various road surfaces.

1.3 The Project

This master’s thesis is a part of a project consisting of three parallel master’s theses. The other two theses are titled Model Predictive Control for Heavy Duty Vehicle Platooning, [12], and Estimation and Pre-Processing of Sensor Data in Heavy Duty Vehicle Platooning, [17], respectively. To clarify the concepts, the

(16)

4 Introduction

project indicates all the three master’s theses together and the subsystem indi-cates the separate thesis, as shown in Figure 1.3.

Below, a brief summary of the three subsystems and the interface between them is described.

Figure 1.3. An overview of the subsystems in the project.

1.3.1 Estimator

The thesis Estimation and Pre-Processing of Sensor Data in Heavy Duty Vehicle Platooning, [17], describes this subsystem. The Estimator, in Figure 1.3, transmits pre-processed sensor signals from both the host HDV and the other HDVs in the platoon. Sensor data from known sources, tagged with an ID number, is fused to estimate position, velocity and heading for the HDVs. The Estimator also handles packet loss, detection of transmitting vehicles outside the platoon and time management. The output from this subsystem is the input to the Sensor Fusion.

1.3.2 Sensor Fusion

The Sensor Fusion handles primary an estimation of the desired states. It is also related to the association problem according to sensor data, tagged with an ID number, i.e. data from WiFi, and data from the radar. The radar detects objects in front but the object is unknown in the sense that the radar does not identify the object. All data is fused to estimate the state as accurately as possible to facilitate the MPC. The estimated states are position, velocity and length for all the HDVs. The reason why these are estimated is described in Chapter 3. Additionally, it covers the problems with packet loss, unknown vehicles, both inside and outside of the platoon, and when the platoon is turning.

(17)

1.4 Interface 5

1.3.3 Model Predictive Control

The thesis Model Predictive Control for Heavy Duty Vehicle Platooning, [12], describes this subsystem in detail. Model Predictive Control (MPC) uses a linear dynamic model as a prediction model and optimizes the control signals online. This subsystem decides the control signals to the vehicle and sends out this signal to the Cruise Control (CC), a system that automatically controls a set value for the velocity of the vehicle. To facilitate this, the subsystem needs the relative distance and velocity for all the HDVs of interest. The input of this subsystem is the output from the Sensor Fusion.

1.4 Interface

To clarify the interfaces between the subsystems, a brief discussion is given in this section. The interface is divided into two parts, the first part is the interface between the Estimator and the Sensor Fusion systems and the second part is the interface between the Sensor Fusion and MPC systems.

1.4.1 Estimator − Sensor Fusion

The Estimator estimates the states, ˆxE, for every transmitting vehicle, with

mea-surements from known sources, tagged with ID number. The output from this system is the estimated states (position, velocity and heading) together with the corresponding ID vector, IDE, Data Status vector, DSE, and Status vector, SE.

These vectors are described in detail in Chapter 3. Radar measurements and other signals, such as torque, mass and yaw rate, are also outputs from the Estimator. All these signals are inputs to the Sensor Fusion. The Sensor Fusion takes into account the measurements from radar. The radar detects objects in front but the object is unknown in the sense that the radar does not identify the object. This system determines if the vehicles in the platoon track the vehicle in front, i.e., if the expected vehicle is in front.

The Sensor Fusion initiates the states, ˆxSF, and the covariance, PSF, with the

states, ˆxE, and covariance, PE, from the Estimator.

If transmitting vehicles outside the platoon enters the systems, i.e., vehicles outside the platoon broadcasts information, information regarding whether the vehicle is located behind the leader of the platoon and in the same lane is estimated. Otherwise the information from this vehicle is neglected. If packet loss from some of the vehicles in the platoon occurs, the Estimator estimates the states as long as the Sensor Fusion determines that the vehicle is still in the platoon. Therefore, an ID vector for all the vehicles that are to be estimated is fed back to the Estimator.

1.4.2 Sensor fusion − MPC

The MPC requires the relative distance and velocity for all the vehicles in the platoon, which is the output from the Sensor Fusion. The Sensor Fusion system detects if there is an unknown vehicle inside the platoon and the information

(18)

6 Introduction

is forwarded to the MPC, due to the fact that the distance must be increased. This information is forwarded through the Status vector SSF. This vector also

includes information regarding information about packet loss. For the same reason the covariance, PSF, related to the states will be relayed to the MPC. Safety

is the reason for increasing distance because the unknown vehicle’s behavior is unpredictable.

1.5 Limitations

In order to achieve the objectives, the master’s thesis is limited to several assump-tions. The first assumptions are that the speed limit on highways, traffic lights and other traffic obstructions are neglected. It is also assumed that the road slope is constant and zero for both the Sensor Fusion and the project but the system is developed in a way that the road slope can be added. There is no access to this information and therefore it is assumed to be zero. A HDV has very complex dynamics and therefore it is difficult to model. Due to this, a simplified model is implemented.

1.6 Related Work

Platooning is a fairly young research area, with many current ongoing projects. For example iQFleet, Intelligent real-time fleet control and management, is a project in progress at Scania CV AB. This project contains parts as surrounding traffic, fuel consumption and driver acceptance.

There are several master’s theses in the area of platooning. The main refer-ences used in this master’s thesis are [2] and [14]. Additionally, the master’s theses [13] and [21] together with the article [19] have been an inspiration in this mas-ter’s thesis. There are also useful articles about the Grand Cooperative Driving Challenge (GCDC), [3] and [5].

Many different articles and books which handles sensor fusion can be found, but the main reference is Statistical Sensor Fusion, [9].

1.7 Thesis Outline

This master’s thesis is structured as followed: Chapter 2 covers a description of the dynamic model for for the HDV’s and a description of the available sensors. Furtermore, Chapter 3 contains a description of how the system is implemented. Testing carried out both in a simulation environment and with real data is pre-sented in Chapter 4. Finally, Chapter 5 contains a summary of the master’s thesis which includes discussions regarding the results and the conclusions. To gain an understanding of the utilized filter theory in this thesis, an overview is given in Appendix A.

(19)

Chapter 2

System Description

To implement the systems described in Chapter 1, a model of the dynamics for a vehicle is required. Additionally, knowledge about the available sensors are necessary. In this chapter these two important parts are described.

2.1 Vehicle Modelling

In order to make a proper prediction of the HDVs, a model for the vehicle’s be-havior is needed. In this section, a model for vehicle’s movement is explained.

2.1.1 Powertrain

The first part of the vehicle model consists of the powertrain, see Figure 2.1. The involved systems in the powertrain are described in detail in [20].

Figure 2.1. A basic model of the powertrain’s components. The powertrain consists of an engine and a driveline as illustrated in the figure. The powertrain model is the same as in [2].

The complete powertrain equation for the force that drives the vehicle, Fw, is

given by

(20)

8 System Description Fw= itifηtηf rw Te− Jw+ i2ti2fηtηfJe r2 w ˙v = Fengine− Finertia (2.1)

where it and ηtdenote the conversion ratio and efficiency constant for the

trans-mission and if and ηf denote the conversion ratio and efficiency constant for the

final drive. Je and Jw denote the mass moment of inertia for the engine and for

the wheel respectively. rw is the radius of the wheels and Te is the net engine

output torque. Fengine is the force from the engine and Finertia is the inner force

that the engine needs to overcome in order to produce a driving force.

2.1.2 External Longitudinal Force

The second part of the vehicle model, which consists of the external longitudinal forces that act on the HDVs is illustrated in Figure 2.2. Below, the forces are described in detail.

Figure 2.2. An overview of the longitudinal forces that acts on the HDVs, [2].

Aerodynamic force

The aerodynamic force is given by

Fairdrag(v, d) =

1

2cD(d)Aaρav

2 _(2.2)

where cD(d) denotes the air drag coefficient as a function of the distance between

two HDVs, Aa denotes the reference area, ρa denotes the density of the air. The

air drag coefficient, cD, is defined as cD(d) = cd(1 −f₁₀₀i(d)) where cdis the air drag

constant and fi(d) is a non-linear function that describes the air drag reduction

produced due to a vehicle ahead. d is the distance between the vehicles and i denotes the position in the platoon. fi(d) is described in Figure 2.3 and is derived

(21)

2.1 Vehicle Modelling 9

Figure 2.3. The figure shows the reduced air drag for HDVs in platoon based on

empirical results, [4].

fi(d) is non-linear and to simplify the expressions the function is approximated

with a linear function fitted with the least squares method and results in the following expression

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

fN −1(d) = −0.4502d + 43.0046 0 ≤ d ≤ 80 (2.3)

fN −2(d) = −0.4735d + 51.5027 0 ≤ d ≤ 80

fother(d) = 0 otherwise

where N is the number of vehicles in the platoon. fN(d) describes Lead HDV,

fN −1(d) describes T2and fN −2(d) describes T3in Figure 2.3. Vehicles beyond the

third vehicle in the platoon are assumed to have the same air drag reduction as the third one.

Roll resistance

The roll resistance is given by

Froll(α) = crmg cos α (2.4)

where cr denotes the rolling resistance coefficient, m denotes the HDVs mass and

g denotes the gravitational constant. α denotes the road slope but in this project

(22)

10 System Description

Gravity

The gravitational force is given by

Fgravity(α) = mg sin α (2.5)

where the constants are the same as in (2.4).

2.1.3 Vehicle Model

By applying Newton’s second law of motion and Figure 2.2, (2.6) is obtained, [7]

m ˙v = Fw− Fbrake− Fairdrag− Froll− Fgravity (2.6)

Equation (2.6) together with (2.1) results in

mtot˙v = Fengine− Finertia− Fbrake− Fairdrag− Froll− Fgravity (2.7)

where mtot denotes the accelerated mass, i.e.

mtot= m + Jw+ i2ti 2 fηtηfJe r2 w (2.8)

Since the braking torque is hard to measure and model and is zero most of the time, Fbrake is neglected in this model.

Combining (2.1)-(2.5) with (2.7)-(2.8) a final expression for the vehicle model is obtained and given by

˙vi = κ1Te− κ2ρ(d)v2− κ3cos(α) − κ4sin(α) (2.9)

where the constants are

κ1= rwitifηtηf Jw+ mr2w+ i2ti2fηtηfJe κ2= 1 2r 2 wAaρacd Jw+ mrw2 + i2ti2fηtηfJe (2.10) κ3= crrw2mg Jw+ mr2w+ i2ti2fηtηfJe κ4= r2 wmg Jw+ mrw2 + i2ti2fηtηfJe and ρ(d) = (1 −fi(d) 100 ) (2.11)

where fi(d) is the described in (2.3).

2.2 Sensors

The HDVs are equipped with several onboard sensors that provide different infor-mation about the vehicle’s position, velocity and other parameters. The different sensors that are used are described in detail below.

(23)

2.2 Sensors 11

2.2.1 Radar

All the HDV’s are equipped with a radar as shown in Figure 2.4. Radar uses radio waves to detect objects and determines the relative distance and relative velocity between the object and itself.

Figure 2.4. The radar is placed in the lower part of the front of the vehicle. The figure is provided at the courtesy of Scania CV AB.

The radar also sends information regarding which field the object is located in, [15]. There are four different fields, RT1, RT2, RT3 and RT4. These are shown in Figure 2.5.

Figure 2.5. An overview of the different radar fields. The dark vehicle is equipped with the radar.

The field of view for this radar is narrow and it operates only if the detected object is moving. The radar has a maximum detection range regarding that varies with the operation condition. In summary, the radar detects objects far away but has a narrow field of view. The collected raw data are filtered by the radar unit to obtain more reliable data.

2.2.2 Global Positioning System

Global Positioning System (GPS) is a satellite navigation system which gives the position in longitude and latitude coordinates and the velocity to the receiver. The GPS also specifies the heading of the vehicle and a time stamp that specifies the time when the messages is received. The vehicles are equipped with a GPS 18

(24)

12 System Description

from the manufacturer GARMIN. The accuracy is better than 15 meter in position and 0.5144 m/s in velocity, [8].

The GPS device combines information from at least four and up to twelve satellites to measure this data and sends the information with frequency 5 Hz.

2.3 Networks

To share information internally between the devices in each vehicle, as well as between the vehicles in the platoon, networks are necessary. In the following section, the two networks used in this application are described.

2.3.1 Controller Area Network

All vehicles manufactured today are equipped with a Controller Area Network (CAN). CAN is defined as a serial communication bus and use message based com-munication, not an address based comcom-munication, [16]. This means that the CAN messages itself contains the priority and the data being transmitted. All nodes in the system receive every messages that are transmitted on the bus. Thereafter it is up to each node to decide whether the message should be discarded or kept to be processed.

In summary, CAN buses are a standardized way to communicate between different Electric Control Units (ECU) available in the vehicle. All the information from the sensors used in this thesis is available on CAN and this information is read with a frequency of 100 Hz.

2.3.2 WiFi

Information from all transmitting HDVs is broadcasted according to the protocol IEEE 802.11p, [11]. This protocol is developed for V2V communication. Vehicles in the platoon receive this information with frequency 10 Hz. The broadcasted information consists of the following data

• Radar - relative distance, velocity and acceleration (different fields) • Tacho - speed

• GPS - position, velocity, heading and time stamp • Yaw rate and engine torque

(25)

Chapter 3

Implementation

To implement the required systems described in Chapter 1, the theoretical refer-ence (Appendix A) is used together with the system description, Chapter 2.

To get understanding of the different situations that may arise during platoon-ing, data was collected early in the project. During the analysis of data, some specific situations were identified and studied more carefully. These situations are discussed in this chapter together with a description of how the system is implemented. The system is implemented in Matlab simulink.

3.1 Platooning

The last vehicle in the platoon has the notation 1 and the preceding vehicles have 2,...,N , where N is the number of vehicles in the platoon, see figure 3.1. The host vehicle is referred to as EGO and vehicle N as leader. These notations are local in each vehicle.

Figure 3.1. An overview of the vehicle notation.

The relative distance between vehicle 1 and vehicle 2 are referred to as d1,2

and the velocity for vehicle 1 is referred to as v1etc.

As described in Section 1.3.2 the position and velocity are estimated for every HDVs of interest. This is carried out since the MPC requires information regarding the relative distance and velocity between the HDVs. The vehicles of interest are described later on in this chapter. The length is estimated in order to determine if unknown vehicles are entering the platoon, see Section 3.7.1.

(26)

14 Implementation

The estimated states for the platoon can be summarized as

xN =p1 _v1 _l1_{. . . p}N _vN _lNT

(3.1)

where N denotes the amount of HDV in the platoon, p is the position, v is the velocity and l is the length of the HDVs. The estimated states for the EGO vehicles are written as

xEGO=pEGO _vEGO _lEGOT

(3.2)

and are included in (3.1)

Two types of reference systems have been studied and therefore p can be either a position for the HDVs in a global reference system (GPS-coordinates) or in a local reference system. For further information regarding this, see Section 3.3.3 and 3.3.4.

3.2 Sensor Fusion

All available measurements from onboard sensors and WiFi have different units and different accuracy. For this reason a combination of all signals is to prefer. Furthermore, some measurements may be more reliable than others in certain situations. Therefore, measurements have to complement each other in some sense. By using sensor fusion within estimation theory, this can be obtained. Sensor fusion is mainly used due to the fact that combined data gives a better result than data handled separately.

3.3 Filtering

There are different types of sensor fusion algorithms to use for this purpose but due to the nonlinear dynamics and the simplicity, the EKF algorithm is used in this master’s thesis. The EKF algorithm, Appendix A, is implemented with the dynamics and equations below, Section 3.3.1 - 3.3.4.

It should be mentioned that the equations and vectors are for one HDV in a platoon, except from the leader which is treated specially in some cases. The algorithm is implemented for variable size since the number of vehicles in the pla-toon varies.

Two different types of reference systems are investigated. The first reference sys-tem is a global syssys-tem using the longitude and latitude coordinates as states and the second reference system is a local system using the EGO vehicle as origin. This is explained in more detail in Section 3.3.3 and 3.3.4.

(27)

3.3 Filtering 15

3.3.1 Inputs and Parameters

Some inputs that are used in the dynamic model vary for the different HDVs and also vary over time. These parameters are seen as inputs to the filter. Some of the inputs are received from the V2V communication to give a more reliable filter and others are received from the Estimator. The input vector is written as

uk =Te Φ α T

(3.3)

where the engine torque, Te, and the vehicle heading, Φ, are received from the

V2V communication. Also the road slope, α, is seen as an input but assumed to be constant and zero.

Some of the data used in the model are in some sense constant and can be seen as parameters instead of inputs. In this case the parameters are the mass of the vehicle and the time difference between updates. This means the time difference between two measurements and it is received from the Estimator. The parameter vector is written as

θk=m Ts

T

(3.4)

3.3.2 Measurements

Measurements are an important part of the sensor fusion problem. In this thesis there are two measurement sources. The first one is the Estimator and it gives an estimated position and velocity for each vehicle based on the sensor measurement from the own vehicle and V2V-information. The other source is the radar, which gives a relative distance and relative velocity compared to the preceding HDV. The radar returns this information for four different fields which indicates the position of the tracking object, see Chapter 2. It should be mentioned that only the field RT1 has been studied in this master’s thesis.

The measurement vector can be summarized as

yk =yp yv yp_rel yvrel

T

(3.5)

where yp is the measured position and yv is the measured velocity for one HDV,

obtained from the Estimator. yp_rel is the measured relative distance and yvrel is

the relative velocity obtained from the radar.

For the same reason as in Section 3.1, the measurement for the position, re-ceived from the Estimator, are either in the global or the local reference system.

(28)

3.3.3 Global Reference System

The global reference system uses latitude, φ, and longitude, λ, as states to describe the position of the vehicles. The dynamic model for the states can be written as

φi,k+1 = φi,k+ Tsvi,kcos Φi_R2π360 +T

2 s

2 ai,kcos Φi 360

R2π

λi,k+1 = λi,k+ Tsvi,ksin Φi_R2π360 + T_s2

2 ai,ksin Φi 360

R2π

vi,k+1 = vi,k+ Tsai,k

li,k+1 = li,k

(3.6)

where the discrete version of the acceleration, described in Section 2, is written as

ai,k= κ1Te,k− κ2ρ(d)vi,k2 − κ3cos αk− κ4sin αk (3.7)

and R is the earth radius and Tsis the time difference between the updates.

The measurement model can be written as

yi,k=       φi,k λi,k vi,k di,k− li+1,k vi+1,k− vi,k       (3.8)

where i denotes the i’th vehicle and i+1 denotes the i+1’th vehicle in the platoon.

Time Update

The time update for an EKF is written as ˆ

xk+1|k= f (ˆxk|k, uk) (3.9)

where f is the non-linear function that describes the dynamic, (3.6).

The time update for one HDV in the global reference system is written as in (3.10).

ˆ

φi,k+1|k = φˆi,k|k+ Tsvˆi,k|kcos Φi_R2π360 + T2 s 2 ai,k|kcos Φi 360 R2π ˆ

λi,k+1|k = ˆλi,k|k+ Tsvˆi,k|ksin Φi_R2π360 + T2

s

2 ai,k|ksin Φi_R2π360

ˆ

vi,k+1|k = ˆvi,k|k+ Tsai,k|k

ˆ

li,k+1|k = ˆli,k|k

(3.10)

The derivative of the global reference system with respect to the states is writ-ten as

(29)

3.3 Filtering 17 Fi=      1 + Γ∂ρ(d)_∂φ i Γ ∂ρ(d) ∂λi Tscos Φi 360 R2π− T 2 sκ2ρ(d) 0 Γ∂ρ(d)_∂φ i 1 + Γ ∂ρ(d) ∂λi Tssin Φi 360 R2π− T 2 sκ2φ(d) 0 0 0 1 − 2Tsκ2ρ(d)v 0 0 0 0 1      (3.11) where Γ = −κ2 T2 s 2 cos Φ 360 R2πv 2 (3.12)

The derivatives can be written as

∂ρ(d) ∂φi = ∂ρ(d) ∂di ∂di ∂φi (3.13) ∂ρ(d) ∂λi = ∂ρ(d) ∂di ∂di ∂λi (3.14)

where the derivative of ρ(d) with respect to the distance di is written as

∂ρ(d) ∂di

= − ki

100 (3.15)

where the constant kidepends on the position, i, for the vehicle in the platoon in

the same way as the constants in (2.3) i.e.,

kN kN −1 kN −2 kother = −0.9379 −0.4502 −0.4735 0 (3.16)

where N denotes the leader. The derivatives of the distance with respect to the positions, ∂di

∂φi and

∂di

∂λi, are the same as in (3.24). Measurement Update

The available measurements come from the Estimator and from the radar. This provides vector y according to (3.5).

When using the global reference system, the distance between two GPS-coordinates is calculated according to haversine formula, [6], see Section 3.3.4. The measure-ment equations, hi(ˆx), for the i’th vehicle are written as

hi(ˆx) =       ˆ φi ˆ λi ˆ vi ˆ di− ˆli+1 ˆ vi+1− ˆvi       (3.17)

(30)

where ˆdi is the estimated relative distance to the vehicle in front. ˆvi+1 is the

estimated velocity for the vehicle in front.

Since the measurement of the relative distance and relative velocity depends on the vehicle in front, the same applies to the derivatives. Therefore, the derivative of hi(ˆx) with respect to the states, in (3.10), for the i’th and i+1’th vehicle is

written as Hi=       1 0 0 0 0 1 0 0 0 0 1 0 ∂di ∂φi ∂di ∂λi 0 0 0 0 −1 0       (3.18) Hi+1=       0 0 0 0 0 0 0 0 0 0 0 0 ∂di ∂φi+1 ∂di ∂λi+1 0 −1 0 0 1 0       (3.19)

The derivatives, in (3.18) and (3.19), denote the derivatives of haversine for-mula with respect to the position of the two vehicles, this is declared later on.

The H-matrix for all the vehicles in the platoon is written as

H =          H1 H2 0 · · · 0 0 . .. . .. ... .. . . .. . .. 0 .. . HN −1 HN 0 · · · 0 H_N∗          (3.20)

where HN is the leader of the platoon and do not have a transmitting vehicle in

front. Therefore, the measurement update for HN only consists of the onboard

sensors for the leader, i.e. the measurement from the Estimator. This can be written as H_N∗ =   1 0 0 0 1 0 0 0 1   (3.21)

(31)

Measurement Equations

The distance between two GPS-coordinates is calculated using the haversine for-mula, [6]. This formula is written as

di= 2Rarctan √ a √ 1 − a (3.22) a = sin2∆φ 2

+ cos(φi) sin(φi+1) sin2

∆λ 2

(3.23)

where φiand λi is the latitude and longitude coordinates for the i’th vehicle, φi+1

and λi+1 is the latitude and longitude coordinates for the vehicle in front and ∆φ

respective ∆λ is the difference between the two vehicles and R denotes the radius of Earth.

The derivatives of di with respect to the position (longitude and latitude) for

vehicle i and i + 1 are written as

∂di

∂φi

= d∗cos(φi) cos(φi+1) sin

∆λ 2 cos∆λ 2 (3.24) ∂di ∂λi

= −d∗cos(φi) cos(φi+1) sin

∆λ 2 cos∆λ 2 ∂di ∂φi+1 = d∗sin∆φ 2 cos∆φ 2

− cos(φi) sin(φi+1) sin

∆λ 2 ∂di ∂λi+1 = −d∗sin∆φ 2 cos∆φ 2

− sin(φi) cos(φi+1) sin

∆λ 2 where d∗= R pa(1 − a) (3.25)

and a is the same as in (3.23).

3.3.4 Local Reference System

A local reference system is obtained by moving the origin to the EGO vehicle and the y-axis oriented to the north, see Figure 3.2. An alternative would be to have the the origin at the leaders position. But due to the fact that packet loss sometimes occur, this can lead to an uncertain reference system.

(32)

Figure 3.2. The figure shows how the local reference system is oriented. The coordinates of each vehicle are described.

However, this reference system allows a better map of where the vehicles are located around the EGO vehicle and furthermore, the system is orthogonal and has the unit meter. To transform the global coordinates (longitude and latitude) to this local reference system, (3.26) is used. Figure 3.3 explains the equations.

Figure 3.3. This sketch shows how a global reference system is transformed to a local reference system.

The translation is written as

∆y = R sin(∆φ) (3.26)

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

(33)

in the EGO vehicle. R is the radius of earth, φ and λ is the coordinates in terms of latitude respective longitude.

The dynamic model for the states can be written as

xi,k+1 = xi,k+ Ts∆vi,ksin Φi+ T_s2

2 ∆ai,ksin Φi yi,k+1 = yi,k+ Ts∆vi,kcos Φi+

T2 s

2 ∆ai,kcos Φi vi,k+1 = vi,k+ Ts∆ai,k

li,k+1 = li,k

(3.27)

where ∆vi,k is the relative velocity between vehicle i and the EGO vehicle, i.e.,

∆vi,k= vi,k− vEGO,k (3.28)

and the discrete version of the acceleration, described in Chapter 2, ∆ai,k is the

relative acceleration between vehicle i and the EGO vehicle, i.e.,

∆ai,k= ai,k− aEGO,k (3.29)

where

ai,k = κ1Te− κ2ρ(d)v2i,k− κ3cos α − κ4sin α (3.30)

and Tsis the time difference between the updates.

The measurement model can be written as

yi,k=       xi,k yi,k vi,k di,k− li+1,k vi+1,k− vi,k       (3.31)

where i denotes the i’th vehicle and i+1 denotes the i+1’th vehicle in the platoon.

Time Update

The time update for an EKF is written as ˆ

xk+1|k= f (ˆxk|k, uk) (3.32)

where f is the non-linear function that describes the dynamic, (3.27). The time update for one HDV in the local reference system is written as

(34)

ˆ

xi,k+1|k = xˆi,k|k+ Ts∆vi,k|ksin Φi+ T2

s

2 ∆ai,k|ksin Φi

ˆ

yi,k+1|k = yˆi,k|k+ Ts∆vi,k|kcos Φi+ T_s2

2 ∆ai,k|kcos Φi

ˆ

vi,k+1|k = vˆi,k|k+ Ts∆ai,k|k

ˆ

li,k+1|k = ˆli,k|k

(3.33)

The derivative for the local reference systems time update with respect to the states for vehicle i, except from the EGO vehicle, are summarized in (3.34).

Fi=      1 + Γ∂ρ(d)_∂φ i Γ ∂ρ(d) ∂λi sin Φi(Ts− T 2 sκ2ρ(d)) 0 Γ∂ρ(d)_∂φ i 1 + Γ ∂ρ(d) ∂λi cos Φi(Ts− T 2 sκ2ρ(d)) 0 0 0 1 − 2Tsκ2ρ(d)vi 0 0 0 0 1      (3.34)

Since the time update for vehicle i dependens on the EGO vehicle, the derivatives with respect of the EGO vehicle is essential. This is written as

Fi,EGO=      −Γ_∂φ∂ρ(d) EGO −Γ ∂ρ(d)

∂λEGO − sin ΦEGO(Ts− T 2

sκ2ρ(d)) 0

−Γ_∂φ∂ρ(d)

EGO −Γ

∂ρ(d)

∂λEGO − cos ΦEGO(Ts− T 2 sκ2ρ(d)) 0 0 0 2Tsκ2ρ(d)vEGO 0 0 0 0 0      (3.35)

where Γ is the same as in (3.12). The derivatives can be written as

∂ρ(d) ∂φi =∂ρ(d) ∂di ∂di ∂φi (3.36) ∂ρ(d) ∂λi =∂ρ(d) ∂di ∂di ∂λi (3.37)

where the derivative of ρ(d) with respect to the distance di is written as

∂ρ(d) ∂di

= − ki

100 (3.38)

where the constant ki depends on the position, i, for the vehicle in the platoon in

the same way as the constants in (2.3) i.e.,

kN kN −1 kN −2 kother = −0.9379 −0.4502 −0.4735 0 (3.39)

(35)

The derivatives of the distance with respect to the positions, ∂di

∂φi and

∂di

∂λi, are

the same as in (3.47).

The F -matrix for the EGO vehicle is written as

FEGO=     1 0 0 0 0 1 0 0 0 0 1 − Tsκ2ρ(d)vEGO 0 0 0 0 1     (3.40)

Summerized, the total F -matrix is written as

F =             FEGO 0 · · · 0 F1,EGO F1 . .. ... .. . 0 . .. . .. ... .. . ... . .. . .. . .. ... .. . ... . .. . .. 0 FN,EGO 0 · · · 0 FN             (3.41)

where N is the number of vehicles in the platoon exempt from the EGO vehicle.

Measurement Update

The available measurements in this case comes from the Estimator and the radar. This provides vector y according to (3.5).

When applying the local reference system, the distance is calculated according to the Pythagorean theorem, explained below. The measurement equations are written as hi(ˆx) =       ˆ xi ˆ yi ˆ vi ˆ di+1,i− ˆli+1 ˆ vi+1− ˆvi       (3.42)

The derivative of hi(ˆx) with respect to the states are shown in (3.43)-(3.45).

H =          H1 H2 0 · · · 0 0 . .. . .. ... .. . . .. . .. 0 .. . HN −1 HN 0 · · · 0 HN          (3.43)

(36)

where Hi and Hi+1, i goes from 1 to N , is written as

Hi=       1 0 0 0 0 1 0 0 0 0 1 0 ∂di ∂xi ∂di ∂yi 0 0 0 0 −1 0       (3.44) Hi+1=       0 0 0 0 0 0 0 0 0 0 0 0 ∂di ∂xi+1 ∂di ∂yi+1 0 −1 0 0 1 0       (3.45)

The derivatives, in (3.44) and (3.45), denote the derivative of Pythagorean theo-rem with respect to the position of the two vehicles.

Measurement Equations

When using the local reference system, the distance between two vehicles is cal-culated according to Pythagorean theorem, (3.46).

di= q (xi+1− xi) 2 + (yi+1− yi) 2 (3.46)

The derivatives with respect to the position, x and y-coordinates, for vehicle i are written as ∂di ∂xi = −_q xi+1− xi (xi+1− xi) 2 + (yi+1− yi) 2 ∂di ∂yi = −_q yi+1− yi (xi+1− xi) 2 + (yi+1− yi) 2 (3.47) ∂di ∂xi+1 =_q xi+1− xi (xi+1− xi) 2 + (yi+1− yi) 2 ∂di ∂yi+1 =_q yi+1− yi (xi+1− xi) 2 + (yi+1− yi) 2

(37)

3.3.5 Rotated Local Reference System

By rotating the local reference system, the y-axis is oriented to the EGO vehicle di-rection and thus an even better overview of where the vehicles are located is given. A rotated coordinate system can be obtained by using the standard Rotation of coordinate system formula, [18].

Figure 3.4. An illustration of how a reference system can be rotated.

The formula is as follows

x = ˜x cos(α) − ˜y sin α (3.48)

y = ˜x sin(α) + ˜y cos α

where α is the difference in heading between vehicle i and the EGO vehicle. ˜x

and ˜y is the coordinates in the non-rotated reference system and x and y is the

coordinates in the rotated coordinate system. The rotated local reference system is now oriented as illustrated in Figure 3.5.

Figure 3.5. The figure shows how the rotated local reference system is oriented. The coordinates of each vehicle are described.

This rotated local reference system is the final reference system and the results are based on this.

(38)

3.4 Packet Loss

Noise due to electric wires and highway tunnels are examples of disturbance that can make the WiFi signals weak or disappear. In the case when the signals from vehicles in the platoon do not reach the HDV via WiFi, the Estimator estimates the state without measurement, i.e. only time update is performed. The Estima-tor sends a vecEstima-tor with status bits, DSE, to inform the Sensor Fusion that the

states from the Estimator has not used any measurement for the non-transmitting vehicle, see Table 3.1. In this case there are no available radar data and therefore there is no measurement update for the radar measurements. Note that radar data from the EGO vehicle is always available via CAN.

Table 3.1. The table shows the status bit, DSE, with description.

Status Bit, DSE Description

1 Available data 0 No available data

The main problem with packet loss is to make as reliable estimations as possi-ble of the vehicles without measurements. Additionally, verify that the vehicle is still in the platoon. To investigate this, the radar measurement from the follower have to be studied. If this measurement is the same as the previous value the ve-hicle with packet loss can be assumed to still be a part of the platoon. Otherwise, it is assumed that the vehicle has left the platoon. In any case, the information (IDSF) is fed back to the Estimator to inform if the vehicle has to be estimated

or not.

3.5 Platoon Logic

A platoon consists of one leading HDV and a number of follower HDVs behind the leader. To build or join a platoon the HDV has to turn on a platooning-function to start transmitting information. When the function is turned on the driver will have a request to join a platoon if there is one in front. The HDV will only have the request if the platoon is at least 300 meters in front but will be a part of the platoon when the distance has decreased to 60 meters. These are chosen design parameters here. Then the controller will take over and control the HDV in lon-gitudinal direction to decrease the distance even more.

If there is no platoon in front, the driver will have the opportunity to build a new one. If a driver decides to build a new platoon, this HDV is made to the leader and the platoon ID will be the HDVs vehicle ID. If there is an existing platoon the platoon ID is the same as the leader’s vehicle ID and the vehicles allocated this

(39)

3.6 Classification 27

platoon ID.

3.6 Classification

To keep track of the objects (e.g. vehicles not connected to the platoon) in the environment of the platoon, a classification on every object is implemented. Four types of objects have been considered, see Table 3.2. Most of the information is received from the Estimator since this subsystem reads all data. The Sensor Fusion detects unknown vehicles and therefore this is classified in this subsystem.

Table 3.2. The table shows the classes, SE, that are considered.

Class number, SE Type

1 Leader

2 EGO

3 Platoon member 4 Other vehicles

Class number 1 and 2 are used to know which data belongs to the leader ve-hicle and the EGO. Veve-hicles in the platoon, aside from the two mentioned, are represented with class number 3. The class number 4 is allocated by comparing the Platoon ID with the Vehicle ID and is used to determine if the vehicle is of interest, see Section 3.7.1.

A status bit for every vehicle inside the platoon is forwarded to the MPC from the Sensor Fusion. If the platoon consists of vehicles of class number 1, 2 or 3, a status bit, SSF, of type 1 is forwarded. Furthermore, if there are other vehicles

inside the platoon a status bit of type 0 is forwarded.

Table 3.3. The table describes the status bits, SSF, forwarded to the MPC.

Status Bit, SSF Type

1 Leader, EGO and platoon member 0 Other vehicles

This information is essential for the MPC since the behavior for other vehicles cannot be predicted. Therefore the distance to this type of vehicles has to increase,

(40)

[12].

3.7 Association Problem

In this master’s thesis, three kinds of association problems are investigated. A problem occurs when several vehicles on the highway transmit information via WiFi and the only vehicles of interest are those inside the platoon. Problems also occur due to unknown vehicles (not transmitting vehicles) entering the platoon, since the radar and GPS measurements do not match with the constant length of the vehicles.

3.7.1 Transmitting Vehicles Outside the Platoon

When using WiFi there is a lot of information to handle since every transmitting vehicle broadcasts the information. The only information of interest is the infor-mation from the vehicles belonging to the platoon and transmitting vehicles inside the platoon.

Information from all transmitting vehicle is relayed to the Sensor Fusion from the Estimator and is classified. If there are vehicles that do not belong to the platoon it is classified as 4, Table 3.2. The Sensor Fusion decides if the vehicles is of interest or if the vehicles can be ignored. The decision is then fed back to the Estimator through the Status vector, IDSF, to inform if the vehicle has to be

estimated or not.

A transmitting vehicle outside the platoon is investigated according to the flow chart in Figure 3.6. If the vehicle outside the platoon meet the requirements in the figure, then it is assumed that the vehicle is somewhere inside the platoon, or at least in the adjacent lane beside. When this occurs, the vehicle is of interest since it can affect the platoon with its behavior. Otherwise the vehicle is not of interest and is neglected.

3.7.2 Unknown Vehicles Outside the Platoon

Unknown vehicles, i.e. vehicles that do not communicate via WiFi, is detected by using information from the GPS via WiFi together with the radar. These vehicles can be detected if the length of the vehicles inside the platoon is estimated. Since the length of a HDV is constant, the estimated length should be constant. Oth-erwise something is disturbing the platoon, e.g. an unknown vehicle has entered. This is explained in Figure 3.7.

If the system detects an unknown vehicle in the platoon the information is for-warded to the MPC through the SSF.

(41)

3.7 Association Problem 29

Figure 3.6. A flow chart over the decisions for continuing to estimate the vehicle states. Almost the same heading means that the the vehicle direction should be the same as the platoons direction, thus not in the opposite direction. Almost the same velocity means that the vehicle’s velocity should have the same magnitude as the platoon, i.e. ± 5 km/h. The same area means that the vehicle should be located behind or in the front of the platoon with a distance of the platoons length.

(42)

Figure 3.7. The left figure shows the estimated length when no unknown vehicle is in the platoon and the right figure shows how the estimated length increase when a unknown vehicle is in the platoon.

3.7.3 Platoon in Curves

When a platoon is entering a tight curve, the radar returns a default value due to inability of the radar to detect any target since it has a narrow field, see Figure 3.8.

Figure 3.8. Clarification of when the radar misses the object in front.

In this case the radar measurement changes on one sample, from a realistic value to a default value. This behavior is not realistic and therefore something has happened to the object in front or to the source vehicle, possibly due to a turning event of the platoon. To ensure that this is the case, different measurements (yaw rate, heading etc.) can be investigated. If the yaw rate for the vehicle in front is increasing, the probability for turning is high. Comparing the headings between two vehicles can be a third condition that the platoon is turning.

(43)

3.7 Association Problem 31 50 100 150 200 0 200 400 600 Relative distance Time [s] [m] Radar measurement 50 100 150 200 0 100 200 300 400 Heading Time [s] [degrees] Leader EGO 50 100 150 200 −0.2 −0.1 0 0.1 0.2 Yaw rate Time [s] [rad/s] Leader EGO

Figure 3.9. This figure shows the relative distance between two vehicles, heading and yaw rate for the two vehicles. The radar measurement has 620 m as a default value, as seen in the figure. This value is obtained when the radar is unable to detect the object.

Figure 3.9 shows the relative distance between two vehicles, heading and yaw rate for two vehicles from collected data. As seen in the figure, three short radar losses occurs. When these occurs, the heading and yaw rate are not affected. But when the long period of radar loss occurs, at the time 115 seconds, the heading changes direction and the yaw rate is significantly nonzero. Therefore, the long period of radar loss along with a change in heading and yaw rate can be identified as a curve. The other three losses are probably caused by other factors such as small slopes etc.

The measurements from the GPS can, in some cases, be misleading, especially in curves and slopes, due to its accuracy in position. The case when the platoon enters a curve, the relative distance can be recalculated using an algorithm which assumes that the curve is a circle.

(44)

Figure 3.10. A sketch of how the distance is recalculated in a curve using the difference in heading of the two vehicles and the distance between the two GPS positions.

The assumption that the curve is a circle together with the difference in head-ing and the distance between the GPS receivers gives a radius of the circle. This can be written as β = Φi− ΦEGO (3.49) r = 1 sinβ₂ d 2

By calculating the arc length of the sector, dr, a more realistic value of the relative

distance is obtained. dr= rβ = (Φi− ΦEGO) 1 sinβ₂ d 2 (3.50)

It should be mentioned that the recalculated distance only makes sense when the headings differ a lot and the distance between the vehicles is big, e.g., when a long platoon enters a tight curve. Otherwise the recalculated distance can be neglected.

3.8 Adaptive Weight Matrices

As discussed above, there are many cases when information loss occurs, e.g. packet loss, radar loss etc.. Due to this, the estimated states get a discontinuity when the radar finds a target or measurements via WiFi returns. Since the estimated states are forwarded to the MPC, the states have to be smooth to obtain a smooth control signal. To avoid the discontinuity, adaptive weight matrices are introduced. In Appendix A, the matrices Qkand Rkare explained. These matrices are the process

noise and the measurement noise, respectively. This means that the matrices describe how the system relies on the model and on the measurements, respectively. When packet loss or radar loss occurs, the system relies more on the measure-ments from the Estimator than the model. Furthermore, when the radar finds a

(45)

3.8 Adaptive Weight Matrices 33

target or when measurements via WiFi returns again, the system relies more on the model to get a minor discontinuity. This is adjusted by decreasing Qk and

increasing Rk with respect to the time from the return. As the states are getting

closer to the measurements, the system relies more on the measurements i.e. Rk

(46)

(47)

Chapter 4

Experiments and Results

Since a major part of the master’s thesis is to implement the integrated systems described in Chapter 1, a large part of the resources have been invested in this. Therefore, both simulations including simulated and real data have been per-formed. This chapter covers a description of the environments for the two cases together with the obtained results. Both the results from the Sensor Fusion and the project are shown. Additionally, results in terms of fuel consumption at different runs in the environment are shown.

4.1 Simulation

To obtain results when the project (Estimator, Sensor Fusion and MPC) is run-ning, simulations have been performed. To run the entire system, a simulation environment was created. This environment consists of the three systems and ve-hicle models, almost the same structure as in Figure 1.3. The veve-hicle model has been developed by Scania CV AB and is a good description of a vehicle. The model uses the dynamics described in Chapter 2 together with an engine management system, transmission system and cruise controller.

To represent the measurements from the sensors as reliable as possible, the vehicle’s states are converted to the units that the Estimator requires and fed back to the Estimator.

The vehicles start at the same time but in different positions depending on the required time gap in between. The GPS coordinates are set such that the length of the vehicles is 7 meters. This length is the actual length of a vehicle without trailer.

The vehicle models only drive in the latitude direction, i.e. the longitude is constant and zero. This means that the vehicles drive straight forward in one direction. The heading for the vehicles is oriented to the north and therefore zero. Additionally, three cases are investigated. In the first case, noise was added to the signals to examine how robust the system is together with reference tracking. In the other case a packet loss was simulated and in the last case an unknown

(48)

36 Experiments and Results

vehicle enters the platoon. The results from the different scenarios are shown below.

4.1.1 Noisy Signals

In this scenario, noise is added to the measurements for the Estimator, i.e. the GPS and tacho signals. The noise is added after 50 seconds to investigate how the noise affects the signals. The added noise is in this case grater than the noise in the collected measurement. The driving scenario includes a two vehicle platoon with a reference velocity for the leader at 70 km/h. After 60 seconds the reference velocity increases to 80 km/h and after 200 seconds it is 75 km/h, finally the reference velocity is 70 km/h at the time 320 seconds.

Sensor Fusion

The results obtained from the Sensor Fusion are shown in Figure 4.1 - 4.2. The relative distances from the radar and Estimator (absolute position subtracted by the length) differ from each other because the Estimator is biased. This is due to the uncertainty in conversion of position signals from the vehicle model to GPS-coordinates. The profiles are the same for the two measurements since the data comes from the same source. The fused relative distance in Figure 4.1 is a combining of these two measurements and therefore the profile for this is the same as well. 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 4.1. The figure shows the relative distance between two simulated vehicles

when a simulated reference tracking has been performed. Additionally, noise is added to the measurements. In this plot the estimated length has been subtracted from the Estimator’s measurement.

The estimated length of the leader vehicle is shown in Figure 4.2. In this case the length varies around 7.3 and 7.6 meters compared to the real length, which

Sensor Fusion for Heavy Duty Vehicle Platooning

Institutionen för systemteknik

Department of Electrical Engineering

Examensarbete

Sensor Fusion for Heavy Duty Vehicle Platooning

Sensor Fusion for Heavy Duty Vehicle Platooning

Examensarbete utfört i Reglerteknik

vid Tekniska högskolan i Linköping

av

Abstract

Sammanfattning

Acknowledgments

Contents

Chapter 1

Introduction

1.1

Background

1.1.1

Platooning

1.1.2

Vehicle-To-Vehicle Communication

1.2

Goal

1.3

The Project

1.3.1

Estimator

1.3.2

Sensor Fusion

1.3.3

Model Predictive Control

1.4

Interface

1.4.1

Estimator − Sensor Fusion

1.4.2

Sensor fusion − MPC

1.5

Limitations

1.6

Related Work

1.7

Thesis Outline

Chapter 2

System Description

2.1

Vehicle Modelling

2.1.1

Powertrain

2.1.2

External Longitudinal Force

2.1.3

Vehicle Model

2.2

Sensors

2.2.1

Radar

2.2.2

Global Positioning System

2.3

Networks

2.3.1

Controller Area Network

2.3.2

WiFi

Chapter 3

Implementation

3.1

Platooning

3.2

Sensor Fusion

3.3

Filtering

3.3.1

Inputs and Parameters

3.3.2

Measurements

3.3.3

Global Reference System

3.3.4