Integration of a Stochastic Model Predictive Control and an Occupancy Grid for Autonomous Driving Highway Scenario

UNIVERSITY OF PISA

Department of Information Engineering

Master’s Degree in Robotics and Automation Enginnering

THESIS

Integration of a Stochastic Model Predictive

Control and an Occupancy Grid for

Autonomous Driving Highway Scenario

Internal supervisor Prof. Lucia Pallotino External supervisor M.Sc. Tim Br¨udigam

Candidate

B.Sc. Fulvio Di Luzio

Abstract

This thesis presents an integration of an Occupancy Grid (OG) with a Model Pre-dictive Control (MPC) framework for autonomous vehicles in highway scenarios. The highway is represented as an evenly spaced field of cells. At first Stochastic Model Predictive Control (SMPC) planning considers a set of potential maneuvers of other traffic participants by assigning an occupancy probability to the cells that represent the highway. In order the guarantee the safety and the property of recur-sive feasibility of the algorithm, a second control is designed, called Failsafe Model Predictive Control (FMPC). This one takes into account the worst-case realization of other traffic participants. The combination of SMPC and FMPC provides an efficient, fast and feasible algorithm whose results will be evaluated in a simulated environment.

Questa tesi presenta l’integrazione di una Occupancy Grid (OG) con un Model Pre-dictive Control (MPC) per la guida autonoma di veicoli in autostrada. L’autostrada `

e rappresentata da un insieme di celle di griglia. Dapprima un Stochastic Model Pre-dictive Control (SMPC) considera un insieme di possibili manovre degli altri veicoli presenti nell’ambiente assegnando alle celle della griglia dei valori di probabilit`a che esprimono la possibilit`a che una cella sia occupata da un certo veicolo, e pianifica la traiettoria. In modo da garantire sicurezza e la fisibilit`a ricorsiva dell’algoritmo, viene progettato un secondo controllore chiamato Failsafe Model Predictive Con-trol (FMPC). Questo ultimo prende in considerazione il worst-case scenario delle manovre degli altri veicoli presenti nell’ambiente. La combinazione di SMPC e FM-PC porta ad un efficiente, veloce e sicuro algoritmo i cui risultati sono valutati in simulazione.

Zusammenfassung

Diese Masterarbeit pr¨asentiert eine Kombination von Occupancy Grid (OG) und Model Predictive Control (MPC) f¨ur selbstfahrende Autos in Autobahn. Die Auto-bahn wird als ein Feld von Zellen dargestellt. Im ersten Moment bedenkt ein Stocha-stic Model Predictive Control (SMPC) eine Reihe von den m¨oglichen Man¨overn der anderen Fahrzeuge und weist den Zellen eine Belegungswahrscheinlichkeit zu, die repr¨asentieren die Autobahn. Dann wird ein zweiter Controller entwickelt, den wir Failsafe Model Predictive Control (FMPC) nennen, um die Sicherheit und die Ei-genschaft der rekursiven Machbarkeit des Algorithmus zu garantieren. Dieser zweite Controller bedenkt das Worst-Case-Szenario von den Man¨overn der anderen Fahr-zeuge in der Autobahn. Die Kombination von SMPC und FMPC bietet einem effizi-enten, schnellen und machbaren Algorithmus, dessen Ergebnisse in einer simulierten Umgebung bewerbt werden.

3

This thesis has been conducted at the Chair of Automatic Control of the Technical University of Munich under the supervision of M.Sc. Tim Br¨udigam.

CONTENTS 5

Contents

2.1.1 Linear model . . . 11

2.1.2 Kinematic bicycle model . . . 12

2.1.3 Road-aligned coordinate frame . . . 13

2.1.4 Ego vehicle constraints . . . 14

2.2 Target vehicle model . . . 15

2.3 Controllers . . . 15

2.3.1 Stochastic MPC . . . 15

2.3.2 Failsafe MPC . . . 16

3 Method 19 3.1 Occupancy Grid . . . 19

3.2 Occupancy Grid for SMPC planning . . . 20

3.2.1 Probabilistic Occupancy Grid . . . 21

3.2.2 Extension to multiple TVs . . . 24

3.2.3 Extension to multiple maneuvers . . . 24

3.2.4 Binary Occupancy Grid . . . 25

3.2.5 Other chance constraint approximations . . . 26

3.3 Occupancy Grid for FMPC planning . . . 26

3.4 Computing EV state constraints . . . 29

3.4.1 Bresenham’s line algorithm . . . 29

3.4.2 Determine admissible convex hull . . . 30

3.5 Curved grid . . . 33

3.6 Combining Stochastic and Failsafe MPC . . . 35

4 Evaluation 39

4.1 Simulations . . . 39

4.1.1 Linear model . . . 40

4.1.2 Nonlinear model (straight road) . . . 43

4.1.3 Nonlinear model (curved road) . . . 45

4.2 Probability threshold pth . . . 50

4.3 Computational cost time analysis . . . 50

5 Discussion 55

6 Summary and Outlook 57

A Simulation parameters 59

List of Figures 61

7

Chapter 1

Introduction

The development of autonomous vehicles has several benefits: reduced number of incidents due to human errors, optimized transportation and fuel consumption, less traffic congestions both in city and highway scenarios, more accessibility to people with disabilities or to seniors. In order to ensure all these points, the research today is mainly concerned with the development of algorithms that make driving a car safe. This means having cars able to handle uncertainty in other traffic participants’ behavior and respond with finely tuned braking and acceleration. The first point is treated in the literature as a trade-off between conservativeness and risk. Taking into account every potential external behavior leads to inefficiency of the algorithm, resulting in inability to move the vehicle properly. On the other hand, conservatism reduction increases risk and therefore the probability of collision with other vehicles. The second point requires an algorithm able to process sensor data for environment mapping, guarantee safety, and plan an appropriate maneuver online.

1.1

Problem Statement

Scenarios regarding autonomous vehicles present a series of uncertainties. The lo-calization of vehicles on the road is affected by error due to several sources, such as sensor noise, data processing, environment. Then one cannot have a perfect knowl-edge of the other traffic participants’ future behavior. All that leads to a stochastic environment, i.e., processes (here in particular the participants’ behavior) that are not predictable. In control science literature this leads to having chance constraints for planned maneuvers, i.e., constraints that can be satisfied only with a certain prob-ability. Moreover, this kind of constraint cannot be directly solved by a computer. One goal of this thesis is transforming these chance constraints into deterministic ones in order that they can be easily solved by an computer. That is accomplished by first introducing an occupancy grid that identifies areas on the road as occupied o free with a certain probability. Then all the cells of the grid with an occupancy probability higher than a certain threshold are marked as not admissible, while all the others as admissible. Then Stochastic Model Predictive Control (SMPC) plans

a trajectory that passes along only those cells that are marked as admissible. Since neglecting low likely events can result in infeasibility, a second controller called Fail-safe Model Predictive Control (FMPC) is introduced. The FMPC, based on the results of the SMPC, plans a second trajectory by taking into account the worst case realization of other traffic participants’ maneuvers. This combination, on the base of few assumptions, leads to an efficient and feasible algorithm.

1.2

Related Work

The Occupancy Grid has seen many variants in the last years both in formulation and denomination. [TBF05] describes Occupancy Grid as a mapping grid of the environment where each grid cell represents the probability that a certain portion of the field is occupied. A first approach to Bayesian reasoning for autonomous driv-ing cars, but where no occupancy grid has been developed, has been proposed in [CFBM02], where data sensor fusion techniques are deployed for multi-target track-ing of other surroundtrack-ing vehicles. Then [CPL+_{06] and [TMY}+_{08] have proposed a}

