Stochastic Model Predictive Control for Trajectory Planning

STOCKHOLM, SWEDEN 2020

Stochastic Model

Predictive Control for Trajectory Planning

Master Thesis

Martí Fernández-Real

KTH ROYAL INSTITUTE OF TECHNOLOGY

Marti Fernandez-Real <martifrg@gmail.com or martifr@kth.se>

Division of Decision and Control Systems, KTH Royal Institute of Technology ATS Pre-Development & Research, EARM, R&D, Scania CV AB

Place for Project

Stockholm, Sweden Södertälje, Sweden

Examiner

Prof. Jonas Mårtensson, Division of Decision and Control Systems, KTH Royal Institute of Technology

Supervisor

Rui Oliveira

Division of Decision and Control Systems, KTH Royal Institute of Technology ATS Pre-Development & Research, EARM, R&D, Scania CV AB

Trajectory planning constitutes an essential step for proper autonomous vehicles’

performance. This work aims at defining and testing a stochastic approach providing safe, length-optimal and comfortable trajectories accounting for road, model and disturbance uncertainties. A Stochastic Model Predictive Control (SMPC) problem is formulated using a Linear Parameter Varying Bicycle Model, state-probabilistic constraints and input constraints. The SMPC is transformed into a tractable quadratic optimisation problem after assuming independent and gaussian uncertainties.

The proposed trajectory planning methodology is intended to be implemented online in a Receding Horizon fashion in a real vehicle. Results are presented after computer- simulated tests have been carried out to study the influence of model uncertainties and SMPC parameters on the planned and executed trajectories in standard driving situations. Particularly, road crosswind is modelled, its effect on vehicles with different steering characteristics is studied and it is considered for improved trajectory planning. The approach constitutes a promising method to provide robust trajectories to unmodeled errors reaching an equilibrium between conservativeness and quality of the solution.

Keywords

Autonomous Driving, Model Predictive Control, Stochastic Model Predictive Control, Trajectory Planning, Uncertainty, Road Crosswind.

Banplanering utgör ett väsentligt steg för riktiga autonoma fordons prestanda.

Syftet med detta arbete är att definiera och testa stokastiska strategier som ger säkra, optimala och bekväma banor som tar hänsyn till vägen, modelbrus och osäkerheter. En stokastisk Model Predictive Control (SMPC) problem är formulerat med hjälp av Linear Parameter Varying Bicycle Model, tillstånds-sannolikhetsbivillkor och inmatningsbivillkor. SMPC transformeras till ett lätthanterlig kvadratisk optimeringsproblem efter oberoende gaussfördelade osäkerheter antagits.

Den föreslagna banplaneringsmetoden är avsedd att implementeras online med en Receding Horizon för ett riktigt fordon. Resultatet är presenterat efter datorsimulerade experiment har blivit genomförda för att studera påverkan av modelosäkerheter och SMPC parametrar på den planerade och genomförda banor för standard körsituationer. I synnerhet, är sidovind modellerat, dens effekt på fordon med olika styrkaraktäristik är studerad och är tagen hänsyn till för förbättrad banplanering. Tillvägagångssättet utgör en lovande metod för att tillhandahålla robusta banor för icke-model fel som når en jämvikt mellan konservativitet och kvalitet hos lösningen.

Nyckelord

Autonom Körning, Model Predictive Control, Stochastic Model Predictive Control, Banplanering, Osäkerhet, Vägkorsvind.

I would like to thank my supervisor Rui Oliveira for introducing me to the topic and his useful comments, tips and observations. This tough but motivating and rewarding experience would not have been the same without him.

Furthermore, I want to express my gratitude to my examiner Jonas Mårtensson for the constructive feedback along the development of the thesis, and I also thank Gonçalo Pereira for giving me guidance in the most unexplored areas of the project.

Finally, I would like to thank my family and loved ones for giving me the best comfort along the way. I will never forget my spanish-swedish friends.

The research presented in this thesis has been supported by the iQPilot 2016-02547 project funded by the innovation and research programme FFI/VINNOVA.

1 Introduction

2 Background

2.1 Trajectory Planning Techniques . . . 11

2.2 Numerical Optimisation approach . . . 12

2.3 MPC and SMPC for Trajectory Planning . . . 14

3 Model, Methodologies and Methods

3.1.1 Nonlinear Bicycle Model . . . 18

3.1.2 Linear Parameter-Varying Model . . . 21

3.1.3 Boundaries and Input Constraints . . . 22

3.1.4 Model Predictive Control . . . 22

3.1.5 Stochastic Model Predictive Control . . . 24

3.1.6 Making SMPC tractable . . . 25

3.1.7 Collision-avoidance Constraints Tightening . . . 26

3.1.8 LPVBM Uncertainty Analysis . . . 30

3.2 Trajectory Planning Algorithms . . . 31

3.3 Lateral wind disturbance . . . 33

3.3.1 Wind force Y_w . . . 36

3.3.2 Stochastic wind . . . 37

4 Results

4.1.1 Effect of Obstacle location . . . 41

4.1.2 Effect of Risk Parameter p . . . 41

4.2 Receding Horizon Trajectory Planning . . . 45

4.2.1 Crosswind Effect in Trajectory Planning . . . 50 4.2.2 Uncertainties assessment . . . 58

5 Conclusions

6 Future Work

Bibliography

7 Appendix

Introduction

This thesis aims at contributing to a certain extent in the development of autonomous driving technology. This technology can be subdivided in different modular parts, generally as:

• Data Acquisition: From sensors on the vehicle.

• Perception: Environment recognition, vehicle localisation and obstacle detection/classification.

