Path Planning for Autonomous Heavy Duty Vehicles using Nonlinear Model Predictive Control

(1)

Institutionen för systemteknik

Department of Electrical Engineering

Examensarbete

Path Planning for Autonomous Heavy Duty

Vehicles using Nonlinear Model Predictive Control

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

av

Christoffer Norén

LiTH-ISY-EX--13/4707--SE

Linköping 2013

Department of Electrical Engineering Linköpings tekniska högskola

Linköpings universitet Linköpings universitet

(2)

(3)

Path Planning for Autonomous Heavy Duty

Vehicles using Nonlinear Model Predictive Control

Examensarbete utfört i Reglerteknik

vid Tekniska högskolan i Linköping

av

Christoffer Norén

LiTH-ISY-EX--13/4707--SE

Handledare: Tom Nyström

REPA, Scania CV AB

Isak Nielsen

isy, Linköpings universitet

Examinator: Daniel Axehill

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 2013-07-08 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--13/4707--SE

Serietitel och serienummer

Title of series, numbering

ISSN

—

Titel

Title

Ruttplanering för tunga autonoma fordon med olinjär modellbaserad prediktions-reglering

Path Planning for Autonomous Heavy Duty Vehicles using Nonlinear Model Pre-dictive Control Författare Author Christoffer Norén Sammanfattning Abstract

In the future autonomous vehicles are expected to navigate independently and manage complex traffic situations. This thesis is one of two theses initiated with the aim of researching which methods could be used within the field of autonomous vehicles. The purpose of this thesis was to investigate how Model Predictive Con-trol could be used in the field of autonomous vehicles. The tasks to generate a safe and economic path, to re-plan to avoid collisions with moving obstacles and to operate the vehicle have been studied. The algorithm created is set up as a hierarchical framework defined by a high and a low level planner. The objective of the high level planner is to generate the global route while the objectives of the low level planner are to operate the vehicle and to re-plan to avoid collisions. Op-timal Control problems have been formulated in the high level planner for the use of path planning. Different objectives of the planning have been investigated e.g. the minimization of the traveled length between the start and the end point. Ap-proximations of the static obstacles’ forbidden areas have been made with circles. A Quadratic Programming framework has been set up in the low level planner to operate the vehicle to follow the high level pre-computed path and to locally re-plan the route to avoid collisions with moving obstacles. Four different strate-gies of collision avoidance have been implemented and investigated in a simulation environment.

Nyckelord

Keywords Path Planning, MPC, NMPC, Autonomous Vehicle, Optimal Control, Move

(6)

(7)

Abstract

In the future autonomous vehicles are expected to navigate independently and manage complex traffic situations. This thesis is one of two theses initiated with the aim of researching which methods could be used within the field of autonomous vehicles. The purpose of this thesis was to investigate how Model Predictive Con-trol could be used in the field of autonomous vehicles. The tasks to generate a safe and economic path, to re-plan to avoid collisions with moving obstacles and to operate the vehicle have been studied. The algorithm created is set up as a hierarchical framework defined by a high and a low level planner. The objective of the high level planner is to generate the global route while the objectives of the low level planner are to operate the vehicle and to re-plan to avoid collisions. Op-timal Control problems have been formulated in the high level planner for the use of path planning. Different objectives of the planning have been investigated e.g. the minimization of the traveled length between the start and the end point. Ap-proximations of the static obstacles’ forbidden areas have been made with circles. A Quadratic Programming framework has been set up in the low level planner to operate the vehicle to follow the high level pre-computed path and to locally re-plan the route to avoid collisions with moving obstacles. Four different strate-gies of collision avoidance have been implemented and investigated in a simulation environment.

(8)

(9)

Sammanfattning

I framtiden förväntas autonoma bilar klara av att självständigt navigera och att hantera komplexa trafiksituationer. Det här arbetet är ett av två exjobb initie-rade med målet att undersöka vilka metoder som skulle kunna användas inom området autonoma bilar. Huvudsyftet med det här exjobbet var att undersöka hur modellbaserad prediktionsreglering (MPC) skulle kunna vara applicerbart in-om in-området av autonin-oma bilar. De uppgifter sin-om har undersökts är att generera en säker och ekonomisk färdväg, att omplanera rutten för att undvika kollisioner med föremål i rörelse och att styra fordonet. Den skapade algoritmen är uppsatt som ett hierarkiskt ramverk definierat av en hög- och en lågnivåplanerare. Mål-et med högnivåplaneraren är att generera den globala färdvägen och målen med lågnivåplaneraren är att styra fordonet och att omplanera rutten lokalt för att undvika kollisioner med hinder i rörelse. Ett optimalt styrningsproblem har blivit formulerat i högnivåplaneraren och används för ruttplanering. Olika syften med ruttplaneringen har undersökts t.ex. minimering av den totala färdvägen mellan start och ändpunkten. Statiska hinders förbjudna områden är approximerade som cirklar. Ett kvadratiskt programmeringsramverk (QP) har blivit inrättat i lågnivå-planeraren med syftet att reglera fordonet efter den referensväg som är skapad av högnivåplaneraren och att lokalt omplanera rutten för att undvika kollisioner med föremål i rörelse. Fyra olika strategier för att undvika kollisioner med andra före-mål i rörelse har blivit implementerade och undersökta i en simuleringsmiljö.

(10)

(11)

Acknowledgments

I would like to thank my examiner Daniel Axehill at Linköping University, my supervisor Tom Nyström at Scania and my supervisor Isak Nielsen at Linköping University for many interesting discussions and for contributing with excellent knowledge.

Furthermore, I would like to thank Tom Nyström, Jon Andersson and the group REPA at Scania for making this thesis possible and for making the collaboration with Bashar Mengana conceivable. I also want to thank Bashar Mengana and all other personnel at Scania for making all of the days enjoyable.

Finally, I would like to thank my family and friends for their love and support throughout my university studies.

Christoffer Norén

Södertälje, June 2013

(12)

(13)

Notation

Acronyms

Acronym Meaning

EKF Extended Kalman Filter

GPS Global Positioning System

IMU Inertial Measurement Unit

LQ Linear Quadratic

MIMO Multiple Input Multiple Output

MPC Model Predictive Control

NMPC Nonlinear Model Predictive Control

PI Proportional-Integral

QP Quadratic Programming

SLAM Simultaneous Localization And Mapping

General Notations

Notation Meaning

r(t) Reference signals in continuous time

r(k) Reference signals in discrete time

R Set of real numbers