Bayesian Occupancy Filter (BOF) for autonomous vehicles, i.e., an occupancy grid where each grid cell represents the probability to be occupied by another vehicle and its state is updated with a Bayesian rule. This is similar to [TBF05]. However, with respect to classical tracking methodology where the problem to associate data to dif-ferent objects is a hard task, the BOF handles data to a higher level of abstraction. Concepts as objects, pedestrians and vehicles do not exist and data association from multiple sensors is easier for efficient fusion in the grid. What is important is only the capability to assign to each cell an occupancy probability. The BOF framework has been developed in many different ways, as reviewed in [SCVCS+_{17], with}

focus-ing on autonomous vehicles for both highway and urban traffic scenarios. One of the key features that permitted such developments is the possibility to design a BOF to different levels of abstractions. Lower levels involve data treatment to determine the correspondence between sensor input and cell space. Higher levels involve cell clustering to face problems like obstacle avoidance and decision making. However, the research has been mainly developed in the lower levels of this framework than in the higher ones, i.e., poor research has been carried on for the planning section. In this thesis we want to exploit the general framework of the occupancy grid to formulate a planning algorithm and determine a good way of directly including the grid resulting from sensing into the planning part. We suppose to already have a grid representation of the environment, therefore, without considering explicit sen-sor measurements. Moreover, the state of the grid cells will not be updated with a Bayesian approach. We will make use of the terms Probabilistic Occupancy Grid to identify an occupancy grid where the occupancy state of the cells are described with probabilistic values, and Binary Occupancy Grid where instead a grid cell can be defined only as completely free or occupied. Both these terms are not mentioned

1.2. RELATED WORK 9

in the cited works.

Trajectory planning for autonomous vehicles has gained several results with the use of Model Predictive Control (MPC), an optimization-based method for the feedback control of (non)linear systems. Due to the uncertainties in other traffic participants’ behavior, [CGLB14] has presented a Stochastic MPC (SMPC), where the most likely maneuver of other vehicles is estimated by a Multiple Model Kalman Filter. This im-possibility to exactly predict other participant’ behavior leads to chance constraints, i.e., it is possible to guarantee a safe trajectory only in probabilistic terms up to certain levels. Moreover, chance constraints cannot be solved directly by a com-puter and appropriate techniques have to be employed. In order to consider several potential maneuvers of other vehicles, a Scenario-based MPC has been presented in [CSCB17], by sampling known distributions of other traffic participants. How-ever, this method still employs chance constraints since it leads to the possibility of missing some unexpected behavior. A combination of SMPC and Scenario-based MPC has been developed by [BOLW18]. The Scenario-based MPC is used to sample other vehicle maneuvers, while the SMPC is deployed to plan safe trajectories for a controlled vehicle. The use of two MPC frameworks implies to solve two opti-mization problems that leads to algorithms with high computation cost. Moreover, to avoid collision with other vehicles, ellipses are set around the other participants’ vehicles, leading to the implementation of nonlinear space constraints. This further increases the computational cost. A common feature of the cited work is that it is not possible to prove the property of recursive feasibility of the algorithm, i.e., that it is always possible to find a solution that leads the controlled vehicle in a feasible state. To overcome this problem, [Olb19] has proposed to integrate an SMPC with a Failsafe motion planning that takes into account the worst case scenario of other traffic participants. This framework is safe but also very conservative in terms of motion planning. Moreover, the formulation of nonlinear constraints with the ne-cessity of solving two optimization problems leads to an algorithm with expensive computational cost.

Based on the above works, we are going to integrate an Occupancy Grid with a double MPC framework, i.e., a Stochastic MPC and a Failsafe MPC. The grid will be used to define which spaces of the road are occupied by other traffic participants, and to transform the formulated chance constraints into deterministic ones. Then SMPC is used to plan trajectories for the controlled vehicles by considering only those cells with a high probability of being occupied. To guarantee the property of recursive feasibility of the algorithm, we will employ also an FMPC that considers the worst case scenario. Finally we will show how the exploitation of the occupancy grid framework leads to a fast solving of the two optimal control problems and how the combination of the two MPCs leads to an efficient and safe behavior for the controlled vehicle.

11

Chapter 2

Modeling

This section presents first the mathematical models utilized for the vehicles, then briefly the Stochastic Model Predictive Control (SMPC) and the Failsafe Model Predictive Control (FMPC). We call ego vehicle (EV) the vehicle we control and target vehicles (TVs) all the other vehicles in the scenario.

2.1

Ego vehicle models

We employed first a linear point-mass model that treats the vehicle as a particle, then a kinematic bicycle model that takes into account the real geometry of the vehicle.

2.1.1

Linear model

Given state vector ξEV

k ∈ Rn and control vector uEVk ∈ Rm, the linear, point-mass,

discrete model for the EV is

ξ_k+1EV = AξEV_k + BuEV_k , (2.1) with ξEV_k =     xEV k vEV_x,k yEV k vEV_y,k     , uEV_k =u EV x,k uEV y,k  , (2.2)

where xEV_k , v_x,kEV, uEV_x,k represent the longitudinal position, velocity, and acceleration, while yEV

k , vy,kEV, uEVy,k are the lateral position, velocity, and acceleration.

A =     1 ∆t 0 0 0 1 0 0 0 0 1 ∆t 0 0 0 1     , B =     0.5(∆t)2 ₀ ∆t 0 0 0.5(∆t)2 0 ∆t     , (2.3)

where A ∈ Rn×n_{, B ∈ R}n×m _{and ∆t is the sampling time.}

The admissible state space and control space are denoted respectively Ξk and Uk.

The system is completely controllable since the controllability matrix

R = [B AB A−1B . . . An−1B] (2.4) always has rank(R) = n if ∆t 6= 0.

2.1.2

Kinematic bicycle model

The kinematic model is based on the following assumptions:

Assumption 1. The slip angle at each wheel is zero, i.e., the velocity vector at the wheel is aligned with the wheel.

Assumption 2. Only the front wheels can be steered, and the steering angles at the front left and right wheels are equal.

Given state vector ξEV _{∈ R}nand control vector uEV_{∈ R}m, the non-linear kinematic model for the EV is

˙x = v cos(ψ + α), (2.5a) ˙ y = v sin(ψ + α), (2.5b) ˙ ψ = v lr sin(α), (2.5c) ˙v = a, (2.5d) α = arctan  lr lr+ lf tan(δf)  , (2.5e) with ξEV =     x v y ψ     , uEV =δf a  . (2.6)

where x and y represent the longitudinal and lateral position of the Center of Gravity (CoG) of the vehicle, v and a denote the velocity and the acceleration, α denotes

2.1. EGO VEHICLE MODELS 13

the angle between the velocity vector and the longitudinal axis, called the body slip angle, δf is the steering angle of the front wheels, lf and lr are the distances from the

vehicle’s CoG to the front and rear axles, respectively. Model (2.5) is discretized with the first-order Euler method and represented in Fig. 2.1.

Figure 2.1: Graphic representation of the kinematic bicycle model. Figure taken in [Car16].

2.1.3

Road-aligned coordinate frame

The position and the orientation of the vehicle can be expressed in a curvilinear coordinate frame aligned to the road called the Frenet frame. The Frenet frame as-serts invariant tracking performance under the action of the special Euclidean group SE(2) := SO(2) × R2_{. We apply this method in order to deal with curved highway}

scenarios. The moving reference frame is given by the tangential and normal vector at a certain point of the curve referred to as the lane centerline, that is the reference lane for our EV.

Given state vector ξEV _{∈ R}n _{and control vector u}EV _{∈ R}m_{, the non-linear,}

road-aligned, kinematic model for the EV is

˙s = v cos(φ + α) 1 − Kr_(s)d , (2.7a) ˙ d = v sin(φ + α), (2.7b) ˙ φ = v lr sin(α) − Kr(s) ˙s, (2.7c) ˙v = a, (2.7d) α = arctan  lr lr+ lf tan(δf)  , (2.7e)

with ξEV =     s v d ψ     , uEV =δf a  , (2.8)