• Communication: Vehicle to Vehicle (V2V) , Vehicle to Infrastructure (V2I) or Vehicle to anything (V2X).

• Decision: Global planning (mission and route planning), Behavioral Planning (event and required maneuver identification), Local Planning (Trajectory generation with obstacle avoidance).

• Control: Lateral, longitudinal and reactive control.

• Actuation: Steering, Throttle and other actuators functioning.

In particular, the thesis focuses in the local planning decision module and control module.

Recent advances in software and computing technology allow to perform heavy numerical computations and solve optimisation problems at an unprecedented speed.

This has a direct impact in sensing, computing and control technologies. Autonomous driving has benefited a lot from these advances: now autonomous vehicles can navigate in urban environments [1] interacting with other vehicles and pedestrians.

A great increase in safety of all the road participants is expected since errors caused from human errors are vanished. Humans errors like distraction, decision errors or performance errors are responsible for 94% of road accidents [2]. Nevertheless, relying on autonomous technology which assumes the environment to always behave as predicted is not completely safe. System and environment uncertainties are always nonzero and should be taken into account so as to make the vehicle robust to them.

Robust trajectory planning, for instance, could be performed by assuming worst-case disturbances, but the resulting trajectory would not be practical and generally too conservative. In this work we propose a stochastic MPC approach to plan and control the vehicle making it robust to system uncertainties and disturbances. Planning and controlling using MPC has been used in many related problems in the past since it is able to account for nonlinear vehicle state dynamics, state constraints and inputs constraints in a straightforward way. Uncertainties can be introduced in the MPC formulation such that the resulting trajectory is robust to them. Since the vehicle model used comes from linearising a nonlinear Bicycle Model, the uncertainties are introduced to account for this model mismatch. When avoiding an obstacle, there is a trade-off between conservativeness and risk of the trajectory, which is controlled through a risk parameter p. The planned trajectory also depends on the uncertainty distribution. One of the goals of this thesis is to understand the impact of p on the trajectory and assess the right magnitude of uncertainties required to account for model mismatch.

External disturbances attenuation in driving can also be handled directly using MPC for control. Tuning of the MPC for proper disturbance attenuation is a challenging and time-consuming process. In the particular case of road crosswind disturbance, however, this project introduces a methodology to take wind predictions into account in order to better attenuate crosswind. The aerodynamic force applied on the vehicle has a direct impact on its lateral dynamics, and these will depend on vehicle’s steering characteristics. This lateral force becomes important when driving at high speeds and for vehicles with large lateral surface, such as buses or trucks. Ultimately, the planner is expected to provide the required inputs to drive the vehicle around the optimal trajectory, and these inputs are influenced by vehicle’s steering characteristics.

Nevertheless, since this approach relies on wind forecast for the duration of the trajectory, a methodology to account for uncertainties in wind forecast is introduced and tested in simulations.

The thesis is structured as follows. Chapter 2 mentions literature work related to trajectory planning and the stochastic approach. Chapter 3 describes the model used and assumptions, constraints, resulting SMPC and reformulated MPC to solve, along with a description of the planning-control algorithms and modelling of the lateral wind disturbance. Chapter 4 illustrates and analyses the main results of the thesis obtained through simulations. Finally, Chapters 5 and 6 summarize the main results and suggest research direction to further develop the technology in the future.

Background

2.1 Trajectory Planning Techniques

The problem of trajectory planning for autonomous vehicles has been deeply studied and tackled in the past. Different techniques and algorithms have been used to find a feasible path fulfilling certain criteria, like obstacle avoidance, travel comfort and optimality in terms of total length of planned trajectory. These algorithms can be divided into different groups according to their particular implementations:

• Graph Search based planners: Consists on discretizing the environment into a graph, consisting in vertices (nodes) and edges between them, with associated costs. The most important ones used in autonomous driving are Dijkstra, A^∗and State Lattices.

• Sampling based planners: Takes samples of the environment and try to connect those to the vehicles in an iterative way, making sure the transition is feasible, creating a tree of points at the end. Rapidly-exploring Random Trees (RRT) is the most known.

• Interpolating curve planners: With a previously known set of reference points, these algorithms generate a new set of data in benefit of trajectory continuity/smoothness, vehicle constraints and dynamic environment, through the usage of interpolating curves fitting certain criteria. Some of these curves are clothoids, polynomials and Bézier curves.

• Numerical Optimisation approaches: These methods optimise a certain function

under certain equality and inequality constraints. They allow to consider physical constraints for obstacle avoidance and road centering, dynamic constraints and actuators constraints in the form of inequality, and can easily model the physical model of the vehicle through equality constraints. These methods can compute the resulting optimal trajectory using a reference trajectory [3], or compute them from scratch [4].

A summary of many of these particular methods is given in [5] and more extensively in [6]. The first three family of methods have some disadvantages when it comes to final trajectory properties. From graph search planners and sampling based planners, the final trajectory obtained is often oscillatory and not smooth, and only suboptimal solutions can be found (good solutions can be found increasing granularity, but in exchange of increasing computational cost). Interpolating curves methods provide a final trajectory which has a restricted structure, since the usage of specific curves for interpolation; also most of these planners require global waypoints for trajectory definition and is usually a difficult task to identify best global waypoints to interpolate over.

Numerical optimisation approaches allow to incorporate physical, dynamic and obstacle avoidance constraints on the trajectory in a straightforward way. Also, unlike the other three approaches, it does not rely on a pre-defined grid or elements in order to obtain the points defining the trajectory. Therefore, due to its flexibility, adaptability to the environment and ease of incorporating different types of constraints, this approach has been chosen in this thesis for trajectory planning.