u(t) Control signals in continuous time

u(k) Control signals in discrete time

U Set of control signals

x(t) States in continuous time

x(k) States in discrete time

X Set of states

(18)

(19)

Chapter 1

Introduction

1.1 Background

Future vehicles are expected to drive autonomously to increase safety in traffic. In addition to safe driving, vehicles are also expected to navigate independently in complex situations. The routes must be safe, economic and time-saving and this will require advanced technical solutions.

The DARPA Grand Challenge [1] is a well known competition for autonomous vehicles both in unstructured and urban environments. Unmanned vehicles op-erate through a city environment with ordinary intersections and need to take other manned and unmanned vehicles into account. The vehicles need to manage complex situations such as stop signs, road construction sites and other difficult events in order to reach their goal.

At Scania, interest lies in which methods could be used in the field of auto-nomous vehicles. The task is to perform an evaluation of different methods, to study their advantages and disadvantages. A priority is that the algorithms should satisfy real-time performance.

1.2 Problem Formulation

The problem consists of planning a path for a heavy duty vehicle in an unstructured environment, which can be an open space e.g. a parking lot. Only the size of the map is known at the beginning, and obstacles are detected using sensor information and added to the map as they arrive. The end point is given in GPS coordinates with a specified heading for the vehicle. The objective of the path planning is a safe and economic path from the start point to the end point. As the vehicle travels along the planned path, new obstacles will be detected by the sensors and re-planning of the route will be needed. In Figure 1.1 a visualization of the problem can be seen.

(20)

2 Introduction

Figure 1.1. Picture describing the problem. The grey heavy duty vehicle represents the

initial position with its radar operating range shown and the red vehicle represents the end position with a specified heading for the vehicle.

The vehicle is operated using three control signals: reference speed to the cruise controller, brake demand to the brake system and steering commands to the steer-ing unit.

1.3 Aim of the Thesis

The aim of the thesis is to create an MPC algorithm which is able to plan a safe and efficient path while avoiding obstacles. The algorithm needs to be fast to fulfill real-time performance demands. The focus will be how MPC can be used in the field of autonomous vehicles. Both the ability to plan a path for a heavy duty vehicle and operate it along this path will be investigated.

Parallel to this project a similar one is performed by Bashar Mengana [2], where the same problem is to be solved using graph search, which is an artificial intelligence algorithm.

(21)

1.4 Method 3

1.4 Method

At first, similar work with autonomous vehicles were studied to inspire and provide interesting and useful knowledge. Conclusions of these were studied in order to get a hint of what would be a good approach. Also, e.g. vehicle modeling, sensor fusion, motion models and possible optimization solvers were studied.

A simulation environment was created in collaboration with Bashar Mengana. It consists of a vehicle model describing the motion of the truck, a world model containing information of where the obstacles are placed, a radar simulating func-tion which provides the truck with informafunc-tion of visible obstacles, an EKF to perform sensor fusion to estimate the position and heading of the own vehicle, and a map generating algorithm creating the map with the information of the known obstacles.

The performance of the map generating algorithm was tested with a log from a test vehicle.

With conclusions from the pilot study a structure for the path planning algo-rithm was built and implemented in JModelica [3], MATLAB [4] and Simulink [5].

1.5 Previous Work

Previously, a hierarchical structure with two layers of MPC has been presented in [6]. The trajectory of the vehicle is given as an input to the framework. The top level is based on a simple point-mass vehicle description and is used to re-plan the path with a 4.5 seconds time horizon. The lower level is used as a controller for the vehicle with only 0.85 seconds time horizon. For the lower layer they have managed to implement an NMPC which can operate at 20 Hz and contains an advanced vehicle model. This approach can manage speeds up to 72 km/h, but is only performed in a simulation environment where the NMPC is controlling only the wheel angle.

Another similar approach with two layers has also been done in [7]. The high level planner uses motion primitives, which are predefined movement patterns that the vehicle is able to follow, to plan the path all the way to the final point. This reference trajectory is sent to the NMPC in the lower control layer, where the control signals needed for the car to follow the trajectory are computed. The NMPC uses steering and braking demands as control signals, operates at 20 Hz and has a short time horizon.

A hierarchical structure where the high level planner is using Tile planner (Grid planner) to structure the world into squares, which the car is allowed to enter or not, is used in [8]. The Tile planner does not take the non-holonomic nature of the vehicles into account, which may lead to infeasible global solutions. The non-holonomic nature means that the vehicle is not able to rotate on the spot. The global solution is sent to the low level MPC to locally plan the route and control the vehicle.

Another operation with a hierarchical structure has been done in [9], where the global trajectory is given by an offline trajectory generator. The highest level is used for re-planning and is based on an MPC algorithm operating at 20 Hz. A

(22)

4 Introduction

point-wise repulsive potential function Pkis used to avoid obstacles. The function

is described as: Pk = 1 (x − xo)2+ (y − yo)2+ (1.1) where

Pk – Value of the repulsive function x – X coordinate of the own vehicle

y – Y coordinate of the own vehicle

xo – X coordinate of the obstacle yo – Y coordinate of the obstacle

– A small value to prevent singularities

An example of a Pk function with xo = 5, yo = 5 and = 0.01 can be seen

in Figure 1.2. 4.5 5 5.5 4.5 5 5.5 0 20 40 60 80 100 X [m] Pk Repulsive function Y [m] Pk [Cost]

Figure 1.2. Repulsive function for xo= 5, yo= 5 and = 0.01.

In the objective function for the re-planning algorithm the sum of all Pk functions are added to prevent the car from choosing a path near the obstacles. The results of the simulations are that the computational time increases when an obstacle appears and that the real-time criterion is not fulfilled. Different time horizons of the MPC are used to compare the performances. The lowest layer, consisting of

(23)

1.5 Previous Work 5

an LQ or a PI controller, is regulating the vehicle using the control signal for the steering.

A path-planning and control algorithm for air planes using MPC to plan a safe path avoiding moving obstacles has been introduced in [10]. The airplanes motion is approximated to be within two dimensions and the forbidden areas are described with circles.

p

(xa− xo)2+ (ya− yo)2≥ d (1.2) where

xa – X coordinate of the air plane

ya – Y coordinate of the air plane

xo – X coordinate of the obstacle

yo – Y coordinate of the obstacle

d – Radius of the forbidden area