where the coordinates [x, y, ψ]> have been replaced with [s, d, φ]>. The state vari-able s represents the longitudinal position of the vehicle along the road, and d and φ are the lateral position error and the angular error with respect to the lane cen-terline. The variable Kr_{(s) denotes the curvature of the road that depends on the}

longitudinal position s. Model (2.7) is discretized with the first-order Euler method and represented in Fig. 2.2.

Figure 2.2: Graphic representation of the kinematic model in road-aligned frame. Figure taken in [Car16].

2.1.4

Ego vehicle constraints

The control signal has lower and upper bounds

umin ≤ uEVk ≤ umax. (2.9)

The state constraints are defined as linear inequalities Asafe_k,ξ ξ_kEV≤ bsafe

k,ξ, (2.10)

where Asafe_{k,ξ ∈ R}p×n, bsafe_{k,ξ ∈ R}p and p is the number of state constraints. The sub-script ξ indicates that the matrices refer to the state of the EV vehicle.

2.2. TARGET VEHICLE MODEL 15

2.2

Target vehicle model

Given state vector ξ_kTV _{∈ R}n and control vector uTV_{k ∈ R}m, the linear, point-mass, discrete model for the TV is

ξTV_k+1 = AξTV_k + BuTV_k + GwTV_k , (2.11) where A and B are the same as in (2.3), wTV_k ∼ N 0, ΣTV

w
is a random Gaussian

variable and G = diag(g1, g2, g3, g4) a weighting matrix.

Model (2.11) is known to the EV and used to make future predictions of the TV state. The random noise wTV_k accounts for uncertainty in future predictions. The control input uTV_k is a stabilizing state-feedback controller

uTV_k = K ξ_kTV− ξTV k,ref
, (2.12) K = 0 k12 0 0 0 0 k21 k32  (2.13) where K is obtained by the solution of a Linear Quadratic Control (LQC) problem. Both EV and TV are simulated as rectangles of length vl and width vw in Sec. 4.

2.3

Controllers

MPC is an optimization-based method for the feedback control of (non)linear sys-tems. The main characteristic of MPC is to use a model of the system to control in order to predict and optimize future behavior over a prediction horizon. The following two subsections introduce two different kinds of MPC we employ.

2.3.1

Stochastic MPC

Even if the system model is supposed to be known perfectly, a SMPC formulation is required due to the unbounded uncertainty in other traffic participants. Taking into account all the potential maneuvers of the other vehicles in the scenario leads to a conservative and inefficient trajectory planning. The SMPC will consider only the a finite set of possible maneuvers to generate an aggressive behavior for the EV. The trajectory along the prediction horizon for the EV is obtained by iteratively solving the following optimal control problem,

min U NSMPC−1 X h=0  ∆ξ_hEV 2 QSMPC + uEV_h 2 RSMPC  + ∆ξ_NEV SMPC 2 SSMPC (2.14a) s.t. ξ_h+1EV = f ξ_hEV, uEV_h
, _{h ∈ N,} (2.14b) ξ_h+1TV = AξTV_h + BuTV_h + GwTV_h , _{h ∈ N,} (2.14c) uTV_h = K ξ_hTV− ξTV h,ref
, h ∈ N, (2.14d) uEV_h ∈ Uh, h = 0, . . . , NSMPC− 1, (2.14e) ξ_hEV ∈ Ξh, h = 1, . . . , NSMPC, (2.14f) Asafe_h ξ_hEV ≤ bsafe_h , h = 1, . . . , NSMPC, (2.14g)

with the EV input U = (u0, . . . , uNSMPC−1) >

, kzk2_Z = z>Zz and ∆ξEV

h = ξEVh −ξh,refEV

with maneuver-dependent EV reference, weighting matrices QSMPC, SSMPC ∈ R4×4

and RSMPC ∈ R2×2, f ξEVh , u EV

h
one of the EV models presented in Sec. 2.1, and

the input and state constraints Uh and Ξh.

The stochasticity of the problem is due to the nature of Asafe

h and bsafeh , which depend

on the estimated position of the TV that is not perfectly known to the controller.

2.3.2

Failsafe MPC

To ensure the feasibility of the algorithm, a second controller is introduced that we call FMPC. This controller takes into account the worst-case maneuvers of other traffic participants. The solution of this optimal control problem is formalized as

min U NFMPC−1 X h=0  ∆ξEV_h 2 QFMPC+ uEV_h 2 RFMPC  + ∆ξEV_N 2 SFMPC (2.15a) s.t. ξEV_h+1= f ξEV_h , uEV_h
, h ∈ N, (2.15b) ξTV_h+1= Aξ_hTV+ BuTV_h + GwTV_h , _{h ∈ N,} (2.15c) uTV_h = K ξ_hTV− ξ_h,refTV
, _{h ∈ N,} (2.15d) uEV_h ∈ Uh, h = 0, . . . , NFMPC− 1, (2.15e) ξEV_h ∈ Ξh, h = 1, . . . , NFMPC, (2.15f) ξEV_N FMPC ∈ Ξf, (2.15g) Asafe_h ξ_hEV≤ bsafe h , h = 1, . . . , NFMPC, (2.15h)

with the EV input U = (u0, . . . , uNFMPC−1) >

, kzk2_Z = z>Zz and ∆ξEV

h = ξEVh −ξh,refEV

with maneuver-dependent EV reference, weighting matrices QFMPC, SFMPC ∈ R4×4

and RFMPC ∈ R2×2, f ξEVh , uEVh
one of the EV models presented in Sec. 2.1, and

2.3. CONTROLLERS 17

is discussed in Sec. 3.3.

How the two MPCs are integrated in order to guarantee a feasible algorithm will be explained in Sec. 3.6.

19

Chapter 3

Method

This chapter is devoted to the main core of this thesis. Taking into account each potential event in a highway scenario is infeasible and therefore uncertainty is han-dled generally with a probabilistic approach. This leads to the formulation of chance constraints that have to be satisfied with a certain probability. Moreover, chance constraints cannot be solved directly by a computer. We want to tackle this prob-lem by transforming these chance constraints into deterministic ones by taking into account only those events with high likelihood. This procedure neglects events with low likelihood making impossible to guarantee the recursive feasibility property of the algorithm. To provide this problem, we do not consider only a set of potential maneuvers of other traffic participants but also the worst-case scenarios realization. In the following we present first how the highway is represented by a grid, i.e., an evenly spaced field of cells. For the SMPC planning, we present how to assign an occupancy probability to the cells, formulate the chance constraints, and transform them into deterministic ones. Later we show how to formulate the state constraints for the FMPC planning that, since we consider the worst-case scenario of other traffic participants, are deterministic by definition. Once we have identified admissible and not-admissible cells, we explain how to compute the convex hulls in which the EV can operate. Finally, the combination of SMPC and FMPC is presented.

3.1

Occupancy Grid

An OG G represents a map of the environment as an evenly spaced field of cells ci,j ∈ G with ci,j = (ci, cj), each representing the presence of an obstacle at that

location in the environment. In this work the environment is the highway and the obstacles are the TVs.

The goal of the OG is to define which locations of the highway are potentially oc-cupied by TVs and define the admissible space in which the EV can operate. The simulated highway has dimensions [rl, rw], accounting respectively for length and

G ∈ Rq×r_{, with integer ratios q = r}

l/cx and r = rw/cy. Each cell ci,j holds its

position inside the grid in terms of indexes. The cell size expresses a precision mea-surement: the bigger the cell, the less accurate the vehicle localization on the grid and vice versa.

Figure 3.1: Representation of the highway grid.

The localization of a vehicle on the grid is an approximated calculus since each cell corresponds to an area a = cxcy. To localize the center of a vehicle on the grid, we

use the following formulas,

ˆ

ci = bxkc + b(xk− bxkc)/cxccx, (3.1a)

ˆ

cj = bykc + b(yk− bykc)/cyccy, (3.1b)