2.2 Numerical Optimisation approach

Numerical optimisation approaches are able to provide smooth trajectories fulfilling feasibility and collision avoidance, making use of mature numerical schemes to solve some defined optimisation problem. Generally, this approach aims at solving the

following problem:

arg min

x

J (x)

subject to f (x(t), x^′(t), ...) = 0, ∀t ∈ [0, T ] g(x(t), x^′(t), ...)≤ 0, ∀t ∈ [0, T ] x(0) = x_init, x(T )∈ Xgoal

(2.1)

That is, find the trajectory x(t) over time t minimizing some functional J, under some initial and final conditions, and fulfilling some equality and inequality constraints over the trajectory, which may be a function of subsequent derivatives of x. Here the optimisation problem depends only on the trajectory parameter x for simplicity of notation, but it generally depends on vehicle input u, which may stand for wheel steering and/or velocity. The explanation below has to be extended when inputs u are considered, which actually can be seen as a natural extension of the trajectory function x = [x u] (which will be later converted to a vector). The term J accounts for the optimality of the solution with respect to some measure. The function f usually includes the vehicle dynamics at hand, generally nonlinear. This function is regarded in vector form, such that the 0 in the right hand side of the equality belongs to R^m, m being the number of scalar equality constraints. Finally, the term g can include obstacle avoidance conditions, hard dynamic constraints (like maximum jerk allowable, for comfort) and actuators limitations (maximum acceleration allowable, maximum steering, etc.). Similarly, g is regarded in a vectorized form, such that the 0 at the right hand side of the inequality belongs toRⁿ. In this case n refers to the number of inequality constraints to consider.

Problem (2.1) is hard to solve, since the trajectory x(t) is a general function, and analytic expressions for arbitrary trajectories are rarely found. To solve this, an approach often used in numerical optimisation based methods is to split time into time-steps of length

∆t, ti = t0 + i∆t,∀t ∈ [0, N − 1], and find the optimal N points xi = x(ti)fulfilling the modified optimisation problem. Simple schemes can be used to approximate the derivatives of x:

x^′(ti)≈ xi+1− xi−1

2∆t x^′′(t_i)≈ xi+1− 2xi+ xi−1

∆t² ,

now the functions f and g in problem (2.1) are just a function of the redefined vector

x = [x₀, x₁, ..., x_N−1]. In problem (2.1) the functions f and g had to be fulfilled

∀t ∈ [0, T ]. Now, though, they will only have to be fulfilled at every time-step, which makes the problem tractable. Redefining the terms J, f and g the problem now is about solving:

arg min

x∈R^N

J (x)

subject to f (x) = 0, f : R^N → R^m, g(x)≤ 0, g : R^N → Rⁿ x₀ = x_init, x_N−1 ∈ Xgoal

(2.2)

Where J : R^N → R, f : R^N → R^m and g : R^N → Rⁿ (m and n are the number of equality and inequality constraints, respectively).

The goal is finally to solve (2.2), which is a general nonlinear optimisation problem.

With N not being too large, this problem is feasible to solve using standard nonlinear solvers as SQP in reasonable time such that implementing it iteratively with a certain time window T allows to use the method as an online trajectory planner for real on- road driving [4]. Note that, since here an approximation of the derivatives is used, the solution obtained will be an approximated optimal solution, and only at selected time- steps. The deviation of the interpolated solution with respect to the optimal continuous trajectory can be decreased by increasing time-step and using other finite difference schemes for the derivatives, at the cost of higher computational time.

Also, a fundamental limitation of using iterative numerical solvers in a generally nonlinear and nonconvex problem is the possibility of the solver converging into a solution which is not the desired one. Many local optimals could exist. To guarantee that the trajectory obtained is the optimal that we are interested in we will need to provide the solver with a sufficiently good starting point. Even then, though, there are no guarantees that the solution found is the optimal one.

2.3 MPC and SMPC for Trajectory Planning

Model Predictive Control represents a particular numerical optimisation based approach already used in literature for trajectory planning and control of autonomous vehicles.

In this thesis we use a specific variant to account for road and model uncertainties called Stochastic Model Predictive Control (SMPC). In [7] the authors introduce the

SMPC framework for autonomous driving avoiding other vehicles with lane changing maneuvers. Chance-constraints are introduced to deal with obstacle avoidance, which are probabilistic constraints on vehicle future states. This framework is later extended in [8] which studies the suitability of different vehicle and tire models and constraints to be posed on SMPC. Results are given with respect to a risk parameter p accounting for conservativeness and risk of the planned trajectory, but nothing is stated on the actual meaning of this parameter and the required uncertainty magnitudes to account for model errors.

Another framework based on MPC for trajectory planning and control of an autonomous vehicle is Scenario Model Predictive Control. In [9] this data driven approach is presented, which intends to fulfill probabilistic state constraints of SMPC by simulating a finite amount of scenarios. The authors study the proposed approach for highway autonomous driving and lane change assistance. If the number of successful simulated scenarios is larger than a threshold then state-constraints are known to be fulfilled. A similar approach named Randomized MPC is implemented in [10] for low speed miniature race cars. However, and as stated in [8], this data driven approach has a considerably larger computational cost, which limits its use for online trajectory planning of real vehicles.