This constraint is nonlinear and in order to make it linear an approximation by hyperplanes combined with binary variables or by a single hyperplane has been used. The idea of the multiple hyperplanes surrounding the forbidden area is that the own airplane only needs to be on the correct side of one of the protecting planes to fulfill the criteria of not entering the area. A picture visualizing the positions of the protecting planes can be seen in Figure 1.3.

π 2 π 2 π 6 π 6 π 4 π 4 π 8 π 8 d (xo, yo)

Figure 1.3. Obstacle description with multiple hyperplanes.

A mixed integer linear programming algorithm combined with MPC has been used for an unmanned aircraft to avoid obstacles in [11]. The obstacles are described with hyperplanes combined with binary variables as in [10].

A mixed integer quadratic programming algorithm combined with MPC has been used in [12] for multi-vehicle path planning where the solver CPLEX [13] has been used. Different types of objective functions have been used to e.g. minimize

(24)

6 Introduction

the time, minimize the traveled length or minimize the use of energy in order to reduce fuel consumption.

Semi-autonomous parking has been done with MPC in [14], where a motion model for a non-holonomic vehicle with constraints has been used.

An optimal control problem is formulated for humanoid robots with holonomic and non-holonomic motions in [15]. The generated path should be fast, smooth and almost non-holonomic except for near targets and similar initial and final orientations.

1.6 Outline of the Report

The report is divided into the following chapters:

1. Introduction: This chapter gives the reader an idea of the content in the report with background, problem formulation and literature study included.

2. System Description: This chapter describes the heavy duty vehicle that the work is based on; how to operate it, the sensors and how to model its motion.

3. Control Theory: This chapter includes the control theory used in the thesis and gives information on the different types of controllers used, how to navigate and track the obstacles and information about how to discretize continuous time models.

4. Algorithm: This chapter describes the formulation of the path planning algorithm and how the hierarchical framework is divided into a high and a low level planner. Detailed information about the level planners is also presented.

5. Results: This chapter contains the results from the simulations of the high and low level planner and also the interaction between them.

6. Summary: This chapter contains a discussion, the conclusions made of this thesis and will present possible future work.

(25)

Chapter 2

System Description

In this chapter there is information about the vehicle that the path planning algorithm is designed for. The presented system description concerns the sensors, vehicle operation and motion model. In this thesis the ego vehicle refers to the own vehicle.

2.1 Vehicle

The vehicle to be used is called Conceptus, which is a 4x2 heavy duty vehicle of type Scania R730 and can be seen in Figure 2.1. R represents the cab type and 730 the maximum amount of horsepower produced by the engine. The weight of the vehicle is approximately eight tonnes.

Conceptus is able to turn with its front wheels as an ordinary car. The turning radius has a big impact on the planning, determining how sharp turns the vehicle is able to do. These limitations may lead to the need of the vehicle to go backwards in order to get around a sharp corner.

The vehicle can be operated through the following signals: reference speed to the cruise controller, brake demand to the braking system and steering commands to the steering unit. The operating signals can be seen in Table 2.1.

Operating Signal Notation Unit

Reference speed vRef [m_s]

Brake demand bd [m_s2]

Steering demand cd [rad]

Table 2.1. Operating signals.

(26)

8 System Description

Figure 2.1. Picture of Conceptus, Scania R730.

2.2 Sensors

The vehicle is equipped with a number of sensors to locate its position and to detect obstacles in the surrounding environment. In this section some of the important sensors are described. The received signals can be seen in Table 2.2.

Signal Notation Unit Source

Longitude long [deg] GPS

Latitude lat [deg] GPS

Heading ψ [rad] GPS

Obstacle position [r, φ] [m, rad] Radar

Speed v [m_s] Wheel speed sensor, GPS

Longitudinal acceleration ax [m_s2] IMU

Lateral acceleration ay [m_s2] IMU

Table 2.2. Received signals from the sensors.

2.2.1 GPS

The GPS sensor is used for positioning. It uses several satellites and receives messages from each satellite with time stamps. These time stamps are compared to

(27)

2.2 Sensors 9

the arrival time to compute how long time it took for each message to be received. The distances to each satellite are calculated by multiplying the travel times with the speed of light. These distances form a sphere around each satellite. The surface of these spheres are possible locations of the vehicle and the intersection between these spheres are used to determine the location of the vehicle. The different positions in time can later be used for estimation of heading and velocity.

2.2.2 Radar

The radar sensor is applied for obstacle detection and uses radio waves to detect directions and distances to obstacles. A transmitter in the radar unit emits radio waves, in predetermined directions, which are reflected when they collide with obstacles. A receiver in the unit absorbs the reflected signals and the distances to the obstacles are calculated using the time difference of transmission and reception. The directions to the obstacles are known due to the signals are being sent in predefined directions. In Figure 2.2 the operating range of the radar is shown.

[r

2

,φ

2

]

[r

1

,φ

1

]

Figure 2.2. Visualization of the radar’s operating range. The operating range is not

made to scale.

2.2.3 Inertial Measurement Unit

The IMU contains accelerometers and gyroscopes to measure acceleration in diff-erent directions. It is used to measure longitudinal and lateral accelerations. In this thesis the longitudinal direction is referred to as the heading of the vehicle and the lateral direction is referred to as perpendicular to the longitudinal direction and these can be seen as a local coordinate system. A picture describing the local and global coordinate systems can be seen in Figure 2.3.

(28)

X [m]

Y

[m

]

Global

coordinate

system

Local

coordinate

system

Figure 2.3. Definitions of the global and local coordinate systems.

2.2.4 Wheel Speed Sensors

The rotation speed of each wheel is measured with a wheel speed sensor. With use of the rotation speeds and the knowledge of the radius of each wheel, the speed of the vehicle is calculated.

2.3 Vehicle Dynamics and Kinematics

This section describes the vehicle model used in this thesis and the information is gathered from [14], [16] and [17]. The vehicle model includes the non-holonomic constraints of the vehicle.

2.3.1 Powertrain

A crucial part to get a good description of the motion is modeling the powertrain of a heavy duty vehicle and this is also used to calculate the longitudinal acceleration. In Figure 2.4 a picture describing the forces acting on the truck can be seen.

(29)

2.3 Vehicle Dynamics and Kinematics 11

α

Figure 2.4. Picture describing the forces acting on the truck.

The longitudinal acceleration of the truck can be described as:

ax =

1

mv

(Fpowertrain− Froll− Fair− Flong,gravity− Fbrake) (2.1)

where

ax – Longitudinal acceleration [m_s2]

mv – Mass of the vehicle [kg]

Fpowertrain – Force produced by the engine acting on the

power-train

[N ]