where [ˆci, ˆcj] represents the cell to which the vehicle belongs. The operator b·c

represents the floor operation. The unit of this result is in meter [m]. To convert them as indexes one has to apply the following equations,

ci = ˆci/cx, (3.2a)

cj = ˆcj/cy. (3.2b)

3.2

Occupancy Grid for SMPC planning

Once a TV has been localized on the grid using Eq. (3.1) and (3.2), one can proceed building two grids. The first is called Probabilistic Occupancy Grid (POG) where we assign probabilistic occupancy values to the cells. Then we transform the POG into a Binary Occupancy Grid (BOG), where the cells are identified as free and occupied. In the following we explain the characteristics of the two occupancy grids.

3.2. OCCUPANCY GRID FOR SMPC PLANNING 21

3.2.1

Probabilistic Occupancy Grid

The POG is represented by a matrix P ∈ Rq×r _{where the element p}

i,j ∈ [0, 1]

indi-cates the probability that the cell ci,j is occupied by a TV.

In order to assign a probability value to each cell, one has to choose a probability density function (pdf) for the TV. We assume the potential area occupied by a TV described by a 2D Gaussian probability density function. The main reasons for this choice are that the Gaussian pdf is symmetric about the mean and uncertainty no-tions can be easily inserted into its variance element.

Given time step k and prediction step h = 1, . . . , NSMPC, the CoG of a TV is

identified by the cell cTV

k,h, and the probability density function is given by

fcTV(c) = exp −1₂(c − cTV k,h)TΣk,h−1(c − cTVk,h)
p(2π)2_|Σ k,h| , (3.3)

and stored in the matrix P. This indicates that for each time step k and for each prediction step h, a POG is computed.

Equation (3.3) is the 2D generalization of a Gaussian distribution, where c indicates a cell of the grid, cTV

k,h is the estimated TV position when h = 1 and the h-th

pre-dicted position for h = 2, . . . , NSMPC by using Eq. (2.11), with NSMPC as prediction

horizon length of the SMPC, the covariance matrix Σk,h ∈ R2×2 represents how

much uncertainty is the estimation, and |Σ| ≡ det Σ.

Even if the model of each TV is a discrete, linear, point-mass system, it is necessary to take into account their dimensions [vl, vw] in order to have a realistic

represen-tation of the vehicle on the grid. To achieve this, we first compute (3.3), whose result is displayed in Fig. 3.2(a). The black point represents cTV

k,h, i.e., the CoG of

the TV, the green dashed line represents the normal distribution on the xz-plane passing through cTV_k,h, while the red dashed line the normal distribution on the yz-plane. Then, we start to expand the maximum value computed with (3.3) along both dimensions x and y in order to cover the vehicle area, as shown in Fig. 3.2(b). The final result is displayed in Fig. 3.2(c), where the distance between the two green dashed lines equals the width of the vehicle, and the distance between the red dashed lines equals the length.

The elements of Σk,h in (3.3) are handled such that they increase along the horizon

prediction NSMPC since the more in the future the prediction, the less accurate the

state estimation. The uncertainty propagation is based on the technique described in [CGLB14], and elaborated as in [Olb19]. The TV future state at time step k and prediction step h is

ξ_k,h+1TV = Aξ_k,hTV+ BK ξ_k,hTV− ξTV

k,ref
+ Gw TV

(a) First step.

(b) Intermediate step.

(c) Final step.

Figure 3.2: Illustration of the steps for building the TV distribution. The black point represents cTV_k,h.

3.2. OCCUPANCY GRID FOR SMPC PLANNING 23

while the true predicted state is without noise, i.e., ˆ

ξ_k,h+1TV = A ˆξ_k,hTV+ BK ˆξTV_k,h − ξTV k,ref



. (3.5)

Let us now define the prediction error at future time step h along the prediction horizon NSMPC as

ek,h = ˆξk,hTV− ξ TV

k,h. (3.6)

By substituting (3.4) and (3.5) in (3.6) we get

ek,h+1 = (A + BK) ek,h− Gwk,hTV (3.7)

Let ek,h ∼ N (0, Σk,h), given the known distribution of wTVk . Then, with initial

condition Σk,0 = 0, we have ek,h+1 ∼ N (0, Σk,h+1), where

Σk,h+1 = (A + BK) Σk,h(A + BK) >

+ GΣwG>. (3.8)

Figure 3.3 shows how the TV distribution changes over the prediction horizon NSMPC

in according to (3.3) and (3.8).

Figure 3.3: The variance of the TV distribution increases along the prediction hori-zon NSMPC.

One has to note that theoretically it is possible to choose any pdf to assign occu-pancy probability values to the cells since the grid framework does not depend on the type of distribution.

3.2.2

Extension to multiple TVs

It is reasonable to think about a highway scenario with multiple TVs. Assuming that the position of each TV does not depend on the other TVs, one can compute a POG for each of them and sum them up. More formally,

Pk,h= Nv

X

nv=1

Pk,h,nv, ∀nv= 1, . . . , Nv, (3.9)

where Nv is the number of TVs localized in the range detection of the EV on the

grid. An example is illustrated in Fig. 3.4.

Figure 3.4: Representation of the POG at a prediction step. The EV is on the left in blue and the two TVs are on the right.

3.2.3

Extension to multiple maneuvers

In a highway scenario a TV can execute different kinds of maneuvers and which one is going to be executed is not known to the EV. The SMPC framework considers only a set of possible maneuvers of other traffic participants by computing a POG for each maneuver and weighting it by its probability. More formally,

Pk,h = Nv X nv=1 Nm X nm=1 εnv,nmPk,h,nv,nm, ∀nv = 1, . . . , Nv, (3.10) ∀nm= 1, . . . , Nm, s. t. Nm X nm=1 pnv,nm = 1,

3.2. OCCUPANCY GRID FOR SMPC PLANNING 25

where Nv and Nm are respectively the number of TVs localized, in the range

detec-tion of the EV, on the grid and the number of considered maneuvers, while εnv,nm

is the probability of the nm-th maneuver of the nv-th TV.

3.2.4

Binary Occupancy Grid

Every time a POG is calculated, it is transformed into a Binary Occupancy Grid (BOG), i.e., a matrix B ∈ Rq×r _{where the element b}

i,j ∈ B can assume only two

values, i.e., bi,j ∈ {0, 1}. The value 0 indicates an admissible cell, while 1 indicates

an inadmissible cell. This passage occurs choosing a probability threshold pth,

bi,j =      1, if pi,j ≥ pth, ∀i = 1, . . . , q, ∀j = 1, . . . , r, 0, otherwise. (3.11)

The variable pthis a tuning parameter for the trade-off conservativeness against risk.

If it is too low, an overestimation of the area occupied by a TV can occur, while, if too high, actual occupied areas appear admissible to the algorithm, leading to potential collisions between EV and TVs. The set of occupied cells is denoted as Ξocc

k,h, while the set of admissible cells as Ξsafek,h, for each time step k and prediction

step h.

What has been just described is the passage that enables to transform chance con-straints into deterministic ones. Given a vector x ∈ Rn_{, chance constraints are}

described in the form

Pr (g(x) ≤ 0) ≥ β, (3.12) where g(x) : Rp×n _{→ R}p _{is the set of constraints, p is the number of constraints}

and, β is the probability with which those constraints have to be satisfied. This type of constraints cannot be directly solved by a computer and therefore particular methods have to be employed. However, by transforming the POG into a BOG, we end up with the following condition,

˜

g(x) ≤ 0, (3.13)

that is a set of deterministic constraints, where ˜g(x) is the obtained approximation of g(x) in (3.12) One can argue that these still resemble chance constraints obtained by ignoring all the grid cells with a low occupancy probability. This is true but (3.13) can be directly handled by a computer without the necessity of using further methods.

3.2.5

Other chance constraint approximations