SMPC deals with chance-constraints by assuming a certain behavior on the stochastic uncertainties. By doing that, it is possible to transform the uncertainty by virtually enlarging the size of the obstacles accordingly. In [7] and [11] the uncertainties are assumed gaussian with zero mean. In the former, obstacle avoidance constraints are formulated in terms of a signed distance which makes the distance between the ego vehicle center of gravity and obstacle convex, and after linearising it the threshold on this distance is augmented. In the latter, instead, the ego vehicle is considered as the full body. In this case a linear distance to the closest obstacle is computed and the condition on this distance is increased again according to the assumed uncertainty distribution.

In this project, and inspired by these works, obstacle avoidance constraints were modelled first as a linear function on states. Later, to reduce the number of iterations required to solve a problem, these were considered to be quadratic instead, as done in [12]. In this work authors formulate a Nonlinear MPC to control a vehicle around the

road center-line while avoiding obstacles. Particularly, obstacle avoidance constraints are modelled as quadratic, forcing the vehicle to remain outside an elliptical obstacle region. However, no uncertainties or disturbances are considered in the vehicle. In this thesis we consider such uncertainties and disturbances, a relevant contribution to improving the safety of the planning algorithms.

Model, Methodologies and Methods

In this chapter we describe the vehicle model used and derive the optimisation problem to solve in order to obtain the planned trajectory, after setting appropriately the different constraints.

Later, it is illustrated how to consider uncertainty in the optimisation problem and why is important to do so. Particularly we present a model to study the uncertainty magnitude required to account for vehicle model mismatch. A description of the implemented planning algorithms is presented as well.

Finally, we model the effect of road crosswind on the vehicle and explain how to take it into account in trajectory planning. We introduce a way to consider uncertainty on crosswind forecasts and make the planned trajectory robust to this uncertainty as well.

3.1 Model used and Optimisation problem

In this section we describe the vehicle model used and explain how to derive the optimisation problem posed on the planned trajectory, with and without uncertainty.

We particularly derive how obstacle avoidance constraints are effected by uncertainty.

These constraints can be modelled as linear and also as quadratic. We also propose a test to find the right uncertainty magnitude to account for model mismatch.

Finally we present the implemented algorithms for trajectory planning accounting for uncertainty.

Figure 3.1.1: Road Aligned Bicycle Model taken from [7].

3.1.1 Nonlinear Bicycle Model

The vehicle model chosen to describe the time evolution of the vehicle is the road aligned bicycle model, illustrated in Figure 3.1.1. Similar to [7], the vehicle state ξ(t) at time t is described by 6 components, which relate the vehicle configuration with a desired road center-line, and the inputs u by 2 components:

ξ = [ ˙x, ˙y, ˙ψ, e_ψ, e_y, s]^T

u = [δ_f, a_x]^T

Where all variables depend on time but this has been dropped for visual clarity.

The variables ˙x and ˙y refer to vehicle’s longitudinal and lateral speed in the body frame, respectively, ˙ψ refers to the yaw rate, e_y and e_ψ denote lateral position and angular errors with respect to the road center-line, respectively, and s refers to the longitudinal position of the closest point in the road to the vehicle’s center of gravity. The input terms are δ_f denoting the front steering angle and a_x referring to longitudinal acceleration. Specifically the set of nonlinear equations describing the

vehicle dynamics in this coordinate frame are the following:

¨

x = ˙y ˙ψ + a_x (3.1a)

¨

y =− ˙x ˙ψ + 2

m(F_y,f + F_y,r) (3.1b)

ψ =¨ 2

I_z(aF_y,f − bFy,r) (3.1c)

˙e_ψ = ˙ψ− κ ˙s (3.1d)

˙e_y = ˙xsin (e_ψ) + ˙ycos (e_ψ) (3.1e)

˙s = 1 1− κey

( ˙xcos (eψ)− ˙y sin (eψ)) (3.1f)

where m and I_z refer to vehicle mass and yaw inertia, a and b refer to the distance between the vehicle’s center of gravity to front and rear axles, respectively and κ is the curvature of the road. The lateral forces F_y,f and F_y,rdenote body frame lateral forces, from front and rear axles respectively, and are given by the expression:

Fy,f =−Cfαf (3.2a)

F_y,r =−Crα_r (3.2b)

with C_f and C_reach referring to the tire cornering stiffness of front and rear axles, and α_f, α_rdenoting side slip angle of front and rear wheels:

α_f =−δf +arctan y + a ˙˙ ψ

˙x

!

≈ −δf +y + a ˙˙ ψ

˙x (3.3a)

α_f =arctan y˙− b ˙ψ

˙x

!

≈ y˙− b ˙ψ

˙x (3.3b)

Approximations in Equations 3.3 are valid for small angles. According to [8] , the linear lateral forces model presented in expressions 3.2 is a very fair approximation for slip angles lower than 10 degrees.

The vehicle dynamics relating all these magnitudes can be formulated compactly in differential form as:

ξ(t) = f (ξ(t), u(t), κ(t))˙ (3.4)

Hypothesis on Cornering Stiffness

Road and tire interaction has to be modelled appropriately. For a given axle, front or rear, the experimented lateral force as result of this interaction depends directly on the axle’s slip angle and normal load. These relationships are generally nonlinear. In order to simplify them we will suppose that they are locally linear.

The interaction between road and tire relating F_y,∗ and α_∗ is well described by the so called ”Magic Formula” tire model, developed and explained in [13]. In [8] authors assume this model to be linear for small slip angles, supported by experimental data in Figure 3.1.2. In our case, then, we will assume that the resulting lateral force expression introduced in 3.2 have constant cornering stiffness C_f and C_rwhen α_∗ ∈ [α∗,min, α_∗,max].

Figure 3.1.2: Graph from [8]. Variation of lateral tire force with slip angle using different tire modelling approaches. The estimated dots are obtained from experimental data. It shows that the linear model is very similar to more complex models up to a certain value of α.