Froll – Force produced by the rolling resistance [N ]

Fair – Force produced by the air drag [N ]

Flong,gravity – The longitudinal component of the force produced

by the gravity

[N ]

Fbrake – Force produced by the braking system [N ]

Froll, Fairand Flong,gravity are described as:

Froll= mv(cr1+ cr2v2) (2.2) Fair= 1 2cwAaρav 2 _(2.3) Flong,gravity= mvg sin (α) (2.4) where

Aa – The front area of the vehicle [m2]

cr1 – Constant describing the rolling resistance [sm2]

(30)

cw – Constant describing the air drag resistance [1]

g – Gravity constant [m_s2]

v – The current speed of the vehicle [m_s]

α – Road slope [rad]

ρa – Density of air [_mkg3]

2.3.2 Kinematics

The kinematics is describing the motion of the vehicle. The kinematics used in this thesis are seen below and fulfill the non-holonomic nature of a vehicle.

˙

Xgc= v cos (ψ + β),

˙

Ygc= v sin (ψ + β)

(2.5)

The heading is calculated as:

˙

ψ = vcos (β) tan (δwheel)

l = vc (2.6)

The sideslip angle of the vehicle is calculated as:

β = arctan (vy

vx

) (2.7)

where

c – Current curvature of the vehicle [rad]

l – The length of the vehicle [m]

vx – Longitudinal speed in the local coordinate system [m_s] vy – Lateral speed in the local coordinate system [m_s]

Xgc – Global position in X [m]

Ygc – Global position in Y [m]

β – The sideslip angle of the vehicle [rad]

δwheel – Current wheel angle [rad]

ψ – Current heading of the vehicle [rad]

2.3.3 Modeling the Cruise Controller

To be able to use vRef as a control signal in the model description, the cruise

controller needs to be modeled. In this thesis the cruise controller is modeled as a PI controller. Fpowertrain= mv

(

Kp(vRef− v) + Ki Z (vRef− v)dt

)

(2.8) where Kp – Proportional constant [1_s] Ki – Integral constant [_s12]

(31)

2.3 Vehicle Dynamics and Kinematics 13

2.3.4 Modeling the Brake System

Since the brake system will not apply the brakes exactly when a demand is sent, a delay is modeled in the brake system using a first order system. The time delay is specified with a time constant.

˙ Fbrake = mv Tb (bd− Fbrake mv ) (2.9) where

Tb – Time delay of the brake system [s] bd – Brake demand to the brake system [_sm2]

2.3.5 Modeling the Steering System

Due to friction between the tires and the ground together with the response time in the steering system, the angle of the wheels will not change instantly when a command to the steering unit is transmitted. Therefore a time delay is modeled in the steering system using a first order system.

˙c = 1

Tsteer

(cd− c) (2.10)

where

cd – Desired curvature [rad]

(32)

(33)

Chapter 3

Control Theory

This chapter includes a brief overview of the control theory and the different types of controllers used, motion models used for navigation of obstacles and information about how to discretize continuous time models.

3.1 System

A system may be referred to as various types of applications e.g. a water tank. A system is dependent on input signals u(t), which may be of different types e.g. electrical, thermal or mechanical signals. If an input signal is used to control a system it is also referred to as a control signal. A system consists of internal states

x(t), which are related to the input signals. An input signal which can not be

controlled is seen as a disturbance and noted as w(t). Sensors may be put in a system for measuring interesting quantities, and these signals are referred to as the output signals y(t). A visualization of a system can be seen in Figure 3.1.

System

u(t) y(t)

w(t)

Figure 3.1. Picture of a system.

The dynamics of a system where no disturbances exist in discrete time can be modeled as in (3.1).

x(k + 1) = f (x, u, k),

y(k) = g(x, u, k) (3.1)

(34)

16 Control Theory

A special case of (3.1) is when the dynamics can be described with linear functions an can be seen in (3.2).

x(k + 1) = Ax(k) + Bu(k),

y(k) = Cx(k) + Du(k) (3.2)

where

A – Matrix describing how to update the new states with regard to the current states

B – Matrix describing how to update the new states with regard to the current control signals

C – Matrix describing how the current outputs are related to the curr-ent states

D – Matrix describing how the current outputs are related to the curr-ent input signals

f – Function describing how to calculate the new states according to the current states and control signals

g – Function describing how the outputs of the system are related to the states and control signals

3.2 Proportional-Integral Control

The PI controller [18] is a very simple controller and is used in a wide area of different plants. It is dependent on the proportional and integral gain and uses a reference signal r(t) and the feedback signal from a system y(t). The input to the PI controller is the difference between the reference signal and the output,

e(t) = r(t) − y(t), and the control signal can be calculated as:

u(t) = Kpe(t) + Ki t Z 0 e(τ )dτ where

e(t) – Control error

Kp – Proportional gain Ki – Integral gain

(35)

3.3 Linear Quadratic Control 17 r(t) P _K p Ki R P System

e(t) u(t) y(t)

-Figure 3.2. Picture of the structure of a PI controller.

3.3 Linear Quadratic Control

The LQ controller [19] is a popular state feedback controller that is able to handle MIMO systems easily. The control signals in a state feedback system are based on the states of the system. The problem is formulated as an optimization problem where the linear dynamics of the system are added as constraints. The weight matrices P1and P2are design variables making it possible to tune the system into

achieving a certain behavior. The optimization problem can be formulated as:

minimize u ∞ X k=0 kx(k) − r(k)kP1+ ku(k)kP2 subj. to, x(k + 1) = Ax(k) + Bu(k) (3.3) where

P1 – Positive semidefinite weight matrix P2 – Positive definite weight matrix

The algebraic solution to the optimization problem is the state feedback u(k) = −Lx(k) + Lrr(k), where L is the state feedback gain and Lr is the feed forward

gain for the reference signals. The optimization problem is solved offline and the optimal state feedback gain is used as in Figure 3.3 to control a system.

(36)

18 Control Theory r(k) Lr P System L Lrr(k) u(k) y(k) x(k) Lx(k)

-Figure 3.3. Picture of the structure of a LQ controller.

3.4 Model Predictive Control

Model Predictive Control [19], [20] or MPC, is an advanced control strategy used in many applications today. MPC is able to handle MIMO systems and uses a model of the system to predict what will happen to the system in the future. The idea of MPC is to formulate the control problem into an optimization problem over a finite time horizon. The optimization problem is dependent on a criterion that will be minimized (also referred to as the objective function) with respect to the control signals. The objective function is formulated to achieve a certain behavior, e.g. to make the states follow a specified reference vector. In the optimization formulation, state and control signal constraints can be added to take the limitations of the control signals and possible regions of the states into account.