Above we presented a method to transform chance constraints into deterministic ones. There are other methods in the SMPC literature, such as the sample-based MPC and the scenario MPC.

As describe in [Mes16], the main idea in sample-based MPC is to represent the stochastic system using a finite set of random realizations of uncertainties, that are used to solve the OCP. These samples are used to approximate the chance con-straints as deterministic ones, and the approximation becomes exact as the number of samples approaches infinity. Due to the large amount of required samples, the method can result computationally expensive.

An other approach is scenario MPC. As explained in [CGP09] and in [Mes16], the scenario method reformulates the stochastic OCP as a robust one with an infi-nite number of deterministic constraints due to the unbounded uncertainty in the stochastic problem. Then, the OCP is iteratively solved in MPC terms by sampling a finite number of “scenarios” in the infinite set of deterministic constraints. By doing so, the probability with which the original chance constraints are satisfied grows with the number of scenarios. The complexity of the algorithm depends on the number of scenarios utilized to solve the OCP.

These two methods, i.e., sample-based and scenario, as well our grid-based method, present a way to transform chance constraints into deterministic ones. However, there is a main difference with respect our method. Sample-based and scenario methods still try to solve an optimization problem where the constraints are satis-fied with a certain level of probability. In this work instead, due to the introduction of the BOG, we do not have this problem and can resolve the OCP with classical methods. Furthermore, the probability threshold pth is not linked to the constraints

formulation, but to the probability density function adopted for the TV.

However, even in our grid-based SMPC we cannot assure complete safety since we neglect all the grid cells with a probability to be occupied lower than the threshold pth. Therefore, the next section presents a second controller, called Failsafe Model

Predictive Control (FMPC), to consider the worst-case scenario of other traffic par-ticipants, and guaranteeing safety of the planned trajectory.

3.3

Occupancy Grid for FMPC planning

The following description has been first formulated in [Olb19] and based on [MA16]. The worst-case scenario is formulated by considering the set of all possible TV states at future time steps h along the prediction horizon NFMPC, that we denote

as RTV_k,h. Let X h, ξ_kTV, U (·)
be the solution of the TV dynamics, described by (2.11), starting at the initial state ξTV

k and applying the input sequence U (·). The

3.3. OCCUPANCY GRID FOR FMPC PLANNING 27

RTV

k,h=X h, ξkTV, U (·)
|h = 1, . . . , NFMPC : U (h) ∈ U , ξk+1TV = f ξkTV, uTVk
. (3.14)

The set of possible TV initial states is denoted with ΞTV

k,1 and corresponding to

ΞTV_k,1 = { ˆξ_kTV− w_kTV|wTV_k ∼ N (0, ΣTV_w )}. (3.15) Due to the point-mass model (2.11), the occupancy set Ξocc

k,his obtained by extending

the reachable RTV

k,h set by half of the vehicle dimension in each direction, as shown

in Fig. 3.5.

Figure 3.5: Set of TV initial states. The dark grey box represents the actual TV position, while the light grey boxes represent the set of TV states. Black crosses show the CoG of each vehicle initial state.

Since we use a linear model for the TV, we can decouple the longitudinal and the lateral motion. Therefore, we have four worst initial cases:

ξ_kTV,1 = ˆξTV

k + [ wxmax, wmaxvx , wymax, w vy max]>,

ξ_kTV,2 = ˆξTV

k + [ − wxmax, − wmaxvx , wymax, w vy max]>, ξ_kTV,3 = ˆξTV_k + [ wx_max, wvx max, − wymax, − w vy max]>, ξ_kTV,4 = ˆξTV

k + [ − wxmax, − wmaxvx , − wymax, − w vy max]>,

where wmax = [wxmax, wmaxvx , wmaxy , w vy

max]> is the worst-case realization of the

mea-surement error whose elements are all greater than zero. Then we apply for each future time steps h of the prediction horizon Umax to ξTV,1k and ξ

TV,3

k , while Umin to

ξTV,2_k and ξTV,4_k . The result is showed in Fig. 3.6.

Figure 3.6: Future projections of TV initial states along the prediction horizon NFMPC. The dark grey box represents the actual TV position, while the light grey

boxes represent the set of TV states. Black crosses show the CoG of each vehicle predicted state.

Since Ξocc_k,h represents the worst-case scenario for TV future predictions, we use this information to directly update the cells of the Binary Occupancy Grid described in Sec. 3.2.4. bi,j =      1, if ci,j ∈ Ξocck,h, ∀i = 1, . . . , rl/cx, ∀j = 1, . . . , rw/cy, 0, otherwise. (3.17) As well as we did for the POG in Sec. 3.2.2, we can extend the computed BOG for the FMPC planning for considering scenarios with multiple TVs.

Bk,h = Nv

X

nv=1

Bk,h,nv, ∀nv= 1, . . . , Nv, (3.18)

where Nv is the number of TVs localized in the range detection of the EV on the

3.4. COMPUTING EV STATE CONSTRAINTS 29

Safe invariant set computation

Similar to [Olb19], we want to find a control invariant safe set for the EV for the last step of the prediction horizon NFMPC. This set ensures that a feasible, safe

solution exists from the last predicted state and guarantees the generation of a control sequence that avoids a collision with a TV. The imposed constraints for this set are

1. a steering angle δEV

f,NFMPC = 0 for nonlinear models 2.5 and 2.7 (or a lateral

velocity vEV

y,NFMPC = 0 for the linear model 2.1),

2. a longitudinal velocity v_x,NEV_FMPC ≤ vTV* x .

The first condition is necessary in order that, during a breaking maneuver, the EV does not endanger vehicles in an adjacent lane. The second condition requires that the EV is able to reach a reference velocity equal or lower than the velocity vTV*

x of

the nearest TV in front of it that lies in the same lane.

3.4

Computing EV state constraints

For any of the two cases in which the BOG is computed, i.e., in the SMPC and in the FMPC planning, we have a grid representing admissible and inadmissible space. Now there is the need to find a convex subset of Ξsafe_k,h in which the EV can operate for each time step h of the horizon predictions NSMPC and NFMPC. A convex hull is

required for the solution of the optimal control problems (2.14) and (2.15).

In the following, we first introduce the Bresenham’s line algorithm and then the algorithm that finds the convex admissible space.

3.4.1

Bresenham’s line algorithm

The Bresenham’s line algorithm is a line drawing algorithm that determines the points of an n-dimensional grid that should be selected in order to form a close approximation to a straight line between two points.

In Fig. 3.7 the gray cells represent the ones selected by the algorithm given the black straight line.

Since we want to find a convex hull inside the admissible space, we need to guarantee that for two cells inside the hull it is possible to draw a path made of free cells. Here the Bresenham’s line algorithm plays its role. In the next section its use is explained in details.

Figure 3.7: Illustration of the result of Bresenham’s line algorithm. 1

3.4.2

Determine admissible convex hull

In this section we explain the algorithm that builds the convex hull, given the matrix B. We will show figures to have a visual representation of the algorithm while explaining it step by step.

1. The CoG of the EV is localized on the grid using (3.1). The corresponding cell is denoted as cEV _{and highlighted in yellow in Fig. 3.8. To consider the}

EV dimensions, its rear corners cr,1 and cr,2 are selected. Then, all admissible

cells in the detection range of the EV are selected and stored into a matrix Crange (cyan column in Fig. 3.8).

Figure 3.8: The yellow cell represent the EV center, brown cells show the rear corners of the EV, cyan cells (Crange) indicate the cells in the EV detection range. Grey cells

represent inadmissible cells according to B.

2. For each cell c of Crange, the Bresenham’s line algorithm evaluates if cells are

3.4. COMPUTING EV STATE CONSTRAINTS 31

cell c is added to a matrix Cfree and marked with a green color, otherwise with

red.

Figure 3.9: The pathways between cell c and the rear corners cr,1 and cr,2 are