Even for low slip angles the values of C_f and C_r depend on axle’s normal load. The trajectory planner developed in this project assumes a constant normal load on each axle during the maneuver, but for the same vehicle different cargo distribution causes the center of gravity to shift longitudinally, which changes this normal load. The relation between tire normal load and cornering stiffness will be assumed linear for the low range of normal loads applied.

3.1.2 Linear Parameter-Varying Model

At this point, further expressions can be obtained by linearising expression 3.4 but some assumptions need to be made. We assume that the angular error term e_ψis small, such that sin (e_ψ)≈ eψ and cos (e_ψ) ≈ 1. These are assumed great approximations for

|eψ| < 0.2rad, which generally holds for standard highway and road driving. Similarly, lateral velocity ˙y and yaw rate ˙ψare small, yielding ˙y ˙ψ ≈ 0, and combined with previous assumption also ˙ye_ψ ≈ 0. The last 2 assumptions simplify the analysis when lateral dynamics is neglectable, but when lateral wind is considered in future sections these assumptions are not made and the corresponding terms are considered. Finally, the curvature of the road κ is assumed to be known beforehand as a function of longitudinal position s, by means of approximating it online through sensor measurements or given offline by a dataset connected to the vehicle’s localisation unit. These assumptions allow to formulate the bicycle model dynamics in 3.1 as:

ξ(t) = A˙ _c( ˙x, κ)ξ(t) + B_cu(t) (3.5)

Note that expression 3.5 is the linear version of expression 3.4, linearised around parameters ˙x and κ. The expressions for A_c(., .)and B_ccan be computed from System 3.1:

Ac( ˙x, κ) =















0 0 0 0 0 0

0 ^−2(C_{m ˙}^f_x^{+Cr) −2(aC}_{m ˙}^f_x^−bCr) 0 0 0 0 ^−2(aC_I^f−bCr)

zx˙

−2(a²Cf+b²Cr)

Izx˙ 0 0 0

−κ 0 1 0 0 0

0 1 0 ˙x 0 0

1 0 0 0 0 0















, Bc =















0 1

2C_f

m 0

2aCf

Iz 0

0 0

0 0

0 0















(3.6)

The expression for A_cis slightly different when lateral dynamics are not neglectable.

Since now matrix values are known, expression 3.5 can be formulated in a discrete form using Euler forward method yielding:

ξ_k+1 = A_kξ_k+ B_ku_k (3.7)

In (3.7) in the discretized expression A_k the dependencies on the linearisation point through ˙x and curvature parameter κ have been dropped for visual clarity.

3.1.3 Boundaries and Input Constraints

The vehicle is restricted to drive between the boundaries of the road at all times and the planning algorithm needs to consider this. This can be dealt by limiting the magnitude of e_y at each planned point, i.e.

e_y,min ≤ ey(t)≤ ey,max (3.8)

which can be seen as simple linear inequalities on the state. In practice these constraints need to consider vehicle size as they are posed explicitly on vehicle’s center of gravity, so they would not be directly the coordinates of the road edges.

Another important constraint limiting the dynamics of the vehicle and that can be dealt similarly are constraints on the inputs. Specifically the physical actuators are limited in terms of maximum steering angle allowable and maximum and acceleration and braking, along with the associated rates:



δ_f,min a_x,min



 ≤



δ_f(t) a_x(t)



 ≤



δ_f,max a_x,max



 ,



˙δ_f,min

˙a_x,min



 ≤



˙δ_f(t)

˙a_x(t)



 ≤



 ˙δ_f,max

˙a_x,max



 (3.9)

Where the inequalities are understood in an elementwise sense. Using finite differences on expressions 3.9 allows the input restrictions to be summarized as, u_k =



δ_f,k ax,k



 ∈ U ∀k = 0, ..., (T − 1) for some set U defined accordingly.

3.1.4 Model Predictive Control

Given a vehicle model and restriction on states and inputs as mentioned, the problem of trajectory planning while tracking the road center-line can be solved by formulating the optimisation problem 3.10. The goal is to plan a trajectory from the current time instant up to T time-steps ahead, such that it is feasible, drives close to the road center- line and comfortable. The feasibility and optimality will directly come after solving the optimisation problem, but the comfort of the planned trajectory will be influenced mainly through time horizon T and weight matrices Q, R. The process of tuning these parameters is deeply studied in literature [14], [15], however it is problem specific requiring many tests in order to obtain a sufficiently good performing combination.

arg min

ut

T−1

X

k=0

||ξk|t− ξref,k||²Q+||uk|t||²R

subject to ξ_k+1|t= f (ξ_k|t, u_k|t) ξk|t ∈ Xk,

u_k|t ∈ U, for k = 0, ..., T − 1, ξ_0|t = ξ_t.

(3.10)

The equality constraints refer to the model dynamics and they may be nonlinear.

Terms ξ_ref,kand ξ_0|trefer to the road center-line points to follow and the current state of the vehicle, respectively, and are supposed to be known for the online planner. The first term of the function to optimise refers to following the road center-line at a given road speed as much as possible through minimizing the sum of||ξk|t− ξref,k||²Qfor all time- steps. The second term aims at minimizing the input usage, in order not to perform sudden turns or abrupt accelerations in normal driving situations which would result in an uncomfortable journey. Note, though, that feasibility has a higher priority than comfort in this formulation, allowing an uncomfortable solution (for instance a sudden turn) if an unexpected obstacle is encountered in order to fulfill the obstacle avoidance hard constraints.