The general formulation of an MPC optimization problem in discrete time could look like:

minimize u N −1 X k=0 f0(x, r, u, k), subj. to, x(k + 1) = f (x, u, k), x(k) ∈ Sx ⊂Rn, ∀k, u(k) ∈ Su⊂Rm, ∀k (3.4) where N – Prediction horizon

f0 – The objective function or the criterion to minimize

f – The linear model of the system describing how to update the states

Sx – Constraint set for the states

Su – Constraint set for the control signals m – Number of control signals

n – Number of states

(37)

3.4 Model Predictive Control 19

Tp= TsN

where

Tp – Prediction time Ts – Sampling time

In Algorithm 1 a description of how the optimization problem 3.4 can be used to control a system is presented.

Algorithm 1 MPC

1: Measure or estimate x(k)

2: Calculate the control sequence u(k), . . . , u(k + N − 1) according to the opti-mization problem 3.4

3: Use u(k) in the system. 4: Time update, k := k + 1. 5: Repeat from step 1.

3.4.1 Objective Function

The objective function can be seen as the cost function and is used in order to get the system to behave in a certain way. The most common objective function in the context of MPC is the quadratic one and it is used to get the states to follow a specified reference vector and at the same time penalize the use of control signals. Q1 and Q2 are weight matrices used to prioritize wanted performance. A

demonstration of the objective function is shown in (3.5).

N −1 X k=0 f0(x, r, u, k) = N −1 X k=0 kx(k) − r(k)kQ1+ ku(k)kQ2 (3.5) where

Q1 – Positive semidefinite weight matrix Q2 – Positive semidefinite weight matrix

3.4.2 Inequality Constraints

Inequality constraints are used to prevent the states from being in prohibited areas and to take the limitations of the control signals into account. These constraints will never be violated for a feasible problem. An example of how the constraints can be formulated is presented in (3.6).

xmin≤ x(t) ≤ xmax, ∀t umin≤ u(t) ≤ umax, ∀t

(38)

3.4.3 Nonlinear Model Predictive Control

Nonlinear Model Predictive Control is similar to MPC; the difference is that non-linear equations are accepted in the model description f , the objective function f0

and in the equations of the constraints. The general NMPC optimization problem in discrete time can be formulated as:

minimize u N −1 X k=0 f0(x, r, u, k), subj. to, x(k + 1) = f (x, u, k), x(k) ∈ Sx⊂Rn, ∀k, u(k) ∈ Su⊂Rm, ∀k (3.7)

3.5 Optimal Control Formulation

An optimal control problem is similar to the MPC problem but is used when initial and end states for the system are given and when a different type of objective function should be minimized, e.g. minimization of the energy used by the system. A generalized optimal control formulation in continuous time could look like:

minimize u T Z t=t0 f0(x, r, u, t)dt, subj. to, ˙ x = f (x, u, t), x(t0) ∈ Sx0, x(t) ∈ Sx ⊂Rn, ∀t, u(t) ∈ Su⊂Rm, ∀t, x(T ) ∈ SxT (3.8) where T – Final time t0 – Initial time

Sx0 – Initialization set for the states

SxT – Constraint set for the states in final time

Example 3.1: Goddard’s Rocket

Goddard’s rocket is a famous optimal control problem. The objective of the rocket is to get as high up in the air as possible with a limited amount of fuel. Would the best solution be to burn all the fuel in the beginning which makes the rocket

(39)

3.6 Move Blocking 21

lighter or would it be to keep a constant low throttle? The rocket is dependent on three states: the speed v(t), the traveled height h(t) and the mass m(t). The control signal u(t) is the thrust for the rocket. The optimization problem could be set up as: maximize u h(T ) subj. to, v(0) = 0, h(0) = 0, m(0) = m0, ˙h = v, ˙v = 1 m(u − Fgravity− Fair(v, h)), ˙ m = −γu, 0 ≤ u(t) ≤ umax, ∀t, v(T ) = 0, m(T ) = mT (3.9) where

h(t) – Height of the rocket in continuous time

v(t) – Speed of the rocket in continuous time

m(t) – Mass of the rocket in continuous time

u(t) – Control signal controlling the thrust of the rocket in continuous time

T – Final time

m0 – Initial weight of the rocket with fuel mT – Final weight of the rocket without fuel

Fgravity – Force due to gravity

Fair – Force due to the air drag, dependent on the speed and height of

the rocket

γ – Constant describing the fuel consumption at different throttle

umax – Limitation of the control signal

3.6 Move Blocking

Move blocking is an efficient way of reducing the computational complexity in MPC optimization problems and has been used in, e.g., [21], [22] and [23]. The idea of move blocking is to parameterize the control signal and therefore reduce the number of decision variables in the optimization. One way of doing this is to keep the control signal constant over a number of samples. This can be done with the variable substitution:

(40)

22 Control Theory U = ΩMUM (3.10) where U =     u(1) .. . u(N )     , UM=     uM(1) .. . uM(NM)    

U – All control signals

UM – Reduced set of control signals

ΩM – Matrix describing the relation between U and UM N – Number of time steps

NM – Number of time steps in the reduced set

The relation between N and NMis:

NM≤ N

Example 3.2: An Example of Move Blocking

Imagine a case where N = 4 and four time steps of control signals are to be determined. U =      u(1) u(2) u(3) u(4)     

In order to reduce the computational complexity a move blocking matrix ΩM is

used. ΩM=      1 0 0 0 1 0 0 0 1 0 0 1     

The number of rows in ΩM is equal to N and the number of columns is equal to NM, so in this case: NM= 3, UM=    uM(1) uM(2) uM(3)   

The full relation is:

     u(1) u(2) u(3) u(4)      =      1 0 0 0 1 0 0 0 1 0 0 1         uM(1) uM(2) uM(3)   

(41)

3.7 Motion Models 23 Meaning:    uM(1) = u(1) uM(2) = u(2) uM(3) = u(3) = u(4)   

The last two control signals are constrained to take the same value and therefore the computational complexity is reduced.

3.7 Motion Models

Good motion models [24] are essential for tracking and predicting the motion of obstacles. Sensor fusion is used to select and adapt suitable motion models of each obstacle. Information about the different models are summarized below.

3.7.1 Constant Position

A constant position model is used for static obstacles. The differential equations of the states can be seen in (3.11).