indicated by the orange beams.

3. We then select the edges of the green column, made of cells belonging to Cfree,

and mark them as e1 and e2. Then, the rear corners of the EV are marked as

m1 and m2.

Figure 3.10: The orange cells e1 and e2 represent the edges of Cfree.

4. To obtain a larger convex hull, we first start moving the rear corner m1 outside

the vehicle bounds, proceeding towards the cells with same abscissa but lower ordinate. If pathways are verified to e1 and e2, the new cell is marked as m1.

The process keeps continuing until at least one of the pathways to e1 and e2

is not anymore verified, or if we reached the road boundary.

Figure 3.11: The last cell m1 is marked as vivid orange, while the one at preceding

step with a light orange. Dashed-lines represent the pathways from e1 and e2 at the

preceding step.

5. We now repeat the earlier step by moving m2 towards the cells with same

abscissa but higher ordinate, and checking if the pathways to e1 and e2 are

possible. The process keeps continuing until at least one of the pathways to e1 and e2 is not anymore verified, or if we reached the road boundary.

Figure 3.12: The last cell m2 is marked as vivid orange, while the one at preceding

step with a light orange. Dashed-lines represent the pathways from e1 and e2 at the

3.5. CURVED GRID 33

6. Finally, we mark the vertices of the polygon as h1, h2, h3, and h4, that, by

construction, is a convex hull.

Figure 3.13: The convex hull covers the light-blue area, the vertices are marked with orange cells.

We obtain therefore a vertex representation of the polygon. This is transformed into a half-space representation that can be written as a matrix inequality, already shown in Eq. (2.10).

Ak,ξξkEV ≤ bk,ξ, (2.10 revisited)

obtaining therefore the state constraints of the EV.

3.5

Curved grid

While the formulation of the grid results trivial for a straight road, it is not so ob-vious for a curved one. Here, we briefly want to show the researched approaches. The first attempt is displayed in Fig. 3.14. Here we keep the grid cells oriented with respect to the axes X and Y while curving the road. Despite the immediacy of the method, some cells along the curve do not lay completely inside the road. Such cells could be set as fully occupied in the BOG to avoid to drive the EV outside the road. However, in Sec. 3.1, we conceptualize the grid as a matrix, where moving along the columns means moving along the X dimension of the road. This approach for the curved road would indicate that, when there is a curve, the matrix representing the grid started to increase the number of rows, instead of columns, while the vehicle

proceeds. This results inconsistent with our grid formulation and, therefore, such method has been left.

Figure 3.14: First curved grid approach.

In the second approach we bend the grid cells along the curve of the road, as shown in Fig. 3.15. While avoiding the problems of the preceding method, here we do not any more have cells of the same size. As we can see in Fig. 3.15, the grid cells get bigger as we move from the internal to the external side of the curve. That result is inconsistent with the presented method and, therefore, even this approach has been discarded.

Finally, we decided to tackle this problem from a different point of view. Instead of trying to adapt the grid to the curved road, we instead insert the curved road inside the EV model (2.7). In a straight highway model (2.5) is used but, when a predicted EV state or the actual state lies along a curve, model (2.7) is adopted. It is straightforward to verify that model (2.7) equals model (2.5) when K = 0. The main advantage of this approach is that we do not need to bend the grid cells in order to follow the curvature of the road. However, we loose a realistic visualization of the environment, and relying only on the EV model (2.7) to simulate a curved highway.

3.6. COMBINING STOCHASTIC AND FAILSAFE MPC 35

Figure 3.15: Second curved grid approach.

3.6

Combining Stochastic and Failsafe MPC

In this section we explain how the combination of SMPC and FMPC leads to an efficient and safe algorithm. The SMPC framework yields an efficient and aggres-sive behavior for the EV (depending on the probability threshold introduced in Sec. 3.2.4) taking into account the most likely maneuvers of other traffic participants. However, it may become infeasible and potentially cause a collision if a low chance maneuver actually occurs. Therefore, we accompany the SMPC with an FMPC. The latter considers the worst-case scenario of other traffic participants and leads to a motion planning that is completely safe but inefficient since it is very conservative. The algorithm outline is shown in Fig. 3.16. Each time step of the algorithm in-volves the solution of two optimal control problems. The first one regards the SMPC for which the trajectory with corresponding input sequence is USMPC is generated

along the prediction horizon NSMPC. If a feasible solution is computed, then one

prediction step for the EV is calculated by applying the first element of the control sequence USMPC. Starting from this predicted state ˆξTVk we try to solve the second

optimal control problem, the one regarding the FMPC. If this one has a feasible solution it means that the SMPC has generated a control signal that brings the EV in a safe area. This result is possible since the FMPC considers the worst-case of other traffic participants’ maneuvers. Therefore, we can actually apply USMPC(1)

to the EV. If instead the FMPC gives an infeasible solution, then the SMPC would bring the EV in an area in which we cannot completely guarantee safety. In this

case we apply the preceding failsafe control signal UFMPC,k−1 that brings the EV in

a safe area. This occurs even in the case the SMPC reports an infeasible solution. This approach is valid only if the following assumption is satisfied.

Assumption 3. The initial EV state ξEV₀ is safe and there exist a known input sequence UFMPC that guarantees the existence of a safe invariant set.

3.6.1

Recursive Feasibility

Thanks to the introduction of the FMPC, that takes into account the worst case scenario of other traffic participants, it is possible to demonstrate the property of recursive feasibility of the algorithm, as well as shown in [Olb19].

Definition 1. Let XU

k denote a trajectory starting at initial state ξkEV applying the

input sequence U . Then, we define the set of safe feasible trajectories, leading into a control invariant set Ξf, as

Γk :=XkU | ∃N : X U

k (j) ∈ Ξ safe

, j = 0, . . . , N, X_kU(N ) ∈ Ξf . (3.19)

Then, we introduce the following assumptions.

Assumption 4. The system model of the EV corresponds to the exact dynamics of the real system.

Assumption 5. The system model of the TV corresponds to an over-approximation of its real dynamics.

Assumption 6. At initial time step k = 0 there exists a known input sequence UFMPC so that X0UFMPC ∈ Γ0.

The following theorem that we show has been elaborated in [Olb19].

Theorem 1. Let Assumptions 4-6 be satisfied, then at all the times there exists a feasible trajectory that is guaranteed to be safe.

Proof. We prove the theorem by induction.

n = 0: Let f ξEV, uEV
be the EV dynamics (2.2), (2.5) or (2.7). If a control sequence USMPC,0can be found at initial state ξ0EV, and a failsafe trajectory UFMPC,1

starting at ξEV

1 = f ξEV0 , uEVSMPC,0(1)
resulting in Ξf, then we apply USMPC,0(1) and

therefore we have XUSMPC,0

1 ∈ Γ1. If the FMPC problem is infeasible, it is possible to

apply UFSMP,0(1), since due to Ass. 6 Γ0 6= ∅ , obtaining X

UFMPC,0

1 ∈ Γ1. Therefore

3.6. COMBINING STOCHASTIC AND FAILSAFE MPC 37 ξEV k , ξTVk TV predictions ξTV k,h, h = 1, . . . , NSMPC

Compute Occupancy Grid

Compute SMPC trajectory ˆ ξEV k,h, h = 1, . . . , NSMPC Is SMPC feasible? Apply previous FMPC control input UFMPC,k−1(1)

and shift control se-quence UFMPC,k−1 Prediction ˆ ξEV k+1 = f ˆξkEV, USMPC,k(1)  TV predictions ξTV_k+1,h, h = 1, . . . , NFMPC Yes No

Compute Occupancy Grid

Compute FMPC trajectory ˆ

ξEV_k+1,h, h = 1, . . . , NFMPC

Is FMPC feasible?

Apply actual SMPC con-trol input USMPC,k(1) and save UFMPC,k Yes No SMPC FMPC