The principle of Model Predictive Control is to solve 3.10 online at the current time- step, apply the first input obtained and then reformulate and solve the same problem at the following time-step. This is applied in an iterative fashion.

Problem in 3.10 can be solved using standard optimisation packages. The time and computational effort to solve it will generally depend on the number and type of constraints (linear or nonlinear) and time horizon T . Larger values of T allow to plan for a longer time, but at the cost of the problem growing in size increasing the time required to solve it. Similarly, a large amount of complex constraints allow to tight the vehicle’s performance as desired, possibly describing different types of obstacles or vehicle’s limitations, but increasing the computational effort required to solve the optimisation problem.

Note that all expressions used in the MPC formulation are deterministic. However, generally there is a difference between the real model describing the dynamics of the ego vehicle and the model used (linearised bicycle model for instance), as well as model parameters and other parameters known from sensors which may not be completely

accurate. A way to take those differences and errors into account is introducing stochastic terms in the formulation of MPC, originating the so-called Stochastic Model Predictive Control (SMPC).

3.1.5 Stochastic Model Predictive Control

The model of the vehicle presented in (3.7) may not describe exactly the real vehicle’s dynamics, since linearisation and subsequent discretisation is performed. There might be as well low order dynamics which have not been modelled to simplify the model which make it inexact. A trajectory is to be planned relying on the described dynamics model, but also on the fact that obstacles are detected on the road, which might introduce uncertainty. To be able to plan a trajectory as robust as possible to the errors, uncertainties and disturbances, a Stochastic Optimisation Problem is to be solved. This problem can be formulated as an SMPC as follows:

arg min

ut

T−1

X

k=0

E(||ξk|t− ξref,k||²Q+||uk|t||²R)

subject to ξ_k+1|t = A_kξ_k|t+ B_ku_k|t+ D_kw_k|t, P (ξ_k|t ∈ Xk)≥ p,

uk|t∈ U, for k = 0, ..., T − 1, ξ_0|t= ξ_t.

(3.11)

Note from the problem formulation that the equality constraints contain now the term D_kw_k|t, to account for disturbances or uncertainties in the model, which will be stochastic. These will force the function to optimise to be an expectation, and not a deterministic value as in previous problem 3.10. Also, observe that the inequality constraints coming from obstacle avoidance are now chance-constraints. As before, for the trajectory to avoid obstacles the states ξ_kneed to belong to a certain collision- free set Xk for every time-step, which will generally change with time due to the presence of moving obstacles. However, since now the states have a stochastic nature, the constraints are formulated as chance-constraints and can only be ensured up to a certain probability p ∈ [0.5, 1]. This term will be called the ”risk parameter” and will be a tunable parameter. The larger the p, the more conservative will be the planned trajectory and less risky, but too large p might make the optimisation problem unfeasible.

3.1.6 Making SMPC tractable

Solving the SMPC directly is not tractable, and some transformations and assumptions on uncertainties need to be performed in order to change it and solve a Quadratic Problem based on this. To start, we use a similar approach to the closed-loop paradigm introduced in [16] and successfully used in [7] to decompose the state and input in deterministic and stochastic terms. We denote now ξ_k|t and u_k|tby ξ_kand u_k:

ξ_k = z_k+ e_k (3.12a)

u_k = h_k+ c_k (3.12b)

where z_k refers to the deterministic component of ξ_k and e_k denotes the stochastic component of the state. The term h_k represents the optimal control law function of the deterministic term of the state, and c_krefers to a perturbation of that feedback law.

Computing the trajectory assuming no disturbances nor uncertainties in the vehicle model nor obstacle detection can be performed as a usual MPC problem, and h_k is obtained as the optimal inputs values of the deterministic problem.

Introducing 3.12a and 3.12b into state dynamics of 3.11 gives the following:

z_k+1 = A_kz_k+ B_k(h_k+ c_k) (3.13a)

e_k+1 = A_ke_k+ D_kw_k (3.13b)

With these expressions, having split the state ξ_kat each time-step into a deterministic and a stochastic term, it is possible to determine the distribution of e_kfor k = 0, ..., T−1, assuming known e₀ and w_k distribution. Assuming perfect state feedback, ξ₀ = z₀, implies e₀ = 0. Suppose as well that the uncertainties are gaussian with zero mean, w_k ∼ N (0, Σw), ∀k. Letting ek ∼ N (0, Σk), then e_k+1 ∼ N (0, Σk+1), where Σ_k+1 is found, according to 3.13b, from:

Σk+1 = AkΣkA^T_k + DkΣwD_k^T (3.14)

These matrices can be computed iteratively under the perfect state feedback hypothesis, which yields initially Σ₀ = 0.

3.1.7 Collision-avoidance Constraints Tightening

Quadratic Constraints

In this subsection, we assume that the chance-constraints coming from collision avoidance can be described as quadratic constraints, i.e. as in [12]:

P (ξ_k ∈ Xk)≥ p ⇒ P

1

2ξ_k^TH_kξ_k+ f_k^Tξ_k+ l_k ≤ 0



≥ p (3.15)

Where H_k ∈ R^6×6, f_k ∈ R⁶ and l_k ∈ R. If, for example, there is a static obstacle located in the road-aligned coordiante frame at (s, e_y) = (s_obs, e_y,obs), with elliptic shape of semiaxis (a, b), the condition for a trajectory planned to avoid it would be, for each k, make sure that:

(s_k− sobs)²

a² + f rac(e_y,k− ey,obs)²b² ≥ 1 (3.16) Condition 3.16 on trajectory’s planned points is illustrated in Figure 3.1.3.