˙ Xo= 0, ˙ Yo= 0 (3.11) where

Xo – The obstacle’s X coordinate in global coordinates Yo – The obstacle’s Y coordinate in global coordinates

3.7.2 Constant Speed

A constant speed motion model is used for obstacles traveling in a straight line with constant speed. A visualization can be seen in Figure 3.4. The model is formulated with polar velocity and the heading of the motion. The states and their differential equations are presented in (3.12).

˙ Xo= vocos (ψo), ˙ Yo= vosin (ψo), ˙vo= 0, ˙ ψo= 0 (3.12) where

vo – The total speed of the obstacle ψo – The heading of the obstacle

(42)

v

Figure 3.4. An obstacle traveling according to the constant speed motion model.

3.7.3 Coordinated Turns Polar Velocity

The coordinated turns motion model is used for obstacles traveling along a circle with a constant radius with constant speed. A visualization can be seen in Fig-ure 3.5. The motion is described with polar velocity and a change rate constant for the heading. The equations of the model are shown in (3.13).

˙ Xo= vocos (ψo), ˙ Yo= vosin (ψo), ˙vo= 0, ˙ ψo= ω, ˙ ω = 0 (3.13)

where ω is the angular velocity.

v

(43)

3.8 Extended Kalman Filter 25

3.8 Extended Kalman Filter

The Extended Kalman Filter [24], can be used for sensor fusion between diff-erent types of sensors and therefore be used for estimation of states. The EKF is dependent on two steps: the measurement update and the time update. In the measurement update new measurements of the states are collected. In the time update a prediction of the states in the next time step is made. The prediction is based on a model describing the behavior of the system and the current states of the system. In Algorithm 2 pseudo code for the EKF can be seen. The estimation is based on Gaussian noise specified for the predictions and measurements and the results of the filter are the statistically optimal estimations of the states. The EKF is able to handle nonlinear functions due to linearization using Taylor expansion.

Algorithm 2 Extended Kalman Filter with a first order Taylor expansion 1: _{procedure EKF}

2: k := 1 . Initialize the step counter

3: xˆ1|0:= x0 . Initialize ˆx

4: P1|0:= P0 . Initialize Pk

5: while T rue do

6: Measure yk . Measurement update

x – Estimated states in discrete time

k – Step counter

x0 – Initialization values of the states, often taken from the first measurement Pk – Uncertainty matrix

P0 – Initialization values of the uncertainty matrix

f – Function describing the models behavior used for prediction

h – Function describing how the states are connected to the measured states

Sk – Innovation covariance

Kk – Kalman gain

k – Difference between predictions and measurements

Rk – Gaussian covariance for the measurements

(44)

3.9 Linearization of Nonlinear Models

Linearization of a nonlinear system can be done using Taylor expansions around a linearization point (x, u). How the linearization is made is seen in (3.14).

˙ x = f (x(t), u(t)) ≈ f (x, u) +∂f ∂x x = x u = u (x(t) − x) +∂f ∂u x = x u = u (u(t) − u) (3.14) where

f – The function to be linearized

x – State values in the linearization point

u – Control signal values in the linearization point

3.10 Continuous to Discrete Time Models

This section describes how to transform models from continuous time to discrete time and the theory is gathered from [20], [25]. Discrete time is used when sampling a system. Two discretization methods are presented in this section .

3.10.1 Forward Euler Discretization

Forward Euler is an explicit discretization method, meaning that the new states are only dependent on old states. Shown below is a presentation of the method: Continuous time:

˙

x(t) = f (t, x(t))

where f is the function describing the differential equations of the states. Discretization using Forward Euler:

x(k + 1) = x(k) + hf (k, x(k))

where h is the defined step length.

3.10.2 Backward Euler Discretization

Backward Euler is an implicit discretization method, meaning that the new states

x(k + 1) appears on both sides of the equation and therefore an equation system

needs to be solved. A demonstration of the method is presented below: Continuous time:

˙

x(t) = f (t, x(t))

Discretization using Backward Euler:

(45)

Chapter 4

Algorithm

In this chapter the details of the path planning algorithm are presented; how it is divided and how the tasks to generate a global path, to operate the vehicle and how to locally re-plan in order to avoid collisions with moving obstacles are handled.

The intention is to divide the path planning algorithm into two different parts defined by a high and a low level planner. The high level planner’s objective is to generate a global path from the current position to the end point. When planning the path, it is supposed to take all of the known obstacles that are relevant into account and will only have to re-plan in case the map has changed. The low level planner’s objective is to control the vehicle to follow the path that is pre-computed by the high level planner and at the same time prevent it from traveling close to potential moving obstacles nearby the given path. Because of the high and low level planners’ dependence on accurate estimations of the own vehicle position, its heading and a global map that contains locations of the obstacles, a state estimator is needed. The structure of the hierarchical path planning algorithm is seen in Figure 4.1.

User input High level

planner Low level planner Vehicle State estimator ES xpath u y ˆ x ˆ x

Path planning algorithm

Figure 4.1. Picture describing the structure of the the hierarchical framework.

(46)

28 Algorithm

where

ES – The end state given in X, Y coordinates, with speed in m/s and

heading in radians

xpath – The global path given in X, Y coordinates and with heading in

radians

u – Control signals

y – Sensor and vehicle information from the vehicle ˆ

x – Estimated states and map produced by the state estimator

4.1 State and Map Estimation

The state and map estimator estimates the states of the own vehicle and generates a map with the locations of the static and moving obstacles. In Figure 4.2 the structure of the state and map estimator is shown. The state and map estimator is made in collaboration with Bashar Mengana.

EKF Map generating algorithm Vehicle information Sensor information ˆ xego ˆ xmap ˆ xmObs ˆ xego ˆ x State estimator

Figure 4.2. Picture visualizing the state estimator.

where

Vehicle information – GPS, speed and acceleration information of the own vehicle

Sensor information – Obstacles detected in the sensor region ˆ

xego – Estimated states of the own vehicle

ˆ

xmap – Generated static map

ˆ

xmObs – Information about the moving obstacles

4.1.1 Estimation of the Ego Vehicle States

An EKF with a first order Taylor expansion is used to estimate the position and heading of the own vehicle. The prediction model that is used is described in (4.1) and the estimations are calculated as in Algorithm 2.

(47)

4.2 High Level Planner 29

Xgc(k + 1) = Xgc(k) + h(v(k) cos (ψ(k) + β(k))), Ygc(k + 1) = Ygc(k) + h(v(k) sin (ψ(k) + β(k))),