Γ0 6= ∅ ⇒ Γ1 6= ∅. (3.20)

n = k: This second part is similar to the first one. We again assume that at initial state ξ_kEV to have a control sequence UFMPC,k that leads to X

UFMPC,k

k ∈ Γk. If a

control sequence USMPC,k can be found, and a failsafe trajectory UFMPC,k+1 starting

at ξEV

k+1 = f ξkEV, uEVSMPC,k(1)
resulting in Ξf, then we apply USMPC,k(1) and

there-fore we have XUSMPC,k

k+1 ∈ Γk+1. Otherwise we have to apply UFMPC,k that leads to

XUFMPC,k

k+1 ∈ Γk+1. It follows that,

Γk6= ∅ ⇒ Γk+1 6= ∅, (3.21)

39

Chapter 4

Evaluation

The algorithm has been tested for different kinds of scenarios. Employing the three EV models, by considering straight and curved roads, and the presence of several TVs. The set of potential TV maneuvers contains a Lane Keeping (LK) maneuver, where the TV maintains its lane, and a Lane Changing (LC) maneuver, where in-stead a TV changes lane.

We first show and analyze one example for each EV model, then we discuss the tuning of the parameter pth introduced in Sec. 3.2.4 and its effects on the

algo-rithm. Finally, we present a computational cost analysis of the presented work, by considering both linear and nonlinear models for the EV. All simulation parameters are presented in the App. A.

4.1

Simulations

This section shows examples for each EV model, i.e., the linear model (2.1) and the two nonlinear models (2.5) for straight roads and (2.7) for curved roads. Since the original contribute of this thesis regards the grid-based SMPC, we simulate each scenario two times, one with only the SMPC, and one with the combination of SMPC and FMPC, which has been presented in [Olb19], and we adapt to the grid. This is done in order to evaluate the difference of the planned trajectories with the two methods.

In each experiment the EV has a velocity reference of 30 m/s. The TV has a constant velocity of 27 m/s. Since this work is mainly concerned with motion-planning, we implemented two simple policies to decide on a reference lane for the EV: 1) if the actual lane is occupied 20 m in front of it, the EV moves to the nearest free lane, otherwise it keeps its lane; 2) when it passes a TV and the longitudinal distance between their CoGs is larger than 15 m, the EV will overtake the TV by positioning itself in front of the TV on the TV lane. For all experiments we set pth = 15.0.

4.1.1

Linear model

In this first experiment we employ the EV linear model (2.1). The scenario is a two-lane highway with one TV which performs a LK maneuver with probability ε = 1.0. The initial conditions are shown in Tab. 4.1.

x [m] vx [m/s] y [m] vy [m/s] lref [m]

ξEV _{10.0 26.0 1.75 0.0 1.75}

(a) EV initial conditions: longitudinal postion and velocity, lateral position and velocity, and reference lane.

x [m] vx [m/s] y [m] vy [m/s] εLK εLC

ξTV _{40.0 27.0 1.75 0.0 1.0 0.0}

(b) TV initial conditions: longitudinal position and velocity, later position and velocity, and LK and LC maneuver probability.

Table 4.1: This table summarizes the initial conditions of the scenario. Figure 4.1 shows some frames of the two simulations for the same scenario. The left column shows the case where only the SMPC is employed, while the right column shows the case with th combination of SMPC and FMPC. In this scenario both EV and TV are positioned on the right late of the highway. The TV performs a LK maneuver, therefore, it proceeds its path on the same lane. When the longitudinal distance between the two vehicles is less than 20 m, the EV changes its reference lane and moves towards the left lane. The EV has a reference velocity of 30 m/s and, therefore, at some point it will surpass the TV, which instead has a constant velocity of 27 m/s. When the EV CoG is at least 15 m more forward than the TV CoG, the EV moves towards the right lane by positioning itself in front of the TV. Differences between the two methods can be seen first by looking at Fig. 4.2, where the absolute longitudinal and lateral position difference between EV and TV is shown. The EV LC maneuver from the right lane to the left lane occurs between the time instants t = 4 s and t = 8 s of the simulation. At the same time instants we can see that the combination of SMPC and FMPC keeps the EV more distant with respect to the case of only SMPC. The same thing occurs when the EV surpasses the TV and starts to move to the right lane, i.e., between the time instants t = 16 s and t = 22 s. The combination of SMPC and SMPC delays the EV re-entry in the right lane, with respect to the SMPC alone.

The different control behaviors can be studied with Fig. 4.3 and 4.4 that show, respectively, the command signals, and the longitudinal and lateral velocity of the

4.1. SIMULATIONS 41

SMPC SMPC + FMPC

Figure 4.1: Screenshots of the simulation. Blue box represents the EV, red box represents the TV. Fading boxes show past states. The left column shows the trajectory planned with the SMPC, the right column the trajectory obtained with the addition of the FMPC.

Figure 4.2: Longitudinal and lateral position difference in terms of absolute value between the CoGs of EV and TV.

EV. When the EV is moving towards the left lane, i.e., between the time instants t = 4 s and t = 8 s, the combination SMPC+FMPC makes the vehicle slow down, obtaining a larger distance between the two vehicles, with respect to the case where instead only the SMPC is applied. Same reasoning is to be applied for the re-entry maneuver of the EV once it has surpassed the TV.

Figure 4.3: EV control signals generated with the two methods.

4.1. SIMULATIONS 43

For this scenario, the SMPC always generates a feasible solution for the EV. On the other hand, when the SMPC is integrated with the FMPC, the latter sometimes results unfeasible. This can mean that the SMPC is generating a trajectory that is too much aggressive to let the FMPC find a feasible solution, such as slowing down on time to avoid a potential collision. We have to remember that the SMPC takes into account only a set of possible TV maneuvers, while the FMPC is considering the worst-case scenario and, therefore, a more constringent optimal control problem to solve.

4.1.2

Nonlinear model (straight road)

This second example makes use of the EV nonlinear model (2.5) for the straight road. The scenario is a two-lane highway with one TV which performs a LC maneuver with probability ε = 1.0. The initial conditions are shown in Tab. 4.2.

x [m] v [m/s] y [m] ψ [rad] lref [m]

ξEV _{10.0 26.0 1.75 0.0 5.25}

(a) EV initial conditions: longitudinal position, velocity, lateral position, steering angle, and reference lane.

x [m] vx [m/s] y [m] vy [m/s] εLK εLC

ξTV _{40.0 27.0 1.75 0.0 1.0 0.0}

(b) TV initial conditions: longitudinal position and velocity, later position and velocity, and LK and LC maneuver probability.

Table 4.2: This table summarizes the initial conditions of the scenario. Figure 4.5 shows some frames of the two simulations for the same scenario. The left column shows the case where only the SMPC is employed, while the right column the case with the combination of SMPC and FMPC. In this scenario both EV and TV are positioned on the right lane, and to both of them the left lane is assigned as the reference lane. Therefore, at the beginning of the simulation the two vehicles start a LC maneuver towards the left lane. When the left lane results occupied 20 m in front of the EV, the EV changes its reference lane and moves to the right lane, in order to avoid collision with the TV. Once the EV surpasses the TV, and their longitudinal distance ∆x is bigger than 15 m, the EV overtakes the TV by position itself on the left lane.

Differences between the two methods can be seen by first looking at Fig. 4.6 where the absolute longitudinal and lateral position difference between EV and TV is shown. When the two vehicles are approaching the left lane at the beginning of

SMPC SMPC + FMPC

Figure 4.5: Screenshots of the simulation. Blue box represents the EV, red box represents the TV. Fading boxes show past states. The left column shows the trajectory planned with the SMPC, the right column the trajectory obtained with the addition of the FMPC.