Figure 3.1.3: Trajectory’s planned points need to remain outside the elliptical obstacle region, denoted by the orange ellipse centered at (s_obs, e_y,obs). An example of planning with T = 10 is illustrated.

which can be formulated as in the RHS term of expression 3.15 using H_k, f_kand l_k:

Hk =















0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 0 0 0

0 0 0 0 −2/b² 0

0 0 0 0 0 −2/a²















, fk =













 0 0 0 0 2ey,obs/b²

2s_obs/a²















, lk = 1− e²_y,obs

b² − s²_obs a²

Note that here H_k, f_kand c_kdo not depend on time-step k since the object is static, but

the formulation for dynamic obstacles is straightforward introducing varying terms, like moving obstacle coordinates (s_obs,k, e_y,obs,k)or varying obstacle sizes (a_k, b_k).

In order to solve the SMPC presented in 3.11, the chance-constraints need to be formulated in a tractable way. Under the mentioned assumptions of gaussian disturbance/uncertainty and quadratic constraints for obstacle avoidance, the probabilistic contraint presented in the RHS of expression 3.15 can be appropriately transformed. Introducing the formulation used in 3.12a into 3.15, it yields:

 P

1

2ξ_k^THkξk+ f_k^Tξk+ lk ≤ 0



≥ p



−→

−→

 P

1

2z_k^THkzk+ f_k^Tzk+ lk+ 1

2e^T_kHkek+ z_k^THkek+ f_k^Tek ≤ 0



≥ p



(3.17)

And this is satisfied, similarly to what is presented in [5], if:

1

2z_k^TH_kz_k+ f_k^Tz_k+ l_k+ γ_k ≤ 0. (3.18a) P

1

2e^T_kHkek+ z_k^THkek+ f_k^Tek ≤ γk



= p (3.18b)

For some γ_k. Note that what expression 3.18a is doing is increasing the size of the elliptic obstacle by some magnitude related to γ_k, which makes sense since if the vehicle is uncertain about future states, it better avoid obstacles being extra cautious.

The parameter γ_kcould be found from expression 3.18b if the probability distribution of the term ¹₂e^T_kH_ke_k+ z_k^TH_ke_k+ f_k^Te_k

was known. Nevertheless, if e_k ∼ N (0, Σk), this expression does not have an analytic probability distribution, since the random variable result of the sum of a χ² and gaussian distribution is not analytic (see that term ¹₂e^T_kH_ke_k ∼ χ² as is the result of squaring the gaussian random variable e_k, and the term (z_k^TH_k+ f_k^T)e_kis gaussian if e_k ∼ N (·, ·) ). What can be done here, though, is neglect the quadratic error term¹₂e^T_kH_ke_k: Since H_kis clearly negative semidefinite, this term will be negative with probability 1, which means that neglecting it in expression 3.18b results in γ_kbeing larger. By doing that, the obstacle avoidance constraint 3.18a for the deterministic term of the state z_kis now extra conservative, but at the same time allows us to compute a term γ_kthat takes into account the error probability distribution at future time-steps.

Neglecting ¹₂e^T_kH_ke_k, then, if we define g_k as g_k = H_k^Tz_k + f_k, and knowing that g_k^Te_k ∼ N (0, gk^TΣ_kg_k), the γ_kterm from the modified expression 3.18b can be obtained

using the quantile function of a univariate normal distribution:

P g^T_ke_k ≤ γk

= p−→ γk =

»

2g^T_kΣ_kg_kerf⁻¹(2p− 1) (3.19)

Where erf⁻¹(·) refers to the inverse error function.

Now a tractable version of the SMPC presented in 3.11 can be obtained, by changing the chance constraints by constraints in 3.18a and using the feedback law 3.13a:

arg min

ck

T−1

X

k=0

||zk− ξref,k||²Q+||hk+ c_k||²R

subject to z_k+1 = A_kz_k+ B_kc_k+ B_kh_k, 1

2z_k^TH_kz_k+ f_k^Tz_k+ l_k≤ −γk,

(hk+ ck)∈ U, for k = 0, ..., T − 1, z_0|t = z_t.

(3.20)

This optimisation problem is now a quadratically constrained quadratic optimisation problem, solvable with usual methods like SQP or interior-point approaches. The terms h_kare constant in the optimisation problem: they act as reference input values that drive the vehicle around a trajectory without uncertainties in the model nor obstacle detection, and the goal is to find the input perturbations c_k modifying that trajectory, accounting for uncertainties and disturbances.

Although not explicitly formulated, problem 3.20 is highly dependent on the risk parameter p∈ [0.5, 1], which modifies γkand thus the obstacle avoidance constraints.

Since γ_k is monotonously increasing in p ∀k, a value of p too large may make the problem infeasible since these inequalities may be impossible to fulfill given other inputs and state constraints. Lower values of p allow the trajectory to be planned closer to the real obstacle with obstacle avoidance inequalities being less restrictive, but at the cost of the trajectory being less conservative and more risky.

Linear Constraints

In order to avoid dealing with a problem with quadratic constraints, it is possible to linearise the obstacle avoidance constraints and propagate model uncertainty as similarly done in the Quadratic Constraints subsection. Particularly now we suppose

that chance constraints can be written as:

P (ξ_k ∈ Xk)≥ p ⇒ P gk^Tξ_k ≤ lk

≥ p (3.21)

Where now g^T_k and l_k can be obtained from linearising the expression 3.16 around a linearisation point (e_y,k,0, s_k,0), for each obstacle obs and timestep k:

G_obs(e_y,k, s_k) = 1− (e_y,k− ey,obs)²