ψ(k + 1) = ψ(k)

(4.1)

4.1.2 Map Generating Algorithm

Since the positions of the reported obstacles are given in the local reference sy-stem, the estimated position and heading of the own vehicle are used to place the obstacles on a global map. The map is created continuously as the vehicle travels and detects new obstacles. In order to avoid placing the same obstacle on the map several times, a comparison is done with the existing map to check if the obstacle already exists. The map is used by the planners to generate a safe route.

4.2 High Level Planner

The high level planner is formulated as an optimal control problem and used to generate a safe and economic path, which means that it will not interfere with the positions of the obstacles and will be fuel efficient. When planning the path with the high level planner, only the static obstacles are taken into account. The moving obstacles are instead processed by the low level planner. The current states of the own vehicle, the end states specified by the user and the map with the locations of the static obstacles are used to formulate the optimal control problem. The different objective functions used in the problem are presented below in Section 4.2.3. The high level planner is implemented in ACADO [26], [27], [28], IPOPT [29] and SNOPT [30] using the optimization toolbox YALMIP [31]. In Figure 4.3 a visualization of a typical scenario for the high level planner is shown.

(48)

30 Algorithm

Figure 4.3. The high level planner generates a path between the start and end point.

The gray vehicle visualizes the start point and the red the end point.

4.2.1 Representation of Static Obstacles

The static obstacles and their forbidden areas are approximated as circles described by a center point and radius. Since the forbidden area is represented as a circle, it is possible to travel on both sides of it. The equation for a circle is shown in (4.2).

q

(Xo− Xgc)2+ (Yo− Ygc)2≥ r (4.2)

where r is the radius of the forbidden area.

A visualization of the forbidden area described with a circle can be seen in Fig-ure 4.4.

(X

o

, Y

o

)

r

(49)

4.2 High Level Planner 31

4.2.2 State Space Description

The motion model of the vehicle that is used is a simple point-mass description satisfying the non-holonomic nature of a vehicle. The model is limited to four states in order to reduce the computational complexity. Constraints are used to limit the speed and heading within an interval and the control signals such as the maximum and minimum wheel angle and acceleration of the vehicle.

States and control signals:

Xh=     Xgc Ygc v ψ     , Uh= a δwheel (4.3)

where a is the control signal for the acceleration of the vehicle in [m s2].

The differential equations of the states:

˙ Xh=     ˙ Xgc ˙ Ygc ˙v ˙ ψ     =     v cos(ψ) v sin(ψ) a vtan(δwheel) l     (4.4)

The constraints for the states and control signals:

 vmin ψmin ≤ v ψ ≤ vmax ψmax , amin δwheel,min ≤Uh≤ amax δwheel,max (4.5)

4.2.3 Objective Functions

The objective function is the criterion that is minimized in the optimal control problem and is formulated in a way that makes the generated path economic. With different objective functions, different properties of the generated paths can be seen. Below are the objective functions listed separately, but a combination between any of these is possible:

Minimize the time – minimize

u T Z

t=0 1dt

Minimize the traveled distance – minimize

u T Z

t=0 |v|dt

Minimize the use of steering commands – minimize u

T Z

t=0

δ_wheel2 dt

Minimize the use of acceleration demands – minimize u

T Z

t=0

(50)

32 Algorithm

4.2.4 Summary of the High Level Planner

The high level planner is used to generate a route and re-plan when the map of static obstacles has changed. If the map has not changed, the old generated path is used and sent to the low level planner. The sample rate of the high level planner is low since re-planning the route takes time. In Figure 4.5 a summary of the optimization problem is seen.

Optimal control problem

Objective function Motion model Constraints for static

obstacles Limitations of states

and control signals

High level planner

Figure 4.5. A summary of the optimization problem setup for the high level planner.

In Algorithm 3 the procedure of the high level planner is described in pseudo code.

Algorithm 3 High level planner 1: _{procedure High level planner}

2: k := 1 . Initialize the step counter

3: Initialize the controller

4: xˆego := retrieveTheStatesOfTheVehicle();

5: ES:= getEndState();

6: while vehicleNotInGoal(ˆxego, ES) do

7: xˆego := retrieveTheStatesOfTheVehicle();

8: xˆmap := retrieveTheStatesOfTheMap();

9: if mapHasChanged(ˆxmap) then

10: xpath := solveOptimalControlProblem(ˆxego, ES, ˆxmap);

11: end if

12: Send xpath to the Low level planner

13: k := k + 1

14: end while 15: end procedure

(51)

4.3 Low Level Planner 33

4.3 Low Level Planner

The low level planner manages two tasks: operate the vehicle to follow the path generated by the high level planner and locally re-plan the route when moving obstacles appear close to the reference road. The static and moving obstacles are taken into account when tracking the reference path or re-planning. To avoid collisions with moving obstacles, logical decision making is implemented in the low level planner deciding when re-planning is needed. The optimization pro-blem is formulated as a QP propro-blem and move blocking has been used to reduce the computational complexity. The prediction horizon is a few seconds. The low level planner is implemented in SNOPT [30] using the optimization toolbox YALMIP [31]. In Figure 4.6 a typical scenario for the low level planner is seen.

v Reference Path (X gc , Y gc )

Figure 4.6. The low level planner operates the vehicle along the reference path and

re-plans when moving obstacles appear close to the reference road.

4.3.1 State Space Description

The vehicle model that is used describes the kinematics, the brake system, the steering unit and the cruise controller. The control signals are the settling speed to the cruise controller, the desired retardation to the brake system and steering commands to the steering unit. The motion model is the one derived in Sect-ion 2.3 discretized using Backward Euler discretizatSect-ion according to SectSect-ion 3.10.2. States and control signals:

Xl=           Xgc Ygc v ψ c ei Fbrake           , Ul =   vRef cd bd   (4.6)

(52)

34 Algorithm

The discretized equations of the motion model:

Xl(k + 1) =           Xgc(k + 1) Ygc(k + 1) v(k + 1) ψ(k + 1) c(k + 1) ei(k + 1) Fbrake(k + 1)           =           Xgc(k) Ygc(k) v(k) ψ(k) c(k) ei(k) Fbrake(k)           + . . . h           v(k + 1) cos(ψ(k + 1) + β(k + 1)) v(k + 1) sin(ψ(k + 1) + β(k + 1)) 1

mv(Fpowertrain(k + 1) − Fexternal(k + 1) − Fbrake(k + 1))