the simulation, the distance ∆x decreases. The EV re-entry maneuver to the right lane starts at time t = 2 s and, once completed, the longitudinal distance is about ∆x = 15 m. During this first part of the scenario the two methods seem to behave in the same way. A difference is more evident when the EV overtakes the TV by positioning itself in front of it. The combination SMPC+FMPC seems to delay the EV re-entry maneuver by guaranteeing more longitudinal space ∆x between the two vehicles, with respect to the SMPC alone.

The different control behaviors can be studied with Fig. 4.7 and 4.8. During the first part of the scenario, i.e., the two vehicles approaching the left lane and the EV re-entry to the right lane to avoid collision with the EV, the two control profiles result identical, both for the signals a and δf. Between time instants t = 4 s and

t = 6 s, the two profiles slightly differ in both control signals. In this time window we have ∆y = 3.5 m, so the EV lies completely on the right lane behind the TV, as can be seen in Fig. 4.6. That means that at some point in this time window, when the EV is approaching the TV, the FMPC problem was unfeasible, making the EV relying on the FMPC trajectory generated at the preceding time step. This can occur because the FMPC takes into consideration the worst-case scenario and, therefore, resulting potentially in a completely fully occupied road at a point of the prediction horizon. Finally, the EV LC maneuver occurs between the time instants t = 15 s and t = 18 s. As seen in Fig. 4.6, the combination SMPC+FMPC makes the EV overtake slightly later than the SMPC alone, guaranteeing a larger distance between the two vehicles.

Here we can also note a difference between the EV models (2.1) and (2.5) by looking at Fig. 4.4 and 4.8. The EV linear model takes about 5 s to move completely to an

4.1. SIMULATIONS 45

Figure 4.6: Longitudinal and lateral position difference in terms of absolute value between the CoGs of EV and TV.

adjacent lane, while the nonlinear model takes about half the time. This is due to the intrinsic difference of the two models, that has an important effect on the two MPCs. Depending on how much fast the EV can change lane, a different trajectory can be found by the controller. The faster is the LC maneuver, a more aggressive trajectory can be found by the SMPC. This could also help the FMPC in finding a failsafe trajectory for the EV, or making it more difficult since the FMPC should find a solution with more constringent conditions.

For this case study we always obtain a feasible solution with the first method, i.e., only using the SMPC. Therefore, we can conclude that when the two methods differ is due to the presence of the FMPC, whose result sometimes can be unfeasible due to its formulation. This makes the combination of SMPC+FMPC more conservative than SMPC alone.

4.1.3

Nonlinear model (curved road)

This third example makes use of the EV nonlinear model (2.7) for curved roads. The scenario is a two-lane straight highway for the first 200 m and then starts to curve with constant curvature K = 0.002 m−1. A probability weight ε is assigned to both LK and LC maneuvers. The actual dynamics of a TV follows the maneuver with higher probability. The initial conditions are shown in Tab. 4.3.

Figure 4.9 shows some frames of the two simulations for the same scenario. The left column shows the case where only the SMPC is employed, while the right column

Figure 4.7: EV control signals generated with the two methods.

4.1. SIMULATIONS 47

x [m] v [m/s] y [m] φ [rad] lref [m]

ξEV 10.0 26.0 1.75 0.0 1.75

(a) EV initial conditions: longitudinal position, velocity, lateral position, steering angle, and reference lane.

x [m] vx [m/s] y [m] vy [m/s] εLK εLC

ξTV1 _{40.0 27.0 1.75 0.0 0.8 0.2}

ξTV2 _{90.0 27.0 5.25 0.0 0.8 0.2}

(b) TVs initial conditions: longitudinal position and velocity, later position and velocity, and LK and LC maneuver probability.

Table 4.3: This table summarizes the initial conditions of the scenario.

shows the case with the combination of SMPC and FMPC. TV1 is positioned on the

right lane, while TV2 on the left lane. Initially the EV proceeds straightforward.

Then, when the right lane is occupied 20 m in front of it by TV1, it executes a LC

maneuver moving to the left lane. Later, when the EV reaches TV2, and verifying

that the left lane is occupied, a new reference lane is computed, and the EV moves to the right lane. Finally, the simulation ends when the EV surpasses TV2.

Differences between the two methods can be seen by first looking at Fig. 4.10 where the absolute longitudinal and lateral position difference between the EV and the TVs is shown. By looking at Fig. 4.10(a), we see that the two methods make the EV moving to the left lane in the same way. However, once the EV is on the left lane and is approaching TV1, when ∆x ' 0, there is an increment in the

longitudi-nal distance at t = 10 s when using the controller SMPC+FMPC. That is probably due to the conservativeness of the FMPC that has made the EV slowing down. By looking at Fig. 4.10(b), we see instead that the two controllers make the EV move in the same when approaching and surpassing TV2.

The difference control behaviors can be studied with Fig. 4.11 and 4.12. Around t = 10 s, the combination SMPC+FMPC has made the EV slowing down to a velocity v = 23 m/s, as introduced above. This occurred due to the formulation of the FMPC that, taking into account the worst-case scenario of other traffic participants, can generate a fully occupied road. Therefore, the controller makes the EV slow down to avoid a potential collision with TV1. On the other hand, when the EV reaches and

surpasses TV2, the two methods work in the same way, showing that not always the

combination SMPC+FMPC yields to more conservative trajectories than the ones generated by only the SMPC.

SMPC SMPC + FMPC

Figure 4.9: Screenshots of the simulation. Blue box represents the EV, red and orange boxes represent, respectively, TV1 and TV2. Fading boxes show past states.

The left column shows the trajectory planned with the SMPC, the right column the trajectory obtained with the addition of the FMPC.

(a) With respect to TV1. (b) With respect to TV2.

Figure 4.10: Longitudinal and lateral position difference in terms of absolute value between the CoGs of EV and TVs.

4.1. SIMULATIONS 49

Figure 4.11: EV control signals generated with the two methods.

4.2

Probability threshold p

We have introduced the parameter pth in Sec. 3.2.4 for the necessity to transform

the chance constraints into deterministic ones. We did not perform a research of the optimal value for pth since this depends largely on the kind of distribution one

adopts, on how the variance of the distribution is propagated along the prediction horizon, and on how the occupancy probability is assigned to the cells of the grid. However, we can discuss how the tuning of this parameter effects the algorithm. First, let us remember that the values of the probability distribution of (3.3) de-crease along the prediction horizon of the SMPC framework, and the potential area occupied by a TV enlarges. Therefore, by setting a relative low value of pth a high

number of cells will result occupied during most of the prediction horizon. This means that at a certain point of the prediction, the road can result fully occupied, making the algorithm planning a conservative maneuver for the EV. On the other hand, a high value of pth will reduce the number of occupied cells and, therefore,

leaving more available space for the trajectory planning, even in the very first steps of the prediction horizon. The consequence is an aggressive behavior of the EV that can lead to collision with a TV.

Therefore, the parameter pth has to be chosen by evaluating a trade-off between

conservativeness and risk for the planning algorithm, depending on the kind of distribution one decides to adopt.

Comparing SMPC with pth= 0 and FMPC As well as just described above,

the lower is the probability threshold pth, the more conservative is the behavior

of the SMPC. One can argue that having the SMPC with pth = 0 leads to an

equality between SMPC and FMPC in terms of motion-planning. This is not true for the following reason. Even if the SMPC plans trajectories with pth = 0, those

trajectories will still differ from the FMPC, because the SMPC is still considering only a set of potential maneuvers of other traffic participants (in this work the LC and LK maneuvers weighted in probability), while instead the FMPC takes into account several maneuvers, e.g., an accelerating LC maneuver or a decelerating one, and all of them form the worst-case scenario that has been explained in Sec. 3.3.

4.3

Computational cost time analysis

This section presents the computational cost of the algorithm in different scenarios. As well as we explained in Chap. 3, only for the TVs that lie in the detection range of the EV an occupancy grid is built. Therefore, the following results are not related to how many TVs are present in the scenario, instead to the number of TVs lying in the EV detection range.