a²_obs −(s_k− sobs)² b²_obs −→

−→ Gobs(e_y,k, s_k)≈ Gobs(e_y,k,0, s_k,0) +∇G(ey,k, s_k)|^T(ey,k,0,sk,0)



e_y,k− ey,k,0

s_k− sk,0



 =

= (...) = g_k^Tξ_k− lk (3.22)

and the expressions for g_kand l_kare given, using the formulas:

g_k =















0 0 0 0

−2

a²_obs(e_y,k,0− ey,obs)

−2

b²_obs(s_k,0− sobs)















, l_k =−Gobs(e_y,k,0, s_k,0) + g_k^T













 0 0 0 0 e_y,k,0

s_k,0















Note that now the linearisation point might initially be infeasible (i.e. it might be inside the elliptical region of the obstacle). The usual first guess to solve the optimisation is the road center-line, but if there is an obstacle on it then it is infeasible. Setting G_obs ≤ 0 enforces the vehicle to lie outside the elliptical obstacle region and thus represents the obstacle avoidance constraint. Using the linear approximation derived in 3.22 this now corresponds to the notion of a linear signed distance, which is desirable to have as a constraint for numerical convergence issues. It is convenient to use a signed distance function to set obstacle avoidance constraints due to its smoothness.

With the chance constraints being linear, it is possible to use a very similar methodology to the one used for quadratic constraints in order to propagate uncertainty and translate the SMPC into a solvable optimisation problem. The methodology followed to this end is the same as the one presented in [7], and the

resulting optimisation problem is very similar to the one in 3.20:

arg min

ck

T−1

X

k=0

||zk− ξref,k||²Q+||hk+ ck||²R

subject to z_k+1 = A_kz_k+ B_kc_k+ B_kh_k, gkzk≤ lk− γk,

(h_k+ c_k)∈ U, for k = 0, ..., T − 1, z_0|t = z_t.

(3.23)

Where here the term γ_k has the same dependency on p than for the quadratic constraints, and its expression is analogous to the one presented in 3.19.

3.1.8 LPVBM Uncertainty Analysis

The linear parameter varying bicycle model (LPVBM) is used to simulate the state transitioning of the ego vehicle given the corresponding input. Assuming a straight road and some time step ∆t, the notation introduced in previous sections can be summarized by:

ξ_k+1 ≈ fLP V BM(ξ_k, u_k) (3.24)

As mentioned before f_{LP V BM}(.)does not describe exactly the dynamics of the real ego vehicle. The function used is the result of linearising and discretising the nonlinear continuous LPVBM. That is one reason to consider uncertainties at each state, w_k, which try to take the model mismatch into account when planning. If we define f_e(.)by the function denoting the exact state transitioning between states of the real vehicle, then we would like to find w_ksuch that:

ξ_k+1 = f_e(ξ_k, u_k)−→ fe(ξ_k, u_k) = f_{LP V BM}(ξ_k, u_k) + w_k (3.25)

What we are interested in is finding the values for wk which added to fLP V BM(.) better encapsulate the real vehicle’s dynamics f_e(.). Note that here the term w_k ∈ R⁶ corresponds to D_kw_k in 3.11 where D_k ∈ R^6×6 is taken as the identity matrix. If the corresponding value of wkcould be found, then we could simply use the RHS term from the second expression in 3.25 as a better expression describing the real ego vehicle’s dynamics, and thus being able to correct f_{LP V BM}to account for model mismatch errors.

However things are not that simple, and to simplify the analysis we assumed that w_k

are stochastic terms behaving as a multivariate gaussian variable w_k ∼ N (µ, Σw).

What can be performed after that is, from second equation in 3.25, to simulate many ξ_k and u_k uniformly in order to obtain many samples of w_k. Then it will be possible to approximate µ and Σ_w by the mean µ^∗ and covariance matrix Σ^∗ of the best fitting multivariate gaussian to the data, (µ, Σ_w) ≈ (µ^∗, Σ^∗). Though, since f_e(.)is still not known, we will approximate it by f_{LP V BM}(.)but finely discretised. Using a very fine discretisation when planning online is not viable due to restrictions in computational cost per iteration, however for this study the computations can be performed offline, and the resulting conclusions be used online.

After the sampling method proposed we will get the approximate distribution of the uncertainty terms w_k, which is a starting point for the amount of uncertainty required to take into account when formulating the SMPC to solve for trajectory planning.

3.2 Trajectory Planning Algorithms

The arising optimisation problem can be solved at each time-step, use the corresponding inputs, and solve it again at the following time-step. This methodology is commonly referred to as Receding Horizon. The possibility of using this Receding Horizon planning, making use of current computational power, numerical solvers and adequately reusing pre-computed solutions is the reason why MPC has become so popular in the lasts decades.

A distinction needs to be made between the trajectory planning when no previous data about trajectory planning (and uncertainties) is available, and when there is data that can be reused. We present in Algorithm 1 a pseudocode for the planning phase when only information about the road, obstacles and vehicle is available, but there is no information about previous optimal trajectories. Since the method used relies on linearising the dynamics and obstacle constraints, an iterative scheme needs to be implemented until some condition of convergence is fulfilled, which is when the algorithm exits the loop starting in line 6. Note, though, that the output of the algorithm is a single trajectory, and it is called Single Horizon (SH) trajectory planning.

We note that on line 4 of Algorithm 1 an optimal trajectory and its corresponding inputs are computed without considering uncertainty, which is explicitly stated through the expression Σ = 0. In this step a nominal trajectory needs to be computed which ensures collision avoidance supposing no uncertainty is present, and that will be used