v(k + 1) cos (β(k + 1))c(k + 1) 1 Tsteer(cd(k) − c(k + 1)) vRef(k) − v(k + 1) mv Tb(bd(k) − Fbrake(k+1) mv )           (4.7) where

Fexternal= Fair+ Froll+ Flong,gravity

The constraints for the states and control signals:

 vmin ψmin ≤ v ψ ≤ vmax ψmax ,   vRef,min cd,min bd,min  ≤Ul≤   vRef,max cd,max bd,max   (4.8)

4.3.2 Representation of Static Obstacles

Since a circle description of the obstacle’s forbidden area is nonlinear, an ap-proximation is made by a single hyperplane to make the constraint linear. The hyperplane is placed perpendicular to the vector between the own vehicle and the obstacle and put on the surface of the forbidden area. The idea is that the own vehicle must be on the correct side of the hyperplane at all times and this method is also used in [10] for obstacle representation. In Figure 4.7 a visualization of the single hyperplane can be seen.

(53)

4.3 Low Level Planner 35 (Xo, Yo)

π

r (Xgc, Ygc) (Xplane, Yplane) n

Figure 4.7. Obstacle description with a single hyperplane.

The normal of the plane is based on the normalized vector between the own vehicle and the position of the obstacle. The normal n is calculated as in (4.9).

vgc,o= Xo Yo −Xgc Ygc n =nx ny = vgc,o |vgc,o| (4.9)

The point selected in the plane (Xplane, Yplane) is one of the two intersection points

between the circle describing the forbidden area of the obstacle and the infinitely long line created from the positions of the own vehicle and the obstacle. The intersection point chosen as the base of the plane (Xplane, Yplane) is the one that

is closest to the own vehicle. The equation of the circle is presented in (4.10) and the equation of the line is shown in (4.11).

(Xo− Xgc)2+ (Yo− Ygc)2= r2 (4.10)

ygc,o= kgc,oxgc,o+ mgc,o (4.11)

where kgc,o and mgc,o are calculated as in (4.12).

kgc,o= ∆Y ∆X = Yo− Ygc Xo− Xgc , mgc,o= Ygc− kgc,oXgc (4.12)

(54)

36 Algorithm

There exist a special case of the line when Xo = Xgc and Yo 6= Ygc. The line is

then expressed as in (4.13).

xgc,o= Xo (4.13)

The setup of the constraint used in the optimization is presented in (4.14).

nx(Xgc− Xplane) + ny(Ygc− Yplane) ≤ 0 (4.14)

where

kgc,o – Slope of the line between the own vehicle position and the

obsta-cle’s position

mgc,o – Offset of the line between the own vehicle position and the

obsta-cle’s position

nx – The X component of the normal of the plane ny – The Y component of the normal of the plane

vgc,o – Vector between the own vehicle’s position and the obstacle’s

posi-tion

xgc,o – X coordinate of the line between the own vehicle’s position and

the obstacle’s position

Xplane – X coordinate of a point in the plane

ygc,o – Y coordinate of the line between the own vehicles position and the

obstacle’s position

Yplane – Y coordinate of a point in the plane

4.3.3 Representation of Moving Obstacles

In this thesis four different methods for moving obstacle avoidance have been used. One of these methods is initiated when a collision is imminent. They are all using planes to determine which areas the own vehicle is allowed to be in. The planes are calculated using the states of the own vehicle, the states of the moving obstacle and the reference road. The motion models in Section 3.7 have been used giving the ability to predict the future positions of the obstacle. All of the planes are calculated in advance and used as contraints in the optimization problem.

Method 1

Place a plane from a point in front of the own vehicle to the rear of the obstacle’s forbidden area. This method is forcing the own vehicle to choose a path behind the moving obstacle. In Figure 4.8 a visualization of the protecting plane can be seen.

(55)

4.3 Low Level Planner 37 π v Reference Path (X gc , Y gc ) (P1X, P1Y) (P2X, P2Y)

Figure 4.8. Visualization of the protecting plane used in obstacle avoidance method 1.

The constraint is set up as in (4.15).

Ygc− mcXgc− bc≤ 0 (4.15)

where mc and bc are calculated as in (4.16).

mc= ∆Y ∆X =P2Y− P1Y P2X− P1X , bc= P1Y− mcP1X (4.16) where

P1X – X coordinate of the point in front of the own vehicle P1Y – Y coordinate of the point in front of the own vehicle

P2X – X coordinate of the point placed on the rear of the obstacle’s

for-bidden area

P2Y – Y coordinate of the point placed on the rear of the obstacle’s

for-bidden area

Two special cases of the constraint exist when mc = ±∞. In case 1 where P1X= P2X, P1Y6= P2Yand Xgc≤ P1X= P2Xthe constraint is set up as in (4.17).

Xgc− P1X≤ 0 (4.17)

In case 2 where P1X= P2X, P1Y 6= P2Y and Xgc ≥ P1X = P2X the constraint is

set up as in (4.18).

− Xgc+ P1X≤ 0 (4.18)

Method 2

Place a plane in the same direction as the motion of the obstacle on the surface of the forbidden area, forcing the own vehicle to stop in front of the protecting plane.

Path Planning for Autonomous Heavy Duty Vehicles using Nonlinear Model Predictive Control

Institutionen för systemteknik

Department of Electrical Engineering

Examensarbete

Path Planning for Autonomous Heavy Duty

Vehicles using Nonlinear Model Predictive Control

Path Planning for Autonomous Heavy Duty

Vehicles using Nonlinear Model Predictive Control

Examensarbete utfört i Reglerteknik

vid Tekniska högskolan i Linköping

av

Abstract

Sammanfattning

Acknowledgments

Contents

Notation

Chapter 1

Introduction

1.1

Background

1.2

Problem Formulation

1.3

Aim of the Thesis

1.4

Method

1.5

Previous Work

1.6

Outline of the Report

Chapter 2

System Description

2.1

Vehicle

2.2

Sensors

2.2.1

GPS

2.2.2

Radar

[r

,φ

]

[r

,φ

]

2.2.3

Inertial Measurement Unit

X [m]

Y

[m

]

Global

coordinate

system

Local

coordinate

system

2.2.4

Wheel Speed Sensors

2.3

Vehicle Dynamics and Kinematics

2.3.1

Powertrain

α

2.3.2

Kinematics

2.3.3

Modeling the Cruise Controller

(

)

2.3.4

Modeling the Brake System

2.3.5

Modeling the Steering System

Chapter 3

Control Theory

3.1

System

3.2