Thèse De Doctorat

M Denis Gillet

M Sébastien

Glaser Ifsttar

Rapporteur M Philippe Martinet

M Christoph Stiller

M Arnaud

La Fortelle

M Fabien Moutarde

Soutenue Par

Xiangjun Qian

Dirigée Par

Arnaud De

Fabien Moutarde

Model Predictive Control for Autonomous and Cooperative Driving COMPOSITION DU JURY

Keywords: autonomous driving, cooperative autonomous driving, model predictive control, hybrid model predictive control, motion planning, formation control, autonomous intersection management Autonomous vehicle, mode predictive control, motion planning, convoy, autonomous intersection management

Autonomous driving has been gaining more and more attention in the last decades, thanks to its positive social-economic impacts including the enhancement of trafc eciency and the reduction of road accidents. A number of research institutes and companies have tested autonomous vehicles in trac, accumulating tens of millions of kilometers traveled in autonomous driving. With the vision of massive deployment of autonomous vehicles, researchers have also started to envision cooperative strategies among autonomous vehicles. This thesis deals with the control architecture design of individual autonomous vehicles and cooperative autonomous vehicles. Model Predictive Control (MPC), thanks to its eciency and versatility, is chosen as the building block for various control architectures proposed in this thesis. In more detail, this thesis rst presents a classical hierarchical control architecture for individual vehicle control that decomposes the controller into a motion planner and a tracking controller, both using nonlinear MPC. In a second step, we analyze the inability of the proposed planner in handling logical constraints raised from trac rules and multiple maneuver variants, and propose a hybrid MPC based motion planner that solves this issue. We then consider the convoy control problem of autonomous vehicles in which multiple vehicles maintain a formation during autonomous driving. A hierarchical formation control architecture is proposed composing of a convoy supervisor and local MPC based vehicle controllers. Finally, we consider the problem of coordinating a group of autonomous vehicles at an intersection without trac lights. A hierarchical architecture composed of an intersection controller and multiple local vehicle controllers is proposed to allow vehicles to cross the intersection smoothly and safely.

Acknowledgment

My profound gratitudes rst go to my directors of thesis: Arnaud de La Fortelle -who hired me even though I had just quited an unsuccessful Ph.D. thesis and supported me to pursuit what I was interested in, and Fabien Moutarde -who unconditionally helped me when I was in dicult situation and was always supportive when I made decisions on the research directions. Your guidance was indispensable for the achievement of this thesis.

The thesis could not be nished without the immense love of my family: my wife Ning Tan, my parents Lihong Zhu and Qiwei Qian, and my grandparents Juying Liu and Xuexing Qian. You accompanied and will accompany me for ups and downs and you are my reasons to keep moving forward.

Paris is a great city for living and research, but it is always benecial to experience elsewhere. I'd like to thank Prof. Christoph Stiller for accepting my visit in Karlsruhe. I'd also like to thank Sahin and André-Marcel for fruitful collaborations. Three years were not only about research, but also about friendship. I'd like to thank my friends who made this journey of great fun. Thanks in particular to the Babyfoot team Olivier, Houssem, Fernando, Martyna, Ravi, Marie-Anne, Manu and Florent, and non-Babyfoot team Li, Zhuowei, Yanyan, Florent and Philip.

Since the goal was to advance science (and change the world), some research work was still necessary during the breaks of Babyfoot. Special thanks go to Jean, Florent and Philip with whom I had great technical discussions and produced together cutting-edge results. I stayed in two oces during my thesis and the experience was always great. It was amazing to have you around: Axel, David, Sébastien, Florent, Manu, Houssem, Tony, Li, Cyril, Manu, Philip, Marion, Dieu Sang, Eva, Edgar, Philippe and Patrick.

At the end of my thesis, we formed a team and won the Valeo Innovation Challenge. Great thanks to the mates of DOWS team: Philip, Florent, Eva and Soane.

I dedicate my great acknowledgment to Christophe, Christine and Arthur, who were always friendly and helped me to deal with administrative stu.

Many thanks to the members of my thesis jury, who spent precious time to evaluate my thesis and granted me the Ph.D. degree. It is as well of my great honor that my friends came to my defense and participated my cocktail.

Finally, I honor MINES ParisTech and its foundation, who supported my entire graduate study from my engineer's degree to my Ph.D. degree. The knowledge, friends, happiness, frustration and everything that you brought me shaped me to be me.

List of Figures

5.2

Overview of the convoy control architecture. . . . . . . . . . . . . . .

5.3

Road space partitioning with respect to vehicle i. . . . . . . . . . . .

5.4

A sequence of 1-step reachable isomorphic transformations. . . . . .

5.5

First scenario: trajectories of three vehicles. . . . . . . . . . . . . . . Courtesy of Mercedes-Benz and Google.

Autonomous driving has been gaining impetus in the last few years, thanks to its foreseen potential for increasing trac eciency and reducing the number of road accidents. Various research institutes (e.g. Carnegie Mellon University [3],

Karlsruhe Institute of Technology [4]) and companies (e.g. BMW [5], Google [START_REF] Google | Google self-driving car project[END_REF],

PSA [START_REF]Roadmap to the autonomous car[END_REF]) have showcased prototypes of autonomous cars (see Fig. 1.1 for example), demonstrating the enthusiasm and expectations of people towards this new technology. A recent study suggests that up to 50% of road vehicles may be automated by 2030 [START_REF] De | Network of automated vehicles: the autonet2030 vision[END_REF].

Autonomous driving requires three major components (Fig. 1.2): perception and localization, behavior planning and vehicle control. We briey introduce them in the following paragraphs:

• The perception system uses various sensors (radar, lidar, camera, ultrasound, etc.) to retrieve environmental information like lane markings, trac signals, static obstacles, dynamic obstacles, etc. The information is then digitized and represented in a local dynamic map that provide interfaces to other modules. The positioning of the ego vehicle in the local dynamic map is achieved Chapter 1. Introduction • The behavior planning component is responsible for high-level decision making and behavior generation. It sets up driving modes (scenarios) and congures the controller of the autonomous vehicle accordingly. Typical scenarios include lane change, intersection crossing, overtaking, speed regulation due to exceptional events, etc..

• The vehicle control component is responsible for guiding vehicles to proceed while satisfying vehicle dynamic constraints and avoiding obstacles. Although there exists single-level designs for the vehicle control [START_REF] Galceran | Toward integrated motion planning and control using potential elds and torque-based steering actuation for autonomous driving[END_REF][START_REF] Yi | Real time integrated vehicle dynamics control and trajectory planning with mpc for critical maneuvers[END_REF], due to the complexity of the problem, the vehicle control component is usually decomposed into two levels: a motion planner for high level generation of trajectories and a tracking controller to follow these reference trajectories. Note that in some literature, motion planner and tracking controller may also be considered separately as two components for the autonomous vehicle.

Control framework for autonomous driving

In this thesis, we mainly consider the vehicle control component. A considerable amount of literature (see surveys [START_REF] Paden | A survey of motion planning and control[END_REF][START_REF] Gonzalez | A review of motion planning techniques for automated vehicles[END_REF]) can be found on this topic. As mentioned before, most literature proposes to use hierarchical control structures [START_REF] Mcnaughton | Motion planning for autonomous driving with a conformal spatiotemporal lattice[END_REF][START_REF] Kuwata | How. Real-time motion planning with applications to autonomous urban driving[END_REF][START_REF] Ma | Ecient Sampling-Based Motion Planning for On-Road Autonomous Driving[END_REF][START_REF] González | Continuous curvature planning with obstacle avoidance capabilities in urban scenarios[END_REF][START_REF] Gu | Toward human-like motion planning in urban environments[END_REF], with a high-level motion planner to generate dynamically feasible trajectories that avoids all obstacles, and a low-level tracking controller to control the vehicle to track the reference trajectories. Motion planning is computationally intensive due to obstacles and constraints on vehicle dynamics, and replanning is usually done at Control designs based on Model Predictive Control (MPC) [START_REF] Ziegler | Trajectory planning for Bertha -A local, continuous method[END_REF][START_REF] Gao | Model Predictive Control for Autonomous and Semiautonomous Vehicles[END_REF][START_REF] Nilsson | Receding horizon maneuver generation for automated highway driving[END_REF] have attracted increased attention due to their ability to eciently explore the state space using the gradient information. MPC relies on iteratively formulating and solving constrained, nite horizon optimal control problems, generally solved using nonlinear optimization techniques. Because of the predictive nature of MPC, each optimization yields an optimal control trajectory for the given prediction horizon as well as an optimal system trajectory (the expected evolution of the system in the prediction horizon). This specicity of MPC makes it suitable for both the motion planning and the tracking control of autonomous vehicles.

Motion planning for autonomous vehicles consists of two distinct components: a continuous component raised from the vehicle dynamics and usually represented by dierentiable constraints, and a discrete component raised from the driving context usually formulated as non-dierentiable constraints involving binary variables (also referred to as logical constraints). In more detail, there are two main sources of the discrete component. The rst source is trac rules and expected driving behaviors with if-else structure such as: if a vehicle is on a speed bump, then it must drive slowly. The second source is related to the existence of multiple maneuver variants during on-road driving. For example, Fig. 1.3 illustrates an obstacle avoidance scenario for autonomous vehicles. There are four possible maneuvers in this scenario, enumerated as LL, LR, RL, and RR if we use "L" to represent the avoidance by the left of an obstacle and "R" to represent the avoidance on the right-hand.

Nonlinear MPC based motion planners proposed in previous work [START_REF] Ziegler | Trajectory planning for Bertha -A local, continuous method[END_REF][START_REF] Gao | Model Predictive Control for Autonomous and Semiautonomous Vehicles[END_REF][START_REF] Nilsson | Receding horizon maneuver generation for automated highway driving[END_REF] handle well the continuous component while are ill-suited to take the discrete component into account. The rst issue is that nonlinear MPC based methods rely on continuous, gradient-based optimization algorithms that cannot handle logical constraints. Moreover, gradient-based optimization algorithms can be trapped in a local optimum corresponding to one maneuver choice, while various maneuver variants need to be explored in order to nd the global optimum. To handle the rst issue, several methods [START_REF] Gao | Model Predictive Control for Autonomous and Semiautonomous Vehicles[END_REF]2,[START_REF] Weiskircher | Predictive adas: A predictive trajectory guidance scheme for advanced driver assistance in public trac[END_REF] have been proposed to approximate non-dierentiable constraints by dierentiable non-linear functions. Nevertheless, such approximations increase the computational burden. To cope with multiple local optima, some authors [START_REF] Gao | Model Predictive Control for Autonomous and Semiautonomous Vehicles[END_REF] propose to heuristically choose a maneuver choice that is likely to be the best one. However, they provide no guarantee regarding the global optimality and the mere problem of designing ecient heuristics is challenging by itself, especially in complex driving situations.

To sum up, MPC is a promising technique for the control design of autonomous vehicles. However, previously proposed MPC designs are incapable of handling the discrete component of the motion planning problem for on-road autonomous driving.

In consequence, one challenge of this thesis is to solve the following problem:

Problem 1. How to design an MPC based control framework for autonomous driving that can take into account both dierentiable and logical constraints?

Cooperative autonomous driving

With the vision of mass deployment of autonomous vehicles, cooperative strategies for groups of autonomous vehicles start to attract attentions from both automotive industry and research institutions [START_REF] S E Shladover | Automated vehicle control developments in the PATH program[END_REF][START_REF] Bouraoui | Cybercar cooperation for safe intersections[END_REF][START_REF] De | Network of automated vehicles: the autonet2030 vision[END_REF] since they may further amplify the benets of individual autonomous driving.

A major enabler of cooperative autonomous driving is Vehicle-to-Vehicle (V2V)

and Vehicle-to-Infrastructure (V2I) communication technologies [START_REF] Hobert | Enhancements of v2x communication in support of cooperative autonomous driving[END_REF] that allow vehicles to exchange information with each other and with the infrastructure. Significant research eorts [START_REF] Llatser | Greedy algorithms for information dissemination within groups of autonomous vehicles[END_REF][START_REF] Llatser | Vehicular communication performance in convoys of automated vehicles[END_REF] have been made in increasing bandwidth, improving reliability and reducing latency for V2V/V2I communications.

Built upon the communication, two categories of cooperative strategies for autonomous vehicles have been proposed:

Cooperative perception allows the exchange of perception data locally acquired by each autonomous vehicles. The perception data can either be raw sensor data from radar, camera, and other sensors, or fused data that contains a list of detected objects as well as their shapes, positions and predicted trajectories [START_REF] Obst | Multi-sensor data fusion for checking plausibility of v2v communications by vision-based multiple-object Bibliography tracking[END_REF].

Cooperative perception extends the sensing capability of individual vehicles to the V2V/V2X communication range and reduces blind spots that contain security risks.

Cooperative control allows autonomous vehicles to coordinate their trajectories for achieving specic goals. A widely studied form (PATH [START_REF] S E Shladover | Automated vehicle control developments in the PATH program[END_REF], CyberCar-2 [START_REF] Bouraoui | Cybercar cooperation for safe intersections[END_REF],

CHAUFFEUR I & II [START_REF] Harker | PROMOTE-CHAUFFEUR II & 5.8 GHz vehicle to vehicle communications system[END_REF] and SARTRE [START_REF] Chan | Cooperative control of SARTRE automated platoon vehicles[END_REF]) of cooperative control is platooning, in which a group of vehicles forms a linear formation to reduce fuel consumption and enhance road throughput [START_REF] Marjovi | Distributed graph-based convoy control for networked intelligent vehicles[END_REF]. Cooperative control is usually built on the top of cooperative perception as vehicles usually exchanges their intended trajectories 4 1.1. Background and motivations to better maneuver cooperatively.

In this thesis, we mainly consider cooperative control of autonomous vehicles.

More specically, we consider two special forms of cooperation: convoy and autonomous intersection management. Autonomous Intersection Management (AIM) system (Fig. 1.5) coordinates a group of autonomous vehicles at an intersection without trac lights [START_REF] Charalampidis | Speed prole optimization for vehicles crossing an intersection under a safety constraint[END_REF][START_REF] Lee | Development and Evaluation of a Cooperative Vehicle Intersection Control Algorithm Under the Connected Vehicles Environment[END_REF][START_REF] De | Analysis of reservation algorithms for cooperative planning at intersections[END_REF].

In an AIM system, autonomous vehicles cooperate with an intersection controller and/or with each other to cross the intersection without collision. It is shown in [START_REF] Lee | Development and Evaluation of a Cooperative Vehicle Intersection Control Algorithm Under the Connected Vehicles Environment[END_REF] that Autonomous Intersection Management (AIM) systems can signicantly improve intersection throughput.

Control framework for cooperative autonomous driving

One focus of this thesis is to propose frameworks for the previously mentioned two forms of cooperative control: convoy and autonomous intersection management.

In the robotics and control community, generic formation control problems for multiple robots have been an active research area for decades. However, unique Figure 1.5: Screen-shot of the autonomous intersection management system presented in [START_REF] Kurt | A Multiagent Approach to Autonomous Intersection Management[END_REF].

challenges exist for on-road formation control problem. Firstly, vehicles are constrained to move in a highly structured environment (a multi-lane road). Thus the formation must adapt to the road shape. Secondly, each individual vehicle as well as the entire convoy must respect trac rules and avoid collisions with other trafc participants and other convoy members. Thirdly, convoys must be exible so that we can recongure them if necessary. Only a few references [START_REF] Kato | Vehicle Control Algorithms for Cooperative Driving with Automated Vehicles and Intervehicle Communications[END_REF][START_REF] Marjovi | Distributed graph-based convoy control for networked intelligent vehicles[END_REF] consider the coordination of autonomous vehicles on the road. However, none of them fully answers the above mentioned challenges, especially the challenges on intra-convoy collision avoidance and convoy reconguration.

AIM has been an extensive research subject in the last decades. Some AIM designs [START_REF] Lee | Development and Evaluation of a Cooperative Vehicle Intersection Control Algorithm Under the Connected Vehicles Environment[END_REF][START_REF] I H Zohdy | Intersection management for autonomous vehicles using iCACC[END_REF] adopt a centralized approach and use an intersection controller to calculate feasible trajectories for all vehicles. Vehicles are controlled along the planned trajectories to avoid collisions. However, although these designs may have good properties since trajectories can be optimized in advance, a major weakness lies in the diculty to execute the planned trajectories in a changing environment and under control and sensing uncertainties. To enable a quick response to changes and unforeseen events, reactive approaches [START_REF] Makarem | Micro-simulation Modeling of Coordination of Automated Guided Vehicles at Bibliography Intersection[END_REF][START_REF] Makarem | Model predictive coordination of autonomous vehicles crossing intersections[END_REF] have been proposed. Instead of programming complete trajectories, vehicles calculate their current control decisions with respect to other vehicles' states and environmental information. However, purely reactive approaches may be inecient and even lead to deadlocks.

From a more general point of view, the control framework for cooperative autonomous driving also consists of a continuous component and a discrete component.

The continuous component mainly refers to trajectories of individual vehicles, while the discrete component is linked to the existence of multiple maneuver variants when two or more vehicles are involved. In the scenario of convoy, a vehicle have multiple Therefore, the second task of this thesis is to answer the following question:

Problem 2. How to develop control frameworks for cooperative autonomous driving applicationsconvoy and AIM that answer their specic challenges and consider the discrete component?

Contributions

The contributions of this thesis can be synthesized as follows. Chapter 2

Preliminaries

In this chapter, we introduce some preliminary results used in the remainder of the thesis. In the rst part of this chapter, we present the coordinate systems that we will use throughout this thesis. Secondly, we consider the dynamic modeling of the vehicle. Several simplied vehicle models are presented and discussed. Finally, we give a generic presentation of Model Predictive Control methods.

Coordinate systems

x y s r Since we consider on-road driving, it is sometimes convenient to use a roadfollowing coordinate system. We assume that the centerline of the current lane can be described as a curve γ ∈ R 2 with C 3 continuity, ensuring that the curvature and its derivative exist with respect to the curvilinear position. We dene a Frenet coordinate system (s, r), where s is the curvilinear abscissa along γ, and r the lateral deviation. The left and right boundaries of the road are dened as continuous functions r(s) and r(s). To ensure the bijection from the (s, r) frame to a Cartesian frame, we require that for all (s, r), r(s) ≤ r ≤ r(s) ⇒ 1rc(s) < 0, Chapter 2. Preliminaries where c(s) is the curvature prole of the road's centerline at point s.

Vehicle models

We present several vehicle models useful for the planner and controller design of this thesis. Years of research have resulted in a large number of models [START_REF] Galip Ulsoy | Automotive control systems[END_REF][START_REF] Gao | Model Predictive Control for Autonomous and Semiautonomous Vehicles[END_REF][START_REF] Rajamani | Vehicle dynamics and control[END_REF] ranging from overly-simplied point mass models to realistic four-wheel models considering friction and suspension. As the thesis mainly considers the planning and control of vehicles in normal driving scenarios, we focus on presenting some simplied models that are suitable for the model based designs.

Double integrator

ṡ = v, (2.2a) v = a, (2.2b)
where s, v and a are respectively the longitudinal position of the vehicle along the lane centerline, its speed and its acceleration.

To take into account the maximal tire force, we can limit the lateral acceleration of the vehicle using the constraint

v 2 c(s) ∈ [a lat , ālat ], (2.3)
where c(s) is the curvature prole of the centerline. An alternative approach is to 2.2. Vehicle models use the notion of tire-road friction circle

v 2 c(s) 2 + a 2 ≤ (µg) 2 , (2.4)
where µ is the friction coecient and g is the gravity constant. Assumption 2.2. The centerline is considered as straight i.e. c(s) = 0, ∀s.

As the road is considered straight, we can properly chose the Cartesian frame so that the Cartesian frame overlaps with the road-following Frenet frame. As shown in Fig. 2.3, the vehicle dynamics are ruled by the following dierential equation:

ẋ = v x , (2.5a) ẏ = v y , (2.5b) vx = a x , (2.5c
)

vy = a y , (2.5d)
where x and y denote longitudinal and lateral position in the Cartesian (or Frenet) frame, v the speed and a the acceleration, with subscript x or y respectively indicating their longitudinal and lateral components.

This formulation does not consider the nonholonomic constraints of the vehicle, and the longitudinal and lateral dynamics are fully decoupled. The exact coupling Chapter 2. Preliminaries between these dynamics involves nonlinear relations [START_REF] Gao | Model Predictive Control for Autonomous and Semiautonomous Vehicles[END_REF]; therefore, we approximate it by an additional constraint. The vehicle heading θ can be reconstructed as θ = arctan(v y /v x ). We model the coupling of longitudinal and lateral dynamics by

enforcing condition θ ∈ [θ, θ], with v y ∈ [v x tan(θ), v x tan(θ)]. (2.6)
To take into account the maximal tire force, we can limit the lateral acceleration of the vehicle using the constraint similar to (2.3)

a y ∈ [a y , āy ], (2.7)
An alternative approach is to use the tire-road friction circle as in (2.4).

Note that when the road is not straight, simply applying this model to the roadfollowing Frenet frame will induce modeling errors. The magnitude of the errors is related to the road curvature.

ẏ = v sin θ, (2.8b) v = a, (2.8c) θ = ω, (2.8d)
where v, θ are respectively the speed and the heading of the point mass. a and ω are respectively the acceleration and the yaw rate.

Sometimes it is desirable to consider the vehicle motion in the road-following

Frenet frame. The above mentioned model can be transformed to the Frenet frame as ṡ = v cos e θ 1 1rc(s) ,

(2.9a) ṙ = v sin e θ , (2.9b)
v = a, (2.9c
)

ėθ = ω -v cos e θ c(s) 1 -rc(s) , (2.9d)
where s and r are coordinates in the Frenet frame, e θ is the alignment error, i.e., the angle between the vehicle heading and the road centerline. The road information is encoded in c(s), which is the curvature prole of the centerline.

Note that to avoid singularity in the dierential equations, we must add a constraint 1rc(s) > 0.

(2.10)

Finally, the constraint derived from tire-road friction limit must be considered as in (2.3) or (2.4).

Kinematic bicycle model

Bicycle models lump left and right wheels at the front (rear) axes together in the modeling process. Comparing to point mass models, they are more realistic as they consider the side-slip angle. Moreover, they have fewer states than a complete fourwheel model, and they strike a good balance between complexity and realism. They are widely used in the literature [START_REF] Gao | Model Predictive Control for Autonomous and Semiautonomous Vehicles[END_REF][START_REF] Kong | Kinematic and Dynamic Vehicle Models for Autonomous Driving Control Design[END_REF] to plan trajectories or to control the vehicle.

Here we only consider one type of bicycle models: kinematic bicycle model.

As shown in Fig. 2.5, the kinematic bicycle model [START_REF] Rajamani | Vehicle dynamics and control[END_REF][START_REF] Kong | Kinematic and Dynamic Vehicle Models for Autonomous Driving Control Design[END_REF] is governed by the

ẋ = v cos(θ + β), (2.11a)
ẏ = v sin(θ + β), (2.11b)
v = a, (2.11c)
θ = v l r sin(β), (2.11d) φ = ψ, (2.11e) β = tan -1 l r l f + l r tan(φ) , (2.11f )
where x and y are the coordinates of the center of mass. v, θ and φ are respectively the speed, heading and the steering angle of the front wheel, β is the side-slip angle, a and ψ are respectively the acceleration and the steering angular velocity. Eq. (2.11f) is an algebraic equation connecting β and φ. l r and l f are respectively the distance of the center of mass to the rear wheel and to the front wheel.

As in previous models, we must consider tire-road friction ( in (2.3) or (2.4)) limits when using kinematic bicycle model.

Concluding remarks

We have presented dierent vehicle models that will be used in this thesis, ranging from the double integrator for longitudinal motion to the kinematic bicycle model.

Point mass models are computationally cheap while they overly simplify the vehicle dynamics. Therefore, it is adequate to use them in the planning stage. The kine- Model Predictive Control (MPC), also known as Receding Horizon Control, has been shown to be an attractive approach for the motion planning and control of autonomous vehicles [START_REF] Gao | Model Predictive Control for Autonomous and Semiautonomous Vehicles[END_REF][START_REF] Weiskircher | Predictive adas: A predictive trajectory guidance scheme for advanced driver assistance in public trac[END_REF][START_REF] Ziegler | Trajectory planning for Bertha -A local, continuous method[END_REF], thanks to its capability to systematically handle vehicle dynamics, operating limits and on-road obstacles. MPC is a control technique with the fundamental idea of using a model of the system to predict its behavior up to a certain prediction horizon and generating control inputs that satisfy the constraints and minimize a cost function (Fig. 2.6). The optimization is repeated online at each sample time, in a receding horizon fashion, to take into account new measurements on the system state and the environment. Because of the predictive nature of MPC, each optimization yields an optimal control trajectory for the given prediction horizon as well as an optimal system trajectory. This specicity of MPC makes it suitable for both control and motion planning problems.

Traditionally, MPC has been applied to control dynamic systems with slow dynamics [START_REF] Richalet | Model predictive heuristic control: Applications to industrial processes[END_REF] as it was computationally expensive. However, recent advances in optimization algorithms [START_REF] Houska | ACADO Toolkit An Open Source Framework for Automatic Control and Dynamic Optimization[END_REF][START_REF] Inc | Gurobi optimizer reference manual[END_REF]

Model predictive control for systems with real-valued states

We consider a generic dynamic system ξ(t) = f (ξ(t), u(t)), for example, an arbitrary system presented in Section 2.2. Let ξ ∈ R n and u ∈ R m be the state vector and the control input. We assume that all system states are continuously-valued. Without loss of generality, we assume that the control inputs are piecewise constant with a xed sampling time τ , so that the dynamic can be discretized as

ξ k+1 = f d (ξ k , u k ) (2.12)
where f d : R n × R m → R n is the state update function with C 1 continuity. The system (2.12) is subject to the following state and input constraints,

ξ k ∈ X , u k ∈ U, ∀k ≥ 0 (2.13)
where X and U are compact subsets of R n and R m . We shift the time horizon so that the current time is k = 0. We assume that the full measurement of the state ξ 0 is available at the current time as ξ.

Let T be the prediction horizon of the MPC and let T = Kτ with K being the number of time steps and τ being the discretization interval. We can then dene the following nite time optimal control problem to be solved at each time step,

min u J (ξ, u), (2.14)
subj. to

ξ 0 = ξ, (2.15a
)

ξ k+1 = f d (ξ k , u k ), ∀k ∈ [0, ..., K -1],
(2.15b)

ξ k ∈ X , u k ∈ U, ∀k ∈ [0, ..., K],
(2.15c)

ξ K ∈ X f , (2.15d)
where ξ k is the predicted state at time k. ξ = [ξ 0 , ..., ξ K ] is the state trajectory obtained by applying the control sequence u = [u 0 , ..., u K-1 ] to the system starting from the initial state ξ. Equation (2.15d) is the terminal constraint with X f being 2.3. Model predictive control a compact set in R n .

The cost function J (ξ, u) is a C 1 function dened in its generic form as

J (ξ, u) = K-1 k=0 L(ξ k , u k ) + G(ξ K ) (2.16)
where L(•, •) is the running cost and G(•, •) is the terminal cost.

The solution of the problem (2.14) is the optimal control input dened as u * = [u 0, * , ..., u K-1, * ]. Applying u * to the system (2.12) results in the (predicted) optimal state trajectory ξ * = [ξ 0, * , ..., ξ K, * ].

In traditional MPC schemes, the rst sample of u * (in interval [0, τ ) ) is applied to control the system and the optimization problem is reformulated and solved at the next sampling time. However, when (2.12) is just a simplied approximation of the real system dynamics and a low-level tracking controller exists, we may discard u * and pass the state trajectory ξ * to the tracking controller. In this case, the MPC controller actually becomes a motion planner that plans the system trajectory.

The optimization problem is a continuous optimization problem with 2(m + n)K variables, nK equality constraints (from (2.15b)) and a number of inequality constraints (from (2.15c) and (2.15d)). The computational complexity of the MPC is polynomial to the number of variables and constraints. Depending on the (non-)linearity and (non-)convexity of the cost function and the constraints, dierent numeric methods may apply to solve the problem. If the cost function (2.16) is in quadratic form and all constraints are linear, the optimization problem is reduced to a Quadratic Program (QP) for which ecient algorithms exists. On the other hand, if the cost function is not quadratic and/or some constraints are nonlinear, we must rely on Interior Point or Sequential Quadratic Programming algorithms [START_REF] Avriel | Nonlinear programming: analysis and methods[END_REF] which are in general less ecient than QP solvers. Remark that since these solvers rely on gradient descent to minimize the cost function, they can be trapped in local optima if the cost function or some constraints are non-convex.

Model predictive control of hybrid systems

Dynamic systems are traditionally described using a set of dierential or dierence equations, typically derived from physical laws. However, in many applications, the considered system also contains discrete-valued signals along with real-valued signals. We refer to systems with both continuous variables and discrete variables as hybrid systems.

General hybrid systems with nonlinear dierential or dierence equations are hard to control. Here we consider a special type of hybrid systems called the Mixed Chapter 2. Preliminaries Logical Dynamic Systems, which can be described by the following relations in discrete time:

ξ k+1 = Aξ k + B 1 u k + B 2 σ k + B 3 z k + B 4 , (2.17a) C 1 ξ k + C 2 u k + C 3 σ k + C 4 z k + C 5 ≤ 0, (2.17b)
where ξ ∈ R nc × {0, 1} n l is a vector of continuous and binary states, u ∈ R mc × {0, 1} m l are inputs, σ ∈ {0, 1} r l are auxiliary binary variables and z ∈ R rc are continuous auxiliary variables. Capital letters represent matrices of suitable dimensions. Eq. (2.17a) denes the state transition rule of the system. Eq. (2.17b) is the collection of state and control constraints.

Similar to real-valued systems, MPC can be applied to control hybrid systems.

The MPC problem can then be referred as hybrid MPC (hMPC). Wet let the current time be k = 0 and let the full measurement of the state ξ 0 as ξ. Let T be the prediction horizon of the MPC and T = Kτ with K be the number of time steps and τ the discretization resolution. We can dene the following nite time optimal control problem to be solved at each time step:

min u J (ξ, u, σ, z), (2.18) subj. to ξ 0 = ξ, (2.19a) ξ k+1 = Aξ k + B 1 u k + B 2 σ k + B 3 z k + B 4 , k ∈ [0, ..., K -1], (2.19b) C 1 ξ k + C 2 u k + C 3 σ k + C 4 z k + C 5 ≤ 0, k ∈ [0, ..., K -1], (2.19c) ξ K ∈ X f , (2.19d)
where ξ, u, σ, z are respectively vectors for state, control, auxiliary discrete variable and auxiliary continuous variables. Similar to real-valued MPC, the solution of the problem (2.18) is the optimal control input u * with the optimal state trajectory ξ * as by product; the cost function can also be decomposed to running cost terms and a terminal cost as in the real-valued case.

The optimization problem is a Mixed-Integer Programming (MIP) problem.

General MIP problems are hard to solve, but ecient algorithms exist for special instances of MIP, notably Mixed-Integer Linear Programming (MILP) and Mixed-Integer Quadratic Programming (MIQP). Problem (2.18) becomes MILP if the cost function and all constraints are linear. MILP problems can be solved by linear programming based branch-and-bound algorithms. On the other hand, problem (2.18) 20 2.3. Model predictive control is a MIQP if the cost function is quadratic while constraints are linear. Branchand-bound methods based on quadratic programming can be employed to eectively solve the problem. Widely available solvers of MILP and MIQP include CPLEX [START_REF] Ibm | User's manual for cplex[END_REF] and Gurobi [START_REF] Inc | Gurobi optimizer reference manual[END_REF].

Feasibility, stability and robustness

Feasibility of the optimization problem at each sample time must be ensured for the formulated MPC problem. Infeasibility usually rises from the constraints that have some extrinsic variables. For example, in automotive applications, obstacle avoidance constraints contain variables representing the current and future states of obstacles. The values of these variables at k = 0 are usually measured with noisy sensors and the future values are predicted using predened motion models.

An optimization problem that is feasible at the current sample time may become infeasible at the next instant when incorporating new measurements. A possible way to preserve feasibility is to consider all possible values of these variables in the constraint formulation, for example, using the notion of maximal invariant set [START_REF] Sasa | Invariant approximations of the maximal invariant set or "encircling the square[END_REF], so that we have a theoretical feasibility guarantee for the next sample time if the current one is feasible. A simpler alternative is to convert the concerned constraints to soft constraints. For example, a constraint

C 1 x + C 2 u + C 3 ≤ 0 is softened to C 1 x + C 2 u + C 3 ≤ with ≥ 0.
An additional term M 2 is also added to the cost function with M being a large constant. In this way, small violations of constraints are tolerated while strongly penalized to preserve feasibility. Both methods are used in this thesis.

Informally speaking, an MPC formulation is said to be stable if the system is controlled to asymptotically converge to a desired equilibrium. The theoretical guarantee of stability is usually achieved by combining a properly designed terminal cost G(ξ K ) with a terminal constraint ξ K ∈ X f [START_REF] David Q Mayne | Constrained model predictive control: Stability and optimality[END_REF]. However, such design usually increases the computational complexity of the optimization problem. Moreover, in automotive applications, the desired equilibrium points change frequently. Thus, a common practice [START_REF] Gao | Model Predictive Control for Autonomous and Semiautonomous Vehicles[END_REF][START_REF] Weiskircher | Predictive adas: A predictive trajectory guidance scheme for advanced driver assistance in public trac[END_REF] is to ignore the terminal cost and terminal constraints and to verify the stability by extensive simulations and experiments. Naturally, in this way we lose the theoretical guarantee of stability. This thesis adopts the common practice to only consider experimental stability.

Uncertainties exist in the modeling of vehicle dynamics and in the measurement of vehicle states, which raise the problem of robustness for MPC. In more details, we refer to robust stability and robust constraint satisfaction if the respective property is guaranteed under bounded uncertainties. It has been shown that naive MPC Chapter 2. Preliminaries formulation without any robustness consideration is intrinsically robust to small uncertainties thanks to its predictive nature [START_REF] Magni | Stability margins of nonlinear recedinghorizon control via inverse optimality[END_REF]. On the other hand, various robust MPC formulations are discussed in the literature such as Min-Max [START_REF] Scokaert | Min-max feedback model predictive control for constrained linear systems[END_REF], constrainttightening MPC [START_REF] Richards | Robust stable model predictive control with constraint tightening[END_REF], tube based MPC [START_REF] Langson | Robust model predictive control using tubes[END_REF] and multi-stage MPC [START_REF] Lucia | Multi-stage nonlinear model predictive control applied to a semi-batch polymerization reactor under uncertainty[END_REF]. Notably, tube based MPC has been applied to robustly control autonomous vehicles to avoid obstacles [START_REF] Gao | A tube-based robust nonlinear predictive control approach to semiautonomous ground vehicles[END_REF]. This thesis does not consider the problem of robustness, while all MPC formulations in the remaining chapters can be augmented to their robust counterpart by following the tube-based methodology.

Chapter 3

Model Predictive Control for Autonomous Driving

This chapter presents the design of a nonlinear MPC based control framework for autonomous driving. We consider a hierarchical design that decomposes the controller into a motion planner and a tracking controller. At the motion planning level, MPC is used to compute collision-free reference trajectories using the nonlinear point mass model. At the tracking control level, an MPC controller based on the kinematic bicycle model computes control inputs that track the high-level reference trajectories. Simulations are performed to validate the approach. Note that ideas similar to those presented in this chapter can be found in [START_REF] Gao | Model Predictive Control for Autonomous and Semiautonomous Vehicles[END_REF][START_REF] Jacob | An integrated framework for planning and control of semi-autonomous vehicles[END_REF]. Nevertheless, this chapter diers signicantly from [START_REF] Gao | Model Predictive Control for Autonomous and Semiautonomous Vehicles[END_REF][START_REF] Jacob | An integrated framework for planning and control of semi-autonomous vehicles[END_REF] as we use dierent vehicle models and consider obstacle avoidance constraints dierently. The proposed design will serve as a basis for the rest of the thesis.

Introduction

An important research area for autonomous vehicles is the design of a robust and ecient control framework that guides autonomous vehicles to proceed in an environment governed by trac rules and populated with obstacles and other trac participants. Single-level controller designs that map road situations directly to vehicle controls exist in the literature [START_REF] Galceran | Toward integrated motion planning and control using potential elds and torque-based steering actuation for autonomous driving[END_REF][START_REF] Yi | Real time integrated vehicle dynamics control and trajectory planning with mpc for critical maneuvers[END_REF]. However, for computational reasons, most literature designs the control framework in a hierarchical way, with a high-level motion planning (or trajectory planning) module that computes feasible trajectories using simplied vehicle model and considering the environment information, and a low-level tracking controller that tracks the trajectories. With such design, the computationally intensive motion planner can run at a relatively low frequency and the tracking controller can run at a high frequency.

Under this hierarchical philosophy, many motion planning frameworks [START_REF] Mcnaughton | Motion planning for autonomous driving with a conformal spatiotemporal lattice[END_REF][START_REF] Kuwata | How. Real-time motion planning with applications to autonomous urban driving[END_REF][START_REF] Ma | Ecient Sampling-Based Motion Planning for On-Road Autonomous Driving[END_REF][START_REF] Lau | Kinodynamic motion planning for mobile robots using splines[END_REF][START_REF] González | Continuous curvature planning with obstacle avoidance capabilities in urban scenarios[END_REF][START_REF] Gu | Toward human-like motion planning in urban environments[END_REF] have been proposed in the literature, which can be roughly divided into three categories of approaches: path-velocity decomposition approaches, sampling based approaches and Model Predictive Control (MPC) based approaches.

Chapter 3. Model Predictive Control for Autonomous Driving

The path-velocity decomposition technique [START_REF] Kant | Toward Ecient Trajectory Planning: The Path-Velocity Decomposition[END_REF] breaks down the motion planning problem into two sub-problems: path planning and velocity planning. The path planning problem determines a kinematically feasible (curvature-continuous) path along the road. Various path generation methods are proposed using cubic curvature polynomials [START_REF] Nagy | TRAJECTORY GENERATION FOR CAR-LIKE ROBOTS USING CUBIC CURVATURE POLYNOMIALS[END_REF], Bézier curves [START_REF] Lau | Kinodynamic motion planning for mobile robots using splines[END_REF][START_REF] Chen | Quartic bezier curve based trajectory generation for autonomous vehicles with curvature and velocity constraints[END_REF], clothoid tentacles [START_REF] Alia | Local trajectory planning and tracking of autonomous vehicles, using clothoid tentacles method[END_REF], Dubin's paths [START_REF] Villagra | Path and speed planning for smooth autonomous navigation[END_REF], and nonlinear optimization techniques [START_REF] Gu | Toward human-like motion planning in urban environments[END_REF]. The velocity planning problem generates a speed prole that is adapted to the previously generated path.

Some work [START_REF] Xu | Motion planning under uncertainty for on-road autonomous driving[END_REF][START_REF] Gu | Toward human-like motion planning in urban environments[END_REF][START_REF] Li | A practical trajectory planning framework for autonomous ground vehicles driving in urban environments[END_REF] assumes the velocity prole to have certain shape (polynomial, trapezoidal, etc.) and samples some terminal states to generate a set of proles. A best prole for the chosen cost function is then selected.

Sampling based approaches [START_REF] Mcnaughton | Motion planning for autonomous driving with a conformal spatiotemporal lattice[END_REF][START_REF] Kuwata | How. Real-time motion planning with applications to autonomous urban driving[END_REF][START_REF] Ma | Ecient Sampling-Based Motion Planning for On-Road Autonomous Driving[END_REF] sample directly the state space of the autonomous vehicle to obtain a set of feasible trajectories, and then select the best one to execute according to a cost function. Deterministic sampling approaches [START_REF] Mcnaughton | Motion planning for autonomous driving with a conformal spatiotemporal lattice[END_REF] discretize the state space using a spatiotemporal lattice. Graph search methods are then adopted to nd the optimal trajectory. The disadvantage of deterministic sampling is that it is only resolution complete, meaning that the produced trajectory is only resolution-optimal. In order to mitigate this problem, stochastic sampling approaches [START_REF] Kuwata | How. Real-time motion planning with applications to autonomous urban driving[END_REF][START_REF] Ma | Ecient Sampling-Based Motion Planning for On-Road Autonomous Driving[END_REF] are proposed using Rapidly Exploring Random Tree Star (RRT*) and its variants. RRT* is powerful in exploring the state space and is asymptotically complete (the optimal trajectory can be found eventually with a probability of 1).

However, the convergence speed towards the optimal trajectory is usually slow.

The downside of sampling based approaches is that they spend a large amount of computation resources to generate and evaluate a large set of trajectories, while most of them are discarded. Model Predictive Control (MPC) based methods [START_REF] Ziegler | Trajectory planning for Bertha -A local, continuous method[END_REF][START_REF] Nilsson | Manoeuvre generation and control for automated highway driving[END_REF] formulate the trajectory generation problem as an optimization problem over the state space and use gradient-descending techniques to nd the optimal trajectory.

The optimization problem is solved in a periodic fashion with a limited horizon to take into account new environmental information. This category of approaches is ecient in nding the optimal trajectory with the help of the gradient information.

Furthermore, MPC-based approach permits high-precision planning, for example, keeping a precisely given distance from its preceding vehicle, thanks to its optimization nature.

Rich literature also exists for controlling the vehicle to track a reference trajectory. Notably, reference [START_REF] Ackermann | Linear and nonlinear controller design for robust automatic steering[END_REF] presents a sliding mode controller design; reference [START_REF] Antonov | A new atness-based control of lateral vehicle dynamics[END_REF] utilizes the notion of atness to control the lateral dynamics; and reference [START_REF] Menhour | A new model-free design for vehicle control and its validation through an advanced simulation platform[END_REF] presents a model free approach to design the controller. As discussed in 2.3, MPC is applicable both in motion planning and vehicle control, thus some work [START_REF] Gao | Model Predictive Control for Autonomous and Semiautonomous Vehicles[END_REF][START_REF] Yi | Real time integrated vehicle dynamics control and trajectory planning with mpc for critical maneuvers[END_REF] designs MPC-based tracking controllers using vehicle models of various degrees of 3.2. Control architecture overview complexity.

In this chapter, we will present the applications of MPC to the motion planning and the control of autonomous vehicles. More specically, in 3. Detailed planner and tracking controller designs are described in 3.4 and 3.5.

Obstacle Models

A B A major task for autonomous driving is to avoid on-road obstacles including other trac participants, objects on the road and road works. Since obstacles may have dierent sizes, shapes and dynamics, dierent avoidance strategies are required. For example, an autonomous vehicle can avoid bicyclists by swerving around them without changing its lane, while for a slow vehicle driving in the front, the autonomous vehicle must decelerate to avoid rear-end collision (unless a lane change command is issued).

In this section, we study two dierent models for on-road objects. We rst classify objects into two categories:

Obstacle Models

• Lane-Blocking Obstacles (LBOs). Static or dynamic obstacles that block one or more lanes (e.g. Obstacle A in Fig. 3.2).

• Non-Blocking Obstacles (NBOs). Static or dynamic obstacles that only block a small part of a lane such that autonomous vehicles can swerve to avoid them and proceed (e.g. Obstacle B in Fig. 3.2) .

Note that the classication for an object may change overtime considering the current driving context. For example, a bicyclist at the border of the current lane can be considered as an NBO unless we detect that he/she decides to cross the road. In this situation, he/she should be considered as an LBO since the autonomous vehicle must wait until the bicyclist has passed to proceed. As the focus of this thesis is on vehicle control, we assume that obstacle classications are provided in a timely manner through the obstacle information interface by the perception algorithm.

We

-r + (as 2 + bs + c) ≤ 0, (3.2)
and for an obstacle aected to the upper lane boundary, the constraint is given as r -(as 2 + bs + c) ≤ 0.

(3.3)
The unknown coecients a, b, c can be calculated by solving the following linear system.

   s 2 o,0 s o,0 1 s 2 o,1 s o,1 1 s 2 o,2 s o,2 1       a b c    =    r o,0 r o,1 r o,2    , (3.4)
where [s o,0 , r o,0 ] is the vertex of the parabola and [s o,i , r o,i ], j ∈ {2, 3} are the only two points where the enhanced obstacle region intersects with the parabola.

Note that this approximation method inevitably overestimates the occupancy of an NBO on the road space. If the on-road environment is cluttered, more precise estimation will be necessary using higher degree polynomials or other nonlinear functions.

We compactly written the mentioned obstacle avoidance conditions for LBOs and NBOs as: ∀o, h(ξ, p o ) ≤ 0.

(3.5)

Motion planner 3.4 Motion planner

We consider the MPC formulation for the motion planner. We recall the nonlinear point mass model in the Frenet frame (2.9) discussed in Section 2.2.3:

ṡ = v cos e θ 1 1 -rc(s) , (3.6a) ṙ = v sin e θ , (3.6b
) v = a, (3.6c) ėθ = ω -v cos e θ c(s) 1 -rc(s)
.

(3.6d)

We dene ξ = [s, r, v, e θ ] as the state vector. s and r are respectively the longitudinal and lateral coordinates of the vehicle. v is the vehicle speed and e θ is the heading error. The control inputs are u = [a, ω], with the rst component being the acceleration and the second one being the yaw rate. We compactly write the model as ξ = f (ξ, u).

To take into account the dynamic limitations of the vehicle, we dene bounds as Let T be the prediction horizon of the planner and K be the number of steps.

ξ ∈ [ξ, ξ], u ∈ [u, ū], (3.7
Let ξ = [ξ 0 , ..., ξ K ] and u = [u 0 , ..., u K ] be the vector of states and control inputs.

Consider the following cost function in least-square form:

J (ξ, u) = K k=0 (||ξ k -ξ k p,ref || 2 Q + ||u k || 2 R , (3.9
ξ 0 = H( ξ), (3.10a
) Solving the MPC problem in a receding horizon fashion results in a sequence of optimal trajectories ξ * . The optimal trajectories are then fed to the controller for tracking.

ξ k+1 = f d p (ξ k , u k ), (3.10b)
ξ k ∈ [ξ, ξ], u k ∈ [u, ū], (3.10c) v k ω k ∈ [-ā lat , ālat ], (3.10d) 1 -r k c(s k ) ≥ ε, (3.10e) h(ξ k , p k o ) < k o , ∀o, (3.10f

Tracking controller 3.5 Tracking controller

Recall the kinematic bicycle model in the Cartesian frame: Bounds are dened as

ẋ = v cos(θ + β), (3.11a) ẏ = v sin(θ + β), (3.11b) v = a, (3.11c) θ = v l r sin(β), (3.11d) φ = ψ, (3.11e) β = tan -1 l r l f + l r tan(φ) , (3.11f
ξ c ∈ [ξ c , ξc ], u c ∈ [u c , ūc ], (3.12)
and we require

ξ c = [-∞, -∞, 0, -∞, φ], ξc = [+∞, +∞, v, +∞, φ], u c = [a, ψ], ūc = [ā, ψ]. (3.13)
Let T c be the prediction horizon of the controller and K c be the number of steps.

Let ξ c = [ξ 0 c , ..., ξ Kc c ] and u c = [u 0 c , ..., u Kc c ] be the vector of states and control inputs.

Consider the following cost function in least-square form: Table 3.1: Parameters used for the proposed control design

J c (ξ c , u c ) = Kc k=0 (||ξ k c -ξ k c,ref || 2 Qc + ||u k c || 2 Rc ), (3.14
The MPC controller is then formulated as

min uc J c (ξ c , u c ), subj. to ∀k ∈ [0, ..., K c -1], ξ k+1 c = f d c (ξ k c , u k c ), (3.15a)
ξ k c ∈ [ξ c , ξc ], u k c ∈ [u c , ūc ]. (3.15b)
The solution of the MPC problem is the optimal acceleration prole and the optimal steering speed prole. Since the vehicle is controlled by steering, we feed the acceleration and the steering angle (instead of the steering speed) to the vehicle.

Simulations

In this section, we use extensive simulations to illustrate the performance of the proposed control architecture. We have implemented our framework in the robotic simulator Webots [START_REF] Michel | Webots: Professional Mobile Robot Simulation[END_REF]. The control algorithms are coded in C++ and we use the ACADO toolkit [START_REF] Houska | ACADO Toolkit An Open Source Framework for Automatic Control and Dynamic Optimization[END_REF] to solve the MPC problems. Simulations were performed on a personal computer running on a 3.4 GHz Intel Core i7 CPU with 32GB of RAM.

Simulation results

We rst perform the simulation assuming perfect localization. Fig. 3.6a illustrates the vehicle trajectory and Fig. 3.6b presents the speed and steering proles of the vehicle. We observe that the vehicle decelerates when approaching the obstacles (to satisfy the constraint on the lateral acceleration) and successfully avoids four obstacles.

To demonstrate the robustness of the control architecture with respect to localization errors, we disturb the GPS signal with a uniformly chosen random error inside [-0.5 m, 0.5 m]. No ltering technique is used to smooth the GPS signal. The simulation result is presented in Fig. 3.7. We notice that the ego vehicle is still able to plan and track collision-free trajectories. The fact that we replan from the nearest point on the previous trajectory rather than the current position enhances the stability of the planned trajectories.

Dynamic NBO avoidance Scenario description

We consider the scenario illustrated in Fig. We assume that the trajectory predictions of the cyclist are provided by the perception component. In the simulation, the cyclist is assumed to maintain constant speed during the prediction horizon. The vehicle localization is assumed to be perfect.

Simulation results

Fig. 3.9a illustrate the trajectory of the ego vehicle. We observe that the ego vehicle successfully circumvents the cyclist.

Lane change at the presence of an LBO Scenario description

We consider the scenario illustrated in Fig. 3.10 in which the ego vehicle is approaching a slow vehicle (considered as an LBO) driving at a constant speed of 10 m/s. The initial speed and the desired speed of the ego vehicle are both set to 15 m/s. Since The constant time gap from the slow vehicle T gap is set to 2 s. In the simulation, the slow vehicle is assumed to maintain constant speed during the prediction horizon.

The vehicle localization is assumed to be perfect. One major requirement for nonlinear MPC is that its constraints must be differentiable. However, on-road autonomous driving often leads to non-dierentiable or even non-continuous constraints. For example, the surface of a polytope-shaped obstacle is non-dierentiable. In this chapter, we used bounding parabolas to approximate obstacle regions to create dierentiable constraints, which leads to overestimation of the area of the obstacle region. Is there a way to incorporate nondierentiable constraints into the proposed framework? In the next chapter, a novel hybrid MPC based motion planner will be proposed to consider both dierentiable and non-dierentiable constraints.

Simulation results

Chapter 4

Model Predictive Control for Autonomous Driving Integrating Logical Constraints

In this chapter we design a hybrid Model Predictive Control (hMPC) motion planner for autonomous driving. In the previous chapter, we formulated the motion planning problem as a nonlinear program with dierentiable cost functions and constraints. Gradient-based optimization methods were then used to nd the optimal trajectory. However, these methods are ill-suited for logical constraints such as those raised by trac rules, presence of obstacles and, more importantly, to the existence of multiple maneuver variants. We propose a new hMPC design of the motion planner to formulate the trajectory generation problem as a Mixed-Integer Quadratic

Program. This formulation can be solved eciently using widely available solvers, and the resulting trajectory is guaranteed to be globally optimal. We apply our framework to several scenarios that are still widely considered as challenging for autonomous driving, such as obstacle avoidance with multiple maneuver choices, intersection crossing, overtaking with oncoming trac or optimal lane-change decision making. Simulation results and eld experiments demonstrate the eectiveness of our approach and its real-time applicability.

Introduction

In Chapter 3, we have presented a MPC based motion planner which translates the planning problem into a nonlinear optimization problem. The simulation results

have shown that the proposed planner works well with dierentiable cost functions and constraints. However, the context of on-road autonomous driving also leads to a dierent family of constraints, referred to as logical constraints because they can be naturally formulated as propositional logic rules.

The rst source of logical constraints is trac rules and expected driving behaviors, for instance:

• If a vehicle is on a speed bump, then it must drive slowly.

Chapter 4. Model Predictive Control for Autonomous Driving Integrating Logical Constraints

• If a vehicle wants to exit the highway, it must be on the exit lane.

These rules with if-else structures can be easily mapped to logical propositions. Moreover, driving generally involves discrete decisions among multiple ma- The ego vehicle (white car) has multiple maneuver choices, for example, it can overtake the blue car after both the red and the green cars have passed, or it can overtake using the space between the red car and the green car, or it can overtake before the arrival of the red and the green cars. It has been rigorously shown in [2,[START_REF] Park | Homotopy-based divide-andconquer strategy for optimal trajectory planning via mixed-integer programming[END_REF] that each maneuver variant can be mapped to a unique homotopy class of trajectories.

Propositional logic is adequate to implicitly or explicitly encode these homotopy classes.

By nature, sampling-based methods can accommodate logical constraints. These methods try to explore the entire state space with a large set of samples. We only need to verify each sample against all constraints. However, they can only produce sub-optimal trajectories even with many samples. As mentioned in the previous chapter, MPC is ecient in nding optimal trajectories. However, most current MPC based approaches are ill-suited to take these logical constraints into account. The rst issue is that logical constraints are usually discontinuous or nondierentiable, while MPC-based methods rely on continuous, gradient-based optimization algorithms like interior point or sequential quadratic programming [START_REF] Bertsekas | Nonlinear programming[END_REF].

Moreover, gradient-based optimization algorithms can be trapped in local optima inside certain homotopy classes, while in order to nd the global optimum we need to explore various maneuver variants. To handle the rst issue, methods [START_REF] Gao | Model Predictive Control for Autonomous and Semiautonomous Vehicles[END_REF]2,[START_REF] Weiskircher | Predictive adas: A predictive trajectory guidance scheme for advanced driver assistance in public trac[END_REF] 4.1. Introduction have been proposed to approximate logical constraints by continuous and dierentiable non-linear functions. For example, in [2], the rectangle-shaped obstacle region is approximated using sigmoid functions. To cope with multiple local optima, some [START_REF] Gao | Model Predictive Control for Autonomous and Semiautonomous Vehicles[END_REF][START_REF] Wang | Hierarchical hybrid predictive control of an autonomous road vehicle[END_REF] propose to roughly evaluate dierent maneuver choices on the behavioral planning level and heuristically initialize the optimization problem in a homotopy class that is likely to be the best one. However, they provide no guarantee regarding the global optimality and the design of heuristics is also challenging if the driving context is complex.

A promising way of incorporating logical constraints in an MPC based framework is to transform propositional logic rules into mixed integer constraints, thus the optimization problem becomes Mixed-Integer Programming (MIP) problem [START_REF] Lee | Mixed integer nonlinear programming[END_REF].

MPC with integer variables is referred to as hybrid MPC (hMPC) [START_REF] Bemporad | Model predictive control of hybrid systems[END_REF]. General MIP problems are hard to solve, while ecient algorithms exist for special instances of MIP, notably Mixed-Integer Linear Programming (MILP) and Mixed-Integer Quadratic Programming (MIQP). Both have successfully been used for some specic instances of robot motion planning problems. In [START_REF] Schouwenaars | Safe receding horizon path planning for autonomous vehicles[END_REF][START_REF] Bellingham | Receding horizon control of autonomous aerial vehicles[END_REF], MILP is used for Unmanned Aerial Vehicle (UAV) trajectory planning. Logical constraints are used to model the non-convex and non-dierentiable obstacle regions. In [START_REF] Borrelli | Mpc-based approach to active steering for autonomous vehicle systems[END_REF], MILP is applied to multi-vehicle collision avoidance problem, using logical constraints to model the inter-vehicle collision avoidance conditions. Concerning automotive applications, references [START_REF] Nilsson | Strategic decision making for automated driving on two-lane, one way roads using model predictive control[END_REF][START_REF] Du | Autonomous lane-change controller via mixed logical dynamical[END_REF] use MILP for the decision making of automatic lane changes. However, the vehicle dynamics is overly simplied. Reference [START_REF] Park | Homotopy-based divide-andconquer strategy for optimal trajectory planning via mixed-integer programming[END_REF] In this chapter, we propose a novel and generic hMPC based framework for the motion planning problem of on-road autonomous driving using MIQP. Our method permits to seamlessly consider both continuous and logical constraints. We apply our framework to several scenarios which are widely recognized as challenging for autonomous driving: for example, intersection crossing with the presence of other vehicles, avoidance of multiple obstacles, overtaking in the presence of oncoming trac and optimal lane change planning. We demonstrate that our method can intuitively handle these situations and produce globally optimal trajectories without explicitly enumerating all maneuver variants. Finally, the proposed planner is

Motion Planner

In the following sections, we will present the design of the motion planner and its applications to dierent driving scenarios.

Motion Planner

We present an hMPC based motion planner design to formulate the optimal trajectory planning problem as an MIQP problem. We consider that the ego vehicle is driving on a road with lane markings. We make the following assumption:

Assumption 4.1. The road curvature is suciently small to consider the road as straight within the horizon of one MPC stage.

This assumption allows us to model the vehicle dynamics in a Cartesian frame (x, y) with x the longitudinal direction of the road and y the lateral direction.

Remark that this assumption has a major drawback. It induces modeling errors proportional to the road curvature and the lateral deviation of the vehicle with respect to the road centerline. However, we expect this error to be small in major applications like highways and urban arterial roads.

Model

x y We rst recall some basics of propositional logic. We dene a literal as an atomic statement corresponding to a linear mathematical condition on one of the state variables, for instance: the speed of the vehicle is smaller than 10 m/s. Literals can be combined using connectors, such as ∧ (and), ∨ (or), ¬ (negation); other connectors like implications (⇒) and equivalences (⇔) can be formed using the rst two connectors. To illustrate, we consider a speed bump scenario in which Once we have abstracted related logic rules into formal propositions, we can further reformulate them as a set of linear inequalities with continuous and integer variables, as shown in [START_REF] Wayne L Winston | Introduction to mathematical programming[END_REF]. The idea is to force the value of a binary variable to be equal to 0 (or 1) when a given literal is true, and/or equal to 1 (or 0) when the literal is false. In the previous example, we can let δ k i = 1 ⇒ P i for i ∈ {1, 2, 3} so that the speed bump rule can be expressed equivalently as, ∀k ≥ 0,

δ k 1 = 1 ⇒ x k ≥ 40, (4.6a)
δ k 2 = 1 ⇒ x k ≤ 60, (4.6b)
δ k 3 = 1 ⇒ v k x ≤ 10, (4.6c)
-

δ k 1 + δ k 3 ≤ 0, (4.6d) -δ k 2 + δ k 3 ≤ 0, (4.6e)
δ k 1 + δ k 2 -δ k 3 ≤ 1. (4.6f )
The associations (4.6a), (4.6b) and (4.6c) can then be nally translated to mixedinteger inequalities using the so-called Big-M method [START_REF] Wayne L Winston | Introduction to mathematical programming[END_REF]. For instance, (4.6a) is rewritten as

x k ≥ 40 -M (1 -δ k 1 ), (4.7)
where M is a large positive constant. For example, we can conveniently choose M to be 10 6 .

MPC formulation

We introduce a new vector-valued variable σ such that σ k = {0, 1} m is a collection of all binary variables that are introduced at time k from the reformulation of relevant literals as mixed-integer linear inequalities. We consider a time horizon T = Kτ with K being the number of time steps in the prediction horizon. Let ξ = {ξ 0 , ..., ξ K } and u = {u 0 , ..., u K } respectively be the state and control trajectory for the given time horizon. Let ξ ref be the reference trajectory for the vehicle, which can be timedependent, state-dependent, or dependent on propositions. Let σ be the trajectory of the binary variable [σ 0 , ..., σ K ]. We also introduce σ ref as the reference trajectory for the binary variables, so that we can also express preferences on some binary states if needed, for instance to specify a preferred lane in a multi-lane road. At current time t = 0, we formulate the following optimization problem min u,σ

J(ξ, u, σ) = K k=0 (||ξ k -ξ k ref || 2 Q + ||σ k -σ k r || 2 R + ||u k || 2 S + ||∆u k || 2 W ), (4.8)

Motion Planner

subject to

ξ 0 = H( ξ), (4.9a)
ξ k+1 = A d 0 0 A d ξ k + B d 0 0 B d u k , k = 0, ..., K -1, (4.9b) ξ k ∈ [ξ, ξ], u k ∈ [u, ū], k = 0, ..., K, (4.9c) v k y ∈ [v k x tan(θ), v k x tan(θ)], k = 0, ..., K, (4.9d
)

∆u k = u k -u k-1 , k = 0, ..., K, (4.9e) C    ξ ξ ref σ    ≤ D, (4.9f )
where Q, R, S, and W are non-negative weighting matrices of proper dimensions.

Eq. (4.9a) is the constraint on initial value. We introduce ∆u k as the dierence between two consecutive control inputs to penalize jerk (Eq. (4.9e)). Note that we must know the previous control input u -1 . Constraint (4.9f) is the set of all linear inequalities written in matrix form with two matrices C and D of proper dimensions, incorporating all mixed-integer constraints derived from the current context of driving. The cost function (4.8) is quadratic and the constraints (4.9)

are linear and potentially mixed integer. Therefore, the above optimization problem is an instance of mixed-integer quadratic programming (MIQP) problems. Integer programming problem are NP-hard and so are MIQP problems since it is a subset of integer programming problems, however, exact resolution algorithms that deploy ecient heuristics are known to solve such problems without exploring the whole decision tree.

The optimization problem is formulated and solved in a receding horizon fashion.

At each time step, the solution of the problem is a globally optimal trajectory ξ * p which is fed to the tracking controller.

The proposed MPC formulation is suciently generic to cover a vast majority of normal driving scenarios, as will be illustrated in Section 4. T , θ = -0.5 rad, θ = 0.5 rad, q 1 = 1, q 2 = 2, q 3 = 4, s 1 = 2, s 2 = 4, w 1 = 4, w 2 = 16. Applicative examples and simulations

In this section, we present a variety of applicative examples to concretize the proposed motion planner design. This design can be congured to eectively handle various on-road driving scenarios, including speed bump, intersection crossing, obstacle avoidance, overtaking and lane change. Note that, although the major purpose of this section is to demonstrate the exibility of the design, the detailed formulations for dierent driving scenarios are by themselves contributions to some challenging problems for on-road autonomous driving. Simulations are performed with in-depth quantitative analysis, proving the implementability of our framework.

Throughout this section, we use the following cost function for the planner S in the generic formulation is ignored. In all cases, the ego vehicle is assumed to start at position (0, 0). Other parameters that are common in all cases are summarized in Table 4.1. Scenario-specic parameters will be presented respectively in each case study.

J = K k=0 q 1 (v k x -v ref ) 2 + q 2 (y k -y k ref ) 2 + q 3 (v k y ) 2 + s 1 (a k x ) 2 + s 2 (a k y ) 2 + w 1 (a k x -a k-1 x ) 2 + r 2 (a k y -a k-1 y ) 2 ,
In all simulations, we assumed that the perception components estimate the trajectories of surrounding objects using constant velocity hypothesis (objects maintain constant speeds in the prediction horizon). More realistic estimation methods could be envisaged but are not considered in this thesis. The vehicle localization is assumed to be perfect.

The motion planner updates at a frequency of 5 Hz. The low-level tracking controller is identical to the one described in 3.5.

Note that the feasibility issue mentioned in 2.3.3 also raises in hMPC design, in which the optimization problem may become infeasible over successive iterations due 50

Applicative examples and simulations

to small violations of some constraints. The constraint softening technique described in 2.3.3 is adopted to transform the concerned constraints to soft constraints. Here we do not explicitly describe the constraint softening procedure.

We use the commercial solver Gurobi [START_REF] Inc | Gurobi optimizer reference manual[END_REF] to compute solutions to our MIQP formulation. Since the MIQP problem is NP-hard and Gurobi uses heavily optimized heuristics to accelerate calculations, there is no guarantee of computation time (even though the solver is quite ecient most of the time). We notice that Gurobi often nds the optimal solution in a short time while spending much more time in proving the optimality of the solution. This inspires us to set an upper-bound of 200 ms on the computation time so that Gurobi returns the currently best solution if hitting the upperbound. In this way we guarantee the update rate of the motion planner.

However, there is no longer theoretical guarantee of optimality even though we observe that most of the time the solutions are in fact optimal (while not yet proved by the solver).

The non-linear optimization problem of the tracking controller is solved by ACADO Toolkit [START_REF] Houska | ACADO Toolkit An Open Source Framework for Automatic Control and Dynamic Optimization[END_REF]. Simulation codes are written in C++, and experiments are performed on a laptop with a mid-range Intel Core i5-5300U CPU clocked at 2.30GHz with 8GB RAM.

Speed bump

Scenario description

The rst case study considers the speed bump scenario (Fig. 4.4) that is used as an example in 4.

Simulation results

In the simulation, we use a prediction horizon T = 5 s and time step duration τ = 0.25 s. Fig. 4.5 illustrates the longitudinal speed prole of the planned trajectory with respect to the traveled distance. We observe that the vehicle eectively reduces its speed to less than 10 m/s.

Intersection crossing

Scenario description

We consider the problem of controlling a vehicle at an intersection, where it is not allowed to cross the intersection unless no other vehicle (called non-controlled vehicle) is inside the intersecting region. For example, Fig. 4.6 shows a scenario with two non-controlled vehicles. The ego vehicle has four maneuver choices: crossing before both cars, after the blue car and before the red car, before the blue car and after the red car, or after both cars. Some maneuver choices may be impossible or dynamically infeasible for the ego vehicle. In [START_REF] Schildbach | A collision avoidance system at intersections using robust model predictive control[END_REF], the authors propose to construct an MPC problem for each maneuver choice and select the global optimum among all local optimal solutions. Here, we demonstrate that with our framework explicit enumeration is no longer necessary. At the beginning of a motion planner iteration of the ego vehicle, we monitor the states of non-controlled vehicles that have not passed their intersection regions.

Ego Vehicle

We rst compute the earliest arrival time t o for o with respect to its intersection region [L o , H o ] by assuming that it applies an acceleration of āx,o > 0 (Fig. 4.7):

t o = T (L o -x o , v x,o , āx,o ),
δ o = 0 ⇒ x k o ≥ H, (4.14a) δ o = 1 ⇒ x ko ≤ L. (4.14b)
As the variable δ o can only take either 0 or 1 for its value, the ego vehicle is required to either traverse the intersection before o's earliest arrival time or stay out of the intersection until o leaves the intersection.

If k o ≤ K ≤ ko , we let δ o = 0 ⇒ x k o ≥ H, (4.15a) δ o = 1 ⇒ x K ≤ L.

Simulation results

In simulations, we use a prediction horizon T = 5 s and τ = 0.25 s. We choose āx,o = 1 m/s 2 and a x,o = -1 m/s 2 . In the rst simulation, we assume that noncontrolled vehicles keep constant speed during the intersection crossing. In Fig. 4.8a, we observe that the ego vehicle choose to traverse the intersection after Vehicle 1 and before Vehicle 2. Fig. 4.8b illustrates the speed prole of the ego vehicle. We observe that the ego vehicle slightly decelerates at the beginning to yield to Vehicle 1 and then return to its desired speed.

To serve as a comparison, we perform a second simulation in which we force Vehicle 2 to accelerate with an acceleration of 1 m/s 2 between t = 2.5 s and t = 10 s. and Vehicle 2. In Fig. 4.9b, the predicted trajectories suggest that during the rst 2.5 s, the ego vehicle decides to pass between Vehicle 1 and Vehicle 2. However, since Vehicle 2 starts to accelerate from 2.5 s, the motion planner nds that this decision is no longer optimal and switches to another decision.

Note that the proposed formulation can also be adapted to select the optimal We now consider an obstacle avoidance scenario during on-road driving as shown in Fig. 4.10. There are two identical obstacles centered at [START_REF] Alia | Local trajectory planning and tracking of autonomous vehicles, using clothoid tentacles method[END_REF][START_REF] Kurt | A Multiagent Approach to Autonomous Intersection Management[END_REF] and (120, -1). The irregular shapes of obstacles are approximated using minimal bounding rectangles (region with light red color) with the length L o = 10 m and width W o = 2.6 m, taking into account the size of the ego vehicle. A more complex convex polygonal modeling is also possible, at the cost of increased computational complexity.

Formulation of constraints

For an obstacle o with bounding rectangle

[x k o -L o , x k o + L o ] × [y k o -W o , y k o + W k o ]
, the set of constraints for collision avoidance is then given as ∀k ≥ 0,

δ k o,1 = 1 ⇒ x(k) ≤ x k o -L o , (4.16a) δ k o,2 = 1 ⇒ x(k) ≥ x k o + L o , (4.16b) δ k o,3 = 1 ⇒ x(k) ≤ y k o -W o , (4.16c) δ k o,4 = 1 ⇒ x(k) ≥ y k o + W o , (4.16d) δ k o,1 + δ k o,2 + δ k o,3 + δ k o,4 = 1.

Simulation results

We choose the horizon of the motion planner as T = 10 s and τ = 0.5 s. We consider an overtaking scenario on a two-lane road with oncoming trac as in Fig. 4.12. The ego vehicle is driven towards the east with a slow vehicle in the front. Two vehicles are on the adjacent lane driving on the opposite direction.

The ego vehicle has multiple choices for overtaking: overtaking before the arrival of the two oncoming vehicles, overtaking using the space between the two vehicles or overtaking after the passing of two vehicles. This scenario is considered as dicult for both human drivers and autonomous vehicles [START_REF] Murgovski | Predictive cruise control with autonomous overtaking[END_REF][START_REF] Nilsson | Receding horizon maneuver generation for automated highway driving[END_REF]2].

We assume that the initial speed of non-controlled vehicles are 10 m/s and the initial speed of the ego vehicle is equal to its desired speed v ref = 15 m/s. Vehicles are all 3.5 m and 2.5 m wide.

Applicative examples and simulations

Formulation of constraints

It is possible to model surrounding vehicles as rectangles as in the previous case study, thus requiring four integer variables for each vehicle and each time step k.

However, by introducing the so called ramp barrier [START_REF] Nilsson | Receding horizon maneuver generation for automated highway driving[END_REF][START_REF] Murgovski | Predictive cruise control with autonomous overtaking[END_REF], the problem can be further simplied by approximating the rectangular obstacle region by a triangular one only requiring two integer variables as shown in Fig. 4.12.

Consider a surrounding vehicle o; if o is on the same lane as the ego vehicle, the constraints are given as

δ k o = 0 ⇒ - x k -x k o L o + y k -y k o W o ≥ 1, (4.17a) δ k o = 1 ⇒ x k -x k o L o + y k -y k o W o ≥ 1, (4.17b)
where L o and W o are minimal longitudinal and lateral separations during lane change. Similarly, the constraints for an oncoming vehicle o can be modeled as

δ k o = 0 ⇒ x k -x k o L o + y k -y k o W o ≤ -1, (4.18a)
δ k o = 1 ⇒ - x k -x k o L o + y k -y k o W o ≤ -1, (4.18b)

Simulation results

In simulations, we adopt a suciently long prediction horizon of T = 15 s so that the planner can plan a complete overtaking trajectory. We let τ = 0.5 s. The parameters L o = 8 and W o = 3.5.

In the rst simulation, we assume that all surrounding vehicles drive at constant speeds. The duration of simulation is 30 s. Fig. 4.13a illustrates the overtaking trajectory. In Fig. 4.13b, we observe that the ego vehicle rst decelerates slightly to wait the rst on-coming vehicle to pass, then it accelerates to use the space between the rst and the second on-coming vehicles to perform the overtaking.

To serve as a comparison, in the second simulation, we assume that the second on-coming vehicle accelerates with a constant acceleration of 1 m/s 2 between t = 7.5 s and t = 15 s and maintains constant speed for the rest of time. Fig. 4.14a

illustrates the trajectory of overtaking. The ego vehicle rst plans the same strategy as in scenario 1, by using the space between two oncoming vehicles to overtake.

However, as the second on-coming vehicle starts to accelerate, the ego vehicle nds that it's more desirable to wait for both on-coming vehicles to pass and then perform Integrating Logical Constraints the overtaking. This scenario demonstrates the reactivity of the motion planner with respect to the changes of surrounding vehicles.

-100

0

Formulation of constraints

The complexity of this problem lies in the multiple discrete choices raised from multiple lanes and multiple vehicles on each lane. References [START_REF] Du | Autonomous lane-change controller via mixed logical dynamical[END_REF][START_REF] Nilsson | Strategic decision making for automated driving on two-lane, one way roads using model predictive control[END_REF] have considered this problem using MILP formulations; however, their modeling cannot ensure that trajectories are dynamically feasible due to important simplications of the vehicle dynamics.

We consider a road with N lanes, labeled by γ ∈ {1, ..., N }. We introduce a binary variable δ k γ that equals 1 if the ego vehicle is on lane γ at time step k. Let I be the label set of surrounding vehicles and I γ be the set of vehicles inside lane γ.

Let y γ be the centerline of the lane γ and y k r be the reference lateral deviation at time step k. We introduce the following logical constraint: ∀k > 0,

δ k γ = 1 ⇒ y k r = y γ ∧ y k ∈ [y γ , y γ ] , (4.19)

Applicative examples and simulations

such that, if the ego vehicle is in lane γ, then the vehicle should be within the boundary of lane γ and the reference lateral deviation should be set to the centerline of the lane.

Moreover, we add the following collision avoidance constraints: ∀k ≥ 0,

δ k γ = 1 ⇒ ∀i ∈ I γ ,δ k o = 0 ⇒ x k ≤ x k o -L o , δ k o = 1 ⇒ x k ≥ x k o + L o , (4.20)
such that the ego vehicle must avoid collisions with all the vehicles in lane γ.

The ego vehicle is only allowed to be in one lane at any given time, thus we add the following constraint:∀k ≥ 0,

N γ=1 δ k γ = 1

Simulation results

The horizon is set to T = 15 s and the time step τ = 0.5 s. The simulation duration is 30 s. The parameter L o is chosen to be 10 m. We observe in Fig.

Results

We have successfully performed multiple eld tests. Fig. 4.18a demonstrates the trajectory of the ego vehicle as well as the predicted trajectories in one of the tests.

We observe that the vehicle successfully performed the overtaking using the space between two on-coming vehicles. Fig. 4.18b shows the speed and the steering proles of the ego vehicle. Note that there is a large steering input at around t = 24 s, which is caused by the instability of the default tracking controller at low speeds.

Concluding remarks

We Chapter 5

Control Framework for Convoy

In the previous two chapters, we have considered the control framework for individual vehicles. We have employed MPC based techniques to plan collision-free and optimal trajectories for autonomous vehicles and to track reference trajectories.

In this chapter, we will consider the cooperative formation control of multiple autonomous vehicles in an on-road environment. We will present a hierarchical framework that employs MPC based approaches as building blocks. The framework uses a global convoy supervisor to manage the formation and local vehicle controllers to track the formation-keeping reference trajectories satisfying various constraints of the vehicles. The reference trajectories of a vehicle are computed from its leader's trajectories, based on a pre-dened formation tree. The proposed framework will be validated using high-delity simulations.

Introduction

Cooperative strategies for groups of autonomous vehicles start to attract attentions from both automotive industry and research institutions, due to their potential in improving trac eciency and reducing road accidents. Previous projects (PATH [START_REF] S E Shladover | Automated vehicle control developments in the PATH program[END_REF], Cybercars-2 [START_REF] Bouraoui | Cybercar cooperation for safe intersections[END_REF], CHAUFFER I & II [START_REF] Harker | PROMOTE-CHAUFFEUR II & 5.8 GHz vehicle to vehicle communications system[END_REF] and SARTRE [START_REF] Chan | Cooperative control of SARTRE automated platoon vehicles[END_REF]) have extensively studied a special form of cooperative driving: platoon, in which a group of vehicles forms a linear formation. It is shown that platooning vehicles brings a 15% to 30% fuel consumption reduction and a 3 to 5 times increase of road throughput [START_REF] Marjovi | Distributed graph-based convoy control for networked intelligent vehicles[END_REF].

In this chapter, we consider an extended version of platoon in which multiple vehicles can enter a formation (also referred to as convoy) with both longitudinal and lateral separations (e.g. Fig. 5.1). We expect that this extension can nd its applications in cooperative tasks, for instance protecting a VIP vehicle, snowplowing (see example in [START_REF] Ono | Road-Following Formation Control of Autonomous Ground Vehicles[END_REF]), cooperative lane change, etc..

In the robotic and control community, generic formation control problem for multiple robots has been an active research area for decades. Roughly speaking, there are three approaches to tackle this problem, namely leader-following, virtual structure and behavioral approaches. In leader-following approaches [START_REF] Shao | Leader-following formation control of multiple mobile vehicles[END_REF][START_REF] Consolini | Leaderfollower formation control of nonholonomic mobile robots with input constraints[END_REF], a leader is selected to track a reference trajectory, while other robots maintain a relative orientations/position osets from the leader. The virtual structure approach [START_REF] Ren | Virtual structure based spacecraft formation control with formation feedback[END_REF][START_REF] Ren | Decentralized scheme for spacecraft formation ying via the virtual structure approach[END_REF] considers the formation as a rigid structure. Robots are regarded as nodes in a structure: a trajectory of the structure is rst calculated and then transformed to the reference trajectories of individual robots. In behavioral approaches [START_REF] Balch | Behavior-based formation control for multirobot teams[END_REF], robots are prescribed with several behaviors, notably goal seeking, local formation keeping, collision avoidance, etc. The control of each individual robot is a weighted average of the control for each behavior. The global group behavior emerges from the behaviors of individual vehicles.

The literature in robot formation control lays a solid foundation for the formation keeping problem of multiple autonomous vehicles. However, unique challenges exist to apply them to on-road driving. Firstly, vehicles are constrained to move in a highly structured environment. Thus the formation must adapt to the road shape.

Secondly, each individual vehicle as well as the entire convoy must respect trac rules and avoid collisions with other trac participants and other convoy members.

Thirdly, convoys must be exible so that we can recongure them if necessary.

There exists some previous work that tackles the formation keeping problem of autonomous vehicles on the road. Kato et al. [START_REF] Kato | Vehicle Control Algorithms for Cooperative Driving with Automated Vehicles and Intervehicle Communications[END_REF] consider a specic convoy problem with 5 vehicles spreading over two lanes using a leader-following approach. In [START_REF] Marjovi | Distributed graph-based convoy control for networked intelligent vehicles[END_REF], a distributed graph-based convoy control algorithm is proposed. Each vehicle memorizes and tracks its neighborhood. The local control input is calculated using the Laplacian graph method. The advantage of this method is that it is fully decentralized. However, the formation is limited to a rectangle shape and the vehicle controller is in its simplest form (feedback controller based on rst order modeling of the vehicle, no obstacle avoidance capability, etc.). None of the above mentioned work satises the requirements on intra-formation collision avoidance and convoy reconguration.

We propose and validate a novel on-road convoy control framework based on the leader-following approach, upon which we propose multiple adaptations to handle the challenges raised by the on-road driving setting. We adopt a hierarchical design, composed of a global convoy supervisor and multiple local vehicle controllers. The

Control architecture overview

convoy supervisor centrally manages the geometrical model of the convoy and is able to modify the convoy on-the-y. Local vehicle controller employs the hierarchical MPC based architecture to track the reference trajectories computed from the leader's trajectory, based on a pre-dened formation tree. We present simulations to demonstrate that our framework is suitable for actual implementation. The convoy supervisor maintains a formation tree that describes the leaderfollower relations between vehicles. The root of this formation tree is the leader of the convoy. The convoy supervisor is also in charge of monitoring the cooperative status of dierent vehicles (e.g. whether vehicles have reached the desired positions) and of re-conguring the formation if necessary. From the implementation perspective, the convoy supervisor can be implemented as a software module in one or several vehicles inside the convoy, and can use the communication devices of these vehicles to manage the convoy.

Control architecture overview

Local vehicle controllers employ the hierarchical architecture described in 3.

The formation tree is communicated by the convoy supervisor to individual vehicles through the scenario specication interface. Local vehicle controllers then track their desired trajectories ξ i,ref oseted from the trajectories of their leaders ξ j (assuming j is the leader of i), in conformity with the formation tree. Other inputs of vehicle controllers are not shown in Fig. 5.2 while are identical to the architecture in 3.

In the following, we will detail the design for the convoy control architecture.

Convoy supervisor

In this section, we will present in detail the design of the convoy supervisor. 5.3.1 will discuss the mathematical representation of the convoy. 5.3.2 will consider the strategy that allow vehicles in a convoy to avoid collisions with each other. 5.3.3 will investigate the mechanism for safe modications of convoy structures.

Convoy model

We consider the convoy in a road-following coordinate system. We assume that the centerline of a lane γ ∈ R 2 is selected as the reference curve to dene the Frenet coordinate system (s, r), where s is the curvilinear abscissa along γ, and r the lateral deviation.

We consider a set N = {0, .., N } of N + 1 vehicles, in which we arbitrarily use 0 as leader; the leader is considered to be a physical vehicle for simplicity in this thesis, while it could also be a virtual reference point.

Formation tree

To describe the interdependence relations between vehicles in the formation, we dene a formation tree as a directed tree G = (N , E), whose nodes are the vehicles of N and with a set of edges E such that the root node is the leader 0. Such a tree can be represented as an adjacency matrix (g ij ) of size (N + 1) × (N + 1), in which element g ij equals 1 if the edge i → j is in E, and 0 otherwise. Our aim is to design an intra-formation collision avoidance strategy such that vehicles will neither collide in normal on-road driving situations, nor in situations where the formation is perturbed by obstacles.

Remark that the exact rectangle-shaped safety region of a vehicle is non-dierentiable, and multiple maneuver choices exist for collision avoidance. For instance, consider a 2-vehicle scenario (e.g., vehicles 1 and 2 in Fig. 5.1): if vehicle 1 approaches vehicle 2, vehicle 2 has four strategies to avoid collision: accelerate to the front of vehicle 1, decelerate behind vehicle 1, move further to the right or circumvent vehicle 1 from its left. The problem becomes exponentially complex as the number of vehicles increases due to its combinatorial nature. In [START_REF] Borrelli | Hybrid decentralized control of large scale systems[END_REF], a similar problem is handled through the formulation of a mixed integer programming problem, in which the avoidance decisions are modeled as binary variables. However, the real-time requirement of our algorithm stops us from adopting the same method.

Here, we adopt a hybrid modeling approach. Let us dene an ordered priority list L as a permutation of {0, 1, ..., N }, representing the relative priorities between vehicles. With a slight abuse of notation, we let L(i) be the rank of i in L, i.e. L(i) = j if, and only if, i is the j-th element of L. We enforce the following constraint for any pair of vehicles i, j ∈ N : Design constraint:

L(i) < L(j) ⇐⇒ s d i ≥ s d j 73
Chapter 5. Control Framework for Convoy Under this constraint, the list L encodes a trac rule for the formation of vehicles: each vehicle is only responsible for avoiding collisions with the higher-priority vehicles on the road, i.e with its predecessors in L.

We divide the space outside the collision region into six areas: for a vehicle i, we partition the space using three ane functions h 1 i (s, r) = 0, h 2 i (s, r) = 0 and h 3 i (s, r) = 0 as shown in Fig. 5.3:

h 1 i (s, r) = - r -r i ∆r + s -s i ∆s + 1, (5.1a)
h 2 i (s, r) = r -r i ∆r + s -s i ∆s - 1,
(5.1b)

h 3 i (s, r) = s -s i ∆s + 1.
(5.1c)

where ∆s and ∆r are geometric parameters described in Fig. 5.3. For each vehicle i, these ane functions dene a partition of the (s, r) plane in six sub-spaces labeled as A m i , m ∈ {0, ..., 5} as shown in Fig. 5.3. By denition, vehicle i is always inside region A 0 i , and we enforce safety by preventing vehicles with lower priority than i from entering A 0 i . In what follows, we call A 0 i the protected region for i, and the union of subsets A m i , m ∈ {1 . . . 5} forms the safe region with respect to i.

For an arbitrary vehicle j, the following logic rule is introduced for all i ∈ N : L(i) < L(j) ⇒ (s j , r j ) ∈ m={1,...,5}

A m i .

(5.2) Rule (5.2) eectively forces each vehicle to remain in the safe region with respect to the vehicles having higher priority in L. In a classic hybrid MPC setting, a binary decision variable would be required to select in which subspaces A m i , m ∈ {1, . . . , 5} vehicle j should be. In this chapter we adopt a dierent, simpler approach consisting of using the shape matrix M to implicitly constrain the choice of the maneuver choices as follows: ∀i, j ∈ N such that L(i) < L(j),

s d ji ≥ -∆s ∧ r d ji > 0 ⇒ h 1 i (s j , r j ) ≤ 0, (5.3a)
s d ji ≥ -∆s ∧ r d ji < 0 ⇒ h 2 i (s j , r j ) ≤ 0, (5.3b)
s d ji < -∆s ⇒ h 3 i (s j , r j ) ≤ 0.

(s 2 , r 2 ) ≤ 0 ∧ h 2 1 (s 2 , r 2 ) ≤ 0, such that vehicle 2's safe region is (A 2 0 ∪ A 3 0 ∪ A 4 0 ) ∩ (A 3 1 ∪ A 4 1 ∪ A 5 1 ): vehicle 2 must
stay behind vehicle 0, and stay on the right-hand or behind vehicle 1.

Dynamic formation modication

We consider the reconguration of convoy structure during cooperative autonomous driving. Let G be a formation tree, M a shape matrix and L a priority list, all compatible with respect to the design constraint: we denote by F = (G, M, L) the corresponding formation. We give the following denition:

Denition 5.1 (Isormorphic formation). Two formations F 1 , F 2 are said to be

isomorphic if L 1 = L 2 .
In this chapter, we only consider isomorphic formation changes: the reconguration of M and/or G. Non-isomorphic formation changes that involve the modication of L are not considered in this thesis. The reconguration of G only aects the behaviors of vehicles when the formation is perturbed. Thus we are free to modify G as long as the tree structure covers all nodes. On the other hand, we cannot arbitrarily modify the shape matrix M due to the design of intra-formation collision avoidance constraint (5.3). Consider the case of two vehicles i, j with L(i) < L(j),

where vehicle j is in the partition A 5 i of vehicle i. Assuming that we recongure the shape matrix, if the new goal conguration of vehicle j resides in A 4 i , then vehicle j can plan a trajectory to its goal conguration because both A 4 i and A 5 i belong to the constraint h 2 i (s, r) ≤ 0. However, if the goal conguration is in A 2 i , vehicle i cannot plan a feasible trajectory under the current formulation as A 2 i is not a feasible region under the current constraint h 2 1 . It is necessary to set an intermediate goal conguration at A 3 i . Once vehicle j reaches A 3 i , the collision avoidance constraint must be switched from h 2 i to h 3 i and then vehicle j can continue to move towards its goal conguration. This issue is a side eect of the space partitioning technique.

We propose the following denitions: Denition 5.2 (1-step Reachable Element). Consider two elements of the partition A m i and A n i ; we say

A n i is 1-step reachable from A m i if there exists l ∈ {1, 2, 3} such that h l i ≤ 0 over A n i ∪ A m i .
This means we need at least one of the rules (5.3) to be kept during reconguration. We can see that A 3 i and A 4 i ; A 3 i is reachable from all elements except A 0 i ; and symmetrically for the other areas.

A 1 i is reachable from A 2 i and A 3 i ; A 2 i is reachable from A 1 i ,
Denition 5.3 (1-step Reachable Formation). Consider two formations M 1 and M 2 . We say that M 2 is 1-step reachable from M 1 if ∀i, j ∈ N such that L(i) < L(j), the conguration of vehicle j with respect to vehicle i in M 2 is 1-step reachable from M 1 .

Theorem 5.1. Consider an arbitrary pair of isomorphic formations F 1 , F 2 , we can transform from F 1 to F 2 through a nite sequence of 1-step reachable formations.

The proof is intuitive, since A 3 i is reachable from all elements except A 0 i . There- fore, any formation can be transformed to a linear formation in a nite number of steps. With the above theoretical result, in order to recongure the formation to the desired one, we only need to design an intermediate sequence of 1-step reachable formations. Then we can set up a discrete supervisor to control the transformation.

Example 5.4. We consider a formation of four vehicles. Fig. 5.4 illustrates a sequence of 1-step reachable isomorphic transformations. The corresponding shape matrices are given as follows.

M 1 M 2 M 3 M 4       0 0 -10 3 -10 -3 -20 0             0 0 -10 0 -20 0 -30 0             0 0 -10 -3 -10 3 -20 0             0 3 0 -3 -10 3 -10 -3      

Local vehicle controller

We employ the same vehicle controller as described in 3, with adaptations at the motion planner level to take into account cooperative constraints.

Chapter 5. Control Framework for Convoy

min u i J i (ξ i , u i ), subj. to ∀k ∈ [0, ..., K -1],
ξ 0 i = H( ξi ),

(5.7a)

ξ k+1 i = f d (ξ k i , u k i ), (5.7b)
ξ k i ∈ [ξ i , ξi ], u k i ∈ [u i , ūi ],
(5.7c)

v k i ω k i ∈ [-ā lat,i , ālat,i ],
(5.7d)

1 -r k i c(s k i ) ≥ ε.
(5.7e)

h(ξ k i , p k o i ) ≤ 0, ∀o i , (5.7f ) g a j (s i , r i ) ≤ 0, ∀L(j) > L(i), (5.7g)
where (5.7a) initializes the MPC problem, (5.7b) is the discretized state transition equation, (5.7c) sets the bounds for vehicle states and controls, (5.7d) sets the lateral acceleration limits and (5.7e) avoids the singularity. Eq. (5.7f) is the obstacle avoidance constraints written in compact form. Eq. (5.7g) denes the intra-convoy collision avoidance constraints for all vehicles that are prior than vehicle i, with a ∈ {1, 2, 3} a properly chosen index following the rule (5.3). Note that in implementation, (5.7f) and (5.7g) are softened following the procedure described in 2.3.3.

Simulations

We have implemented our framework in the high-delity robotic simulator Webots [START_REF] Michel | Webots: Professional Mobile Robot Simulation[END_REF]. The proposed algorithm is coded in C++ and we use the ACADO toolkit [START_REF] Houska | ACADO Toolkit An Open Source Framework for Automatic Control and Dynamic Optimization[END_REF] to solve the MPC problem. Simulations were performed on a personal computer running on a 3. [START_REF] Kurt | A Multiagent Approach to Autonomous Intersection Management[END_REF]200). The prediction horizon for all vehicles is T i = 5 s. The trajectory re-planning interval is 0.256 s. Thus at time t 0 , a follower has access to the planned trajectory of its leader at time t 0 -0.256.

Q i = diag(1, 2, 0, 100), R i = diag
The parameters for space partition are given as ∆s = 10 m and ∆r = 3 m. To quantify the formation error, we introduce e i as the combination of the longitudinal 5.5. Simulations formation error and the lateral formation error: e i = (e s i ) 2 + (e r i ) 2 .

In the following, we consider two scenarios: obstacle avoidance and dynamic convoy reconguration.

Obstacle avoidance

Scenario description

We consider a triangle-shaped convoy (Fig. 5.1) composed by three vehicles. This convoy design can be used, for instance, for a snowplowing application [START_REF] Ono | Road-Following Formation Control of Autonomous Ground Vehicles[END_REF]. The desired formation remains static during the entire simulation, while vehicles must avoid on-road obstacles, cross narrow corridors, and at the same time avoid collisions with other member vehicles of the convoy.

Simulation results

Fig. 5.5 shows the computed trajectories of three vehicles using our framework. We remark that vehicles are able to quickly form the desired formation, and maintain it in the absence of obstacles. In the vicinity of obstacles (at the 100-meters mark), vehicles are able to swerve around them and even temporarily deform the formation to pass.

Dynamic convoy reconguraiton

Scenario description

The second scenario considers isomorphic formation changes for a four vehicle convoy on a curvy road. The convoy is coordinated both longitudinally and laterally. The

V 0 V 1 V 2 (a) 0 10 20 30 0 5 time (s) error (m) V 1 V 2 (b) 0
V 0 V 1 V 2 (c)

Concluding remarks

Simulation results

Fig. 5.7 presents the trajectories of the four vehicles during the experiment. This simulation demonstrates that isomorphic formation changes can be performed smoothly while avoiding intra-formation collisions. Moreover, the curvy nature of the road is handled nicely by our framework.

Concluding remarks

We have presented a convoy control framework for multiple vehicles on the road.

The proposed framework is composed of a centralized convoy supervisor and multiple local vehicle controllers. The convoy supervisor centrally manages the geometrical structure of the convoy and while each vehicle locally computes a trajectory compatible with the formation control, using information exchanged through communication. We have made use of logical rules to handle intra-formation collision avoidance. Moreover, we have designed a strategy for reconguration between isomorphic formations in a safe and timely manner. High-delity computer simulations have demonstrated the eectiveness of the approach.

1 https://youtu.be/QIGIgCmBr0A

Chapter 6

Control Framework for Autonomous Intersection Management

In previous chapters, we have applied MPC to the motion planning and control of individual vehicles and convoys of vehicles. In this chapter, we apply MPC to coordinate autonomous vehicles at an intersection without trac lights. We propose a hierarchical control architecture with a centralized intersection controller and multiple local planners for individual vehicles. The intersection controller decides the relative crossing orders of vehicles while planners of vehicles compute trajectories that respect the prescribed orders using MPC based techniques in a receding horizon fashion. The proposed system maintains the system-wide safety property even if one or more vehicles suddenly brake. Simulations are performed to illustrate the benets of our approach.

Introduction

Currently, trac lights are installed in many intersections to coordinate conicting trac ows. However, there is a rising concern on the eciency and safety of these systems. Taking advantage of current advances in cooperative autonomous driving technology, studies have been conducted to explore ideas of autonomous intersections without trac lights (Fig. 6.1), as briey presented below.

Planning-based approaches [START_REF] Charalampidis | Speed prole optimization for vehicles crossing an intersection under a safety constraint[END_REF][START_REF] Lee | Development and Evaluation of a Cooperative Vehicle Intersection Control Algorithm Under the Connected Vehicles Environment[END_REF][START_REF] De | Analysis of reservation algorithms for cooperative planning at intersections[END_REF] rst compute collision-free trajectories for all vehicles to cross the intersection without collision; in a second phase, vehicles are controlled to follow these trajectories. In [START_REF] Charalampidis | Speed prole optimization for vehicles crossing an intersection under a safety constraint[END_REF], the optimal speed proles for a two-vehicle intersection are analytically studied assuming simple vehicle behaviors, while the extension to a multi-vehicle intersection is subject to future work. In [START_REF] Lee | Development and Evaluation of a Cooperative Vehicle Intersection Control Algorithm Under the Connected Vehicles Environment[END_REF],

constrained nonlinear optimization techniques are employed to plan trajectories for all vehicles entering the intersection. The control goal is to minimize the total length of overlapped trajectories. However, the complexity of the optimization problem renders the solution hard to obtain. To enable a quick response to changes and uncertainties, reactive approaches [START_REF] Makarem | Micro-simulation Modeling of Coordination of Automated Guided Vehicles at Bibliography Intersection[END_REF][START_REF] Makarem | Model predictive coordination of autonomous vehicles crossing intersections[END_REF][START_REF] Kim | An MPC-based approach to provable system-wide safety and liveness of autonomous ground trac. Automatic Control[END_REF][START_REF] Gabriel | Cooperative receding horizon conict resolution at trac intersections[END_REF] have been proposed. Instead of computing complete trajectories, vehicles calculate their current control decisions with respect to other vehicles' states and environmental information. In [START_REF] Makarem | Micro-simulation Modeling of Coordination of Automated Guided Vehicles at Bibliography Intersection[END_REF], every vehicle uses a navigation function to decide the current control input. The navigation function includes a collision avoidance term which enables a vehicle to respond to maneuvers of other vehicles.

A major diculty of reactive approaches lies in the deadlock avoidance: without global coordination, it is dicult to get a proof that deadlocks are avoided.

In previous work [START_REF] Gregoire | Priority-based coordination of robots[END_REF][START_REF] Qian | Priority based coordination of autonomous and legacy vehicles at intersection[END_REF], a generic priority-based scheme is proposed for coordinating a group of mobile robots on xed and potentially conicting paths. This framework separates the robot coordination problem into two parts: high-level planning of priorities and low-level priority-preserving condition for robot control. Highlevel priority assignment decides the relative priority of any two conicting robots to cross the conicting region. The low-level priority-preserving condition provides an interval of admissible control inputs for each robot that preserves the assigned priorities, taking into account the states of robots. Under this framework, the proposed overall coordination system is proven to be collision-free and deadlock-free.

[L 1 , H 1 ] × [L 2 , H 2 ] ∪ {(s 1 , s 2 ) : |s 1 -s 2 | ≤ d, s 1 > H 1 , s 2 > H 2 }. (d) is the diverging case and the corresponding obstacle region is {(s 1 , s 2 ) : s 1 -s 2 ≤ d, s 1 ≤ H 1 }.
with s i , v i and a i respectively being the position, speed and acceleration of the vehicle i. We let the state ξ i = [s i , v i ] and the control input u i = a i . We bound the speed and the acceleration:

ξ i ∈ Ξ i := [-∞, +∞] × [0, v i ], (6.2a)
u i ∈ U i := [u i , u i ]. (6.2b)
We use u i ∈ U i to denote an admissible input signal, which is a function of time u i : t → u i (t) ∈ U i for t ≥ t 0 and we note U i the space of all admissible control signals. Let ξ i (t, u i , ξ i (t 0 )), s i (t, u i , ξ i (t 0 )) and v i (t, u i , ξ i (t 0 )) respectively denote the state, position and speed of vehicle i at time t, starting from ξ i (t 0 ).

For two vehicles i, j ∈ N , we say that i and j are conicting if γ i ∩ γ j = ∅ and we dene the obstacle region C ij ⊂ R 2 as the set of congurations (s i , s j ) where i and j collide. We assume that the obstacle region is connected. Note that this denition of the obstacle region is not restricted to the case of crossing paths, but can also be used for car-following, merging and diverging paths. In the case of non-intersecting paths, we let C ij = ∅. This formulation can handle general trac intersections with multiple collision points as in Fig. 6.1. Fig. 6.2 illustrates the obstacle regions in various cases.

Control architecture overview

For every pair of vehicles with non-empty obstacle region, one vehicle necessarily passes before the other, which naturally leads to the notion of priority. For cases like Fig. 6.2a or Fig. 6.2d, the initial position of the vehicles implies that vehicle 1 always has priority over vehicle 2. In the other cases (Fig. 6.2b and Fig. 6.2c), a priority order must be determined. We note i j if vehicle i has priority over j.

For the sake of simplicity, in the case where C ij = ∅ we accept either i j or j i as valid, but the choice has no inuence on the behaviors of vehicles. The tracking controllers of vehicles are omitted in the gure for the sake of simplicity.

The intersection controller keeps track of the vehicles that need to coordinate by incorporating newly arrived vehicles and removing departed vehicles. We assume that the intersection controller monitors perfectly the states of vehicles through sensors or V2V communications. The intersection controller assigns priorities to vehicles in the form of a priority list O. Further details will be provided in 6.4.

At low-level, each vehicle is controlled locally to cross the intersection and preserve the assigned priorities. We exploit the exibility of MPC to incorporate Chapter 6. Control Framework for Autonomous Intersection Management where u is the maximal brake control signal. Therefore, if ξ 0 i ∈ B j i (ξ 0 j ) then vehicle i can come to a stop without entering the completed obstacle region even if vehicle j brakes at its maximum for all t ≥ 0. We introduce the denition of brake-safe state: Denition 6.1 (Brake-safe state). Consider a vehicle i, it is in brake-safe state ξ 0 i if and only if, for all j i, ξ 0 i ∈ B j i (ξ 0 j ).

If i is in a brake-safe state (or, more simply, i is brake-safe), it can always stop before violating any of its assigned priorities even if another vehicle brakes at its maximum. However, since vehicles are controlled digitally, their reactions to a brake event will at most have a delay of τ , which is the update interval of the control. We further introduce the denition of brake-safe control as: Denition 6.2 (Brake-safe control). Assuming that i starts in an initial brake-safe

state ξ 0 i , a constant control input u i dened over [0, τ ) such that ∀t ∈ [0, τ ), u i (t) = u i , is said to be a brake-safe control if ∀j i, ξ i (τ, u i , ξ 0 i ) ∈ B j i ξ j (τ, u j , ξ 0 j ) . (6.4)
This condition states that the input u i for [0, τ ) is a brake-safe control if the state of vehicle i at τ under control u i is still brake-safe with regard to any vehicle j i applying a maximal brake command during this period. To simplify the notation, we write (6.4) compactly as

u i ∈ j i U j i (ξ 0 i , ξ 0 j , τ ), (6.5)
where U j i is a set-valued function returning the set of all constant control inputs for [0, τ ) that are brake-safe with regard to vehicle j and j i U j i (ξ 0 i , ξ 0 j , τ ) is the intersection of all such sets for all j i.

If vehicle i is brake-safe, the set (6.5) is a non-empty open interval that contains necessarily u i . The length of this interval corresponds to the leeway vehicle i has in choosing a control.

For assigned vehicles, the brake-safe condition (6.5) must be respected during the entire crossing so that they always respect the assigned priorities.

For unassigned vehicles, default priority relation exists. Notably, an unassigned vehicle should remain brake-safe with regard to the front vehicle if it exists. We denote this condition as

u i ∈ U prev i (ξ 0 i , ξ 0 prev , τ ), (6.6) 90 6

.5. Local vehicle controller

where ξ prev is the state of the preceding vehicle. At the same time, it must maintain brake-safety with regard to the entry of the intersection region: it cannot enter the intersection region unless it is assigned. Let E i be the curvilinear coordinate of the entry of the intersection zone for vehicle i. Maintaining brake-safety with regard to E i can be considered as maintaining brake-safe with regard to a virtual vehicle ν

stationed at E i u i ∈ U ν i (ξ 0 i , ξ 0 ν , τ ), (6.7)
where ξ 0 ν = [E i , 0] is the state of the virtual vehicle.

Remark that the denition of brake-safe control seems to assume that the other vehicles brake at their maximum. However, since the dynamics of the system is monotone [START_REF] Angeli | Monotone control systems[END_REF], condition (6.4) in fact ensures that future states of vehicle i will still be brake-safe regardless of the actual control applied by the other vehicles.

MPC formulation

Let T denote the length of prediction horizon and τ be the update interval of the control. We have T = Kτ with K being the total discretized steps.

We let u k i , v k i , s k i and ξ k i respectively represent the control, speed, position and state of vehicle i at time kτ . The discrete-time state equation can then be given by τ ). The longitudinal trajectory of a vehicle i can be generated using the following MPC formulation:

min u i J i (ξ, u i ) = min u i K k=0 L i (ξ k i , u k i ) (6.9)
subject to ∀k ∈ [0, ..., K], ξ i (0) = ξi , (6.10a) We observe that in both scenarios the system trajectories do not intersect with the obstacle region. Fig. 6.7 and Fig. 6.8 are respectively the speed and acceleration proles for the two scenarios. We observe that the proposed MPC scheme generates smooth longitudinal trajectories for vehicles. Safety is guaranteed in both scenarios.

ξ k i ∈ Ξ i , u k i ∈ U i , (6.10b)
ξ k+1 i = A d x k i + B d u k i , (6.10c
In the second scenario, vehicle 2 and vehicle 3 adapt their speeds when vehicle 1 performs an emergency brake to avoid collisions.

A video is available 1 to illustrate the capacity of the proposed scheme in handling a continuous ow of vehicles using the proposed priority assignment method.

Conclusions and perspectives

In the last decade, autonomous driving has been attracting more and more attentions from both industrial actors and academic institutes thanks to its capability in enhancing trac eciency and reducing accidents. In this thesis, we have considered two major challenges in autonomous driving: how to guide vehicles to proceed in an on-road environment populated with obstacles and governed by trac rules, and how to make autonomous vehicles maneuver cooperatively ?

We have partially dealt with these challenges in our thesis. We started with the rst challenge. We presented a hierarchical control architecture that employs an MPC based motion planner for trajectory generation and another MPC based tracking controller for trajectory tracking. We demonstrated the capability of this design in dierent on-road driving scenario. However, further analysis demonstrated that the motion planner in this design cannot handle an important category of constraints: logical constraints. To cope with this issue, we presented a hybrid MPC based motion planner that is able to handle both dierentiable constraints and logical constraints by formulating the motion planning problem as a Mixed Integer Quadratic Programming problem. We applied the planner to various scenarios (intersection crossing, obstacle avoidance, overtaking, lane change, etc.) to illustrate the advantages of this planner.

We responded to the second challenge with two control designs for cooperative autonomous driving: a control architecture for convoy control of autonomous vehicles and a control architecture for autonomous intersection management. For the formation control problem, we presented a hierarchical framework with a convoy supervisor to manage and recongure the formation and local MPC based controllers to track the formation-keeping reference trajectories while satisfying various constraints. We made use of logical rules to handle intra-formation collision avoidance and proposed a strategy for reconguration between isomorphic formations in a safe and timely manner. For the autonomous intersection management problem, we adopted the priority-based coordination framework and separated the autonomous intersection management problem into a priority assignment problem and a vehicle control problem. A centralized intersection controller is designed to assign priorities to incoming vehicles following a priority assignment policy. Local MPC based Chapter 7. Conclusions and perspectives vehicle planners generate optimal trajectories while respect the assigned priorities.

The proposed design ensures the system to be safe to unexpected events (emergency braking) and allow vehicles to cross the intersection safely.

Perspectives

Hybrid MPC based planner

The hybrid MPC based planner uses a linear point mass model in the formulation of model predictive control problem. This model is suitable if the longitudinal motion dominates the lateral one, for example when driving on highways or on urban arterial roads. However, for some applications, a nonlinear vehicle model might be more desirable. Future work will investigate the applicability of combining the feedback linearizion with the MIQP formulation. Another important assumption in this paper is that the vehicle is driving on road segments with small curvature. For large road curvatures, the current model can be imprecise. This issue should be investigated in the future.

We have not considered in detail the problem of estimating the trajectories of surrounding obstacles. Constant velocity assumption is used in this work. However, uncertainties in estimation can raise from sensor noises and hard-to-estimate intentions of other trac participants. It will be useful to investigate the integration of probabilistic trajectory predictions of obstacles with the motion planner. Finally, the design requires more validation in simulations and eld experiments.

Control framework for convoy

More work can be done to enhance the control design for vehicle convoys. The collision avoidance rules can be further analyzed and enhanced. It will also be useful to consider high-level decision-making process for recongurations of convoys and to investigate algorithms that can compute automatically the formation sequences from an initial formation to a desired one. Finally, we should evaluate this approach using real vehicles and under realistic perception and communication conditions.

Control framework for autonomous intersection management

We adopted a simple Fast First Service policy for priority assignment, more sophisticated policies that take into account parameters like queue lengths or vehicles idling time can be developed. Furthermore, the proposed framework can also be extended to more complex intersection geometries like roundabouts, or multiple intersections.

Finally, eld experiments of the proposed architecture will also be benecial for

Conclusion et perspectives

Ce chapitre conclut la thèse en rappelant les contributions et présentant les perspectives.

Résumé

La conduite autonome a attiré une attention croissante au cours des dernières décennies en raison de son potentiel impact socioéconomique, notamment concernant la réduction du nombre d'accidents de la route et l'amélioration de l'efficacité du trafic. Grâce à l'effort de plusieurs instituts de recherche et d'entreprises, les véhicules autonomes ont déjà accumulé des dizaines de millions de kilomètres parcourus dans des conditions de circulation réelles. Cette thèse porte sur la conception d'une architecture de contrôle pour les véhicules autonomes et coopératifs dans l'optique de leur déploiement massif. La base commune des différentes architectures proposées dans cette thèse est le Contrôle-Commande Prédictif, reconnu pour son efficacité et sa polyvalence. Nous présentons tout d'abord une architecture classique de contrôle hiérarchique, qui utilise la commande prédictive pour planifier un déplacement, puis déterminer un contrôle permettant de suivre cette trajectoire. Toutefois, comme nous le montrons dans un deuxième temps, cette architecture simple n'est pas capable de gérer certaines contraintes logiques, notamment liées aux règles de circulation et à l'existence de choix de trajectoires discrets. Nous proposons donc approche hybride de la commande prédictive, que nous utilisons pour développer une architecture de contrôle pour un véhicule autonome individuel. Nous étudions le problème de contrôler un ensemble de véhicules autonomes circulant en convoi, i.e. maintenir une formation prédéterminée sans intervention humaine. Pour ce faire, nous utilisons à nouveau une architecture hiérarchique basée sur la commande prédictive, composée d'un superviseur de convoi et de contrôleurs pour chaque véhicule individuel. Enfin, nous proposons encore une architecture pour le problème de coordonner un groupe de véhicules autonomes dans une intersection sans feux de circulation, en utilisant un contrôleur d'intersection et en adaptant les contrôleurs des véhicules individuels pour leur permettre de traverser l'intersection en toute sécurité.

Mots Clés

Voiture autonome, commande prédictive, planification de trajectoire, convoi, intersection automatisé Abstract Autonomous driving has been gaining more and more attention in the last decades, thanks to its positive social-economic impacts including the enhancement of traffic efficiency and the reduction of road accidents. A number of research institutes and companies have tested autonomous vehicles in traffic, accumulating tens of millions of kilometers traveled in autonomous driving. With the vision of massive deployment of autonomous vehicles, researchers have also started to envision cooperative strategies among autonomous vehicles. This thesis deals with the control architecture design of individual autonomous vehicles and cooperative autonomous vehicles. Model Predictive Control (MPC), thanks to its efficiency and versatility, is chosen as the building block for various control architectures proposed in this thesis. In more detail, this thesis first presents a classical hierarchical control architecture for individual vehicle control that decomposes the controller into a motion planner and a tracking controller, both using nonlinear MPC. In a second step, we analyze the inability of the proposed planner in handling logical constraints raised from traffic rules and multiple maneuver variants, and propose a hybrid MPC based motion planner that solves this issue. We then consider the convoy control problem of autonomous vehicles in which multiple vehicles maintain a formation during autonomous driving. A hierarchical formation control architecture is proposed composing of a convoy supervisor and local MPC based vehicle controllers. Finally, we consider the problem of coordinating a group of autonomous vehicles at an intersection without traffic lights. A hierarchical architecture composed of an intersection controller and multiple local vehicle controllers is proposed to allow vehicles to cross the intersection smoothly and safely.

3 . 1

 31 Two-level control architecture based on MPC. . . . . . . . . . . . . . 25 3.2 Obstacle classication. . . . . . . . . . . . . . . . . . . . . . . . . . . 26 3.3 Approximation of obstacle region by a parabola. . . . . . . . . . . . 27 3.4 Replanning scheme of the motion planner. . . . . . . . . . . . . . . . 30 3.5 Illustration of the simulation setup for the static obstacle avoidance scenario. EV stands for Ego Vehicle. . . . . . . . . . . . . . . . . . . 33 3.6 Perfect localization. (a) The trajectory of the ego vehicle as well as the predicted trajectories of the motion planner. (b) The vehicle speed and vehicle steering angle during the simulation. . . . . . . . 34 3.7 Imperfect localization with 0.5 m error. (a) The localization signal of the ego vehicle as well as the predicted trajectories of the motion planner. (b) The vehicle speed and vehicle steering angle during the simulation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35 3.8 Illustration of the scenario of dynamic obstacle avoidance. . . . . . . 35 3.9 Dynamic obstacle avoidance. (a) The trajectory of the ego vehicle as well as the predicted trajectories. We mark the positions of the vehicle and the cyclist at six dierent time instants using natural numbers and color codes (lighter color means further time instant). (b) Speed prole and steering prole of the ego vehicle. . . . . . . . List of Figures 3.10 Illustration of the scenario of lane change. . . . . . . . . . . . . . . . 36 3.11 Lane change. (a) The trajectory of the ego vehicle as well as the predicted trajectories. We mark the positions of the vehicle and the slow vehicle at six dierent time instants using natural numbers and color codes (lighter color means further time instant). (b) Speed prole and steering prole of the ego vehicle. . . . . . . . . . . . . . 37 3.12 Computation time for the motion planner and the tracking controller. (a) static obstacle avoidance (perfect localization). (b) dynamic obstacle avoidance. (c) lane change. . . . . . . . . . . . . . . . . . . . . 38 4.1 Illustration of multiple maneuver variants for the white car in an overtaking scenario. Adapted from Fig. 1 of [2]. . . . . . . . . . . . . 42 4.2 Overview of the control architecture. . . . . . . . . . . . . . . . . . 44 4.3 2D linear point mass model. . . . . . . . . . . . . . . . . . . . . . . . 45 4.4 Illustration of the speed bump scenario with the speed bump region marked by gray color. . . . . . . . . . . . . . . . . . . . . . . . . . . 47 4.5 Longitudinal speed prole with respect to the longitudinal oset. Green curves mark the predicted trajectories during the MPC iterations and the blue curve marks the actual trajectory of the vehicle. 52 4.6 Illustrative example of the intersection crossing scenario. The ego vehicle (white) needs to cross the intersection without colliding with the non-controlled vehicles (red and blue cars). We do not consider road priority in this example. . . . . . . . . . . . . . . . . . . . . . . 52 4.7 Illustration of the earliest arrival time and the latest departure time. 53 4.8 Intersection simulation 1: (a) Longitudinal positions of three vehicles as functions of time. (b) Longitudinal speed prole of the ego vehicle as well as the predicted trajectories during MPC iterations. EV -Ego Vehicle, Veh 1 -Vehicle 1, Veh 2 -Vehicle 2. . . . . . . . . . . . . . . 55 4.9 Intersection simulation 2: (a) Longitudinal positions of three vehicles as functions of time. (b) Longitudinal speed prole of the ego vehicle as well as the predicted trajectories during MPC iterations. EV -Ego Vehicle, Veh 1 -Vehicle 1, Veh 2 -Vehicle 2. . . . . . . . . . . . . . . 56 4.10 Illustration of the obstacle avoidance scenario. The obstacle is colored in red. The light-red area is used to take into account the size of the ego vehicle. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57 4.11 Obstacle avoidance scenario: (a) Vehicle trajectory as well as predicted trajectories. (b) Speed prole and steering angle prole. . . . 58 4.12 Illustration of the overtaking scenario. . . . . . . . . . . . . . . . . . 58 vi List of Figures 4.13 Overtaking simulation 1: (a) the trajectory of overtaking as well as the predicted trajectories. We mark the positions of vehicles at six dierent time instants using natural numbers and color codes (lighter color means further time instant). (b) Speed and steering proles. . . 4.14 Overtaking simulation 2: (a) the trajectory of overtaking as well as the predicted trajectories, (b) speed and steering proles. . . . . . . 4.15 Illustration of the lane change scenario . . . . . . . . . . . . . . . . . 4.16 Lane change scenario: (a) The trajectory of lane change as well as predicted trajectories. We mark the positions of vehicles at six different time instants using natural numbers and color codes (lighter color means further time instant). (b) Speed and steering proles. . . 4.17 Statistics of computation time for dierent simulations . . . . . . . . 4.18 Experiments: (a) The trajectory of overtaking as well as the predicted trajectories. We mark the positions of vehicles at six dierent time instants using natural numbers and color codes (lighter color means further time instant).(b) The speed prole and the steering prole. . 5.1 A three-vehicle formation with an obstacle on the road. . . . . . . .

5 . 6 Figure 1 . 1 :

 5611 Figure 1.1: Autonomous vehicles of KIT-Mercedes-Benz (left) and Google (right).

Figure 1 . 2 :

 12 Figure 1.2: Autonomous driving: major components.

Figure 1 . 3 :

 13 Figure 1.3: Multiple maneuver variants in an obstacle avoidance scenario.

Figure 1 . 4 :

 14 Figure 1.4: A convoy formation in GCDC'16. Courtesy of I-GAME project.

Figure 2 . 1 :

 21 Figure 2.1: Illustration of coordinate systems.

Fig. 2 .

 2 Fig.2.1 shows the driving scenario of a vehicle. We introduce x and y as the coordinates of the vehicle in the Cartesian frame; the origin of the Cartesian frame can be set freely.

Figure 2 . 2 :

 22 Figure 2.2: Illustration of the double integrator model.

2. 2 . 2 2D linear point mass model x y Figure 2 . 3 :

 22y23 Figure 2.3: Illustration of the 2D linear point mass model.

Figure 2 . 5 :

 25 Figure 2.5: Kinematic bicycle model.

Figure 2 . 6 :

 26 Figure 2.6: Illustration of an MPC scheme.

Figure 3 . 1 :

 31 Figure 3.1: Two-level control architecture based on MPC.

Figure 3 . 2 :

 32 Figure 3.2: Obstacle classication.

  ) and we require ξ = [-∞, r, 0, e θ ], ξ = [+∞, r, v, ēθ ], u = [a, ω], ū = [ā, ω].

o 2 , 29 Chapter 3 .Figure 3 . 4 :

 229334 Figure 3.4: Replanning scheme of the motion planner.

( 3 .

 3 10g) Eq. (3.10a) initializes the MPC problem. Fig. 3.4 illustrates the initialization procedure. The operator H maps the currently measured state to the spatially closest point on the previously computed trajectory. The MPC problem is then actually initialized from the point on the previous trajectory. This design ensures the reference trajectory to be continuous overtime and reduces the interference between the planner and the controller. Remark that if the previous trajectory does not exist or the vehicle deviates signicantly from the previous trajectory, the motion planner should be initialized from the current position of the vehicle. Eq. (3.10b) is the discretized state equation. Eq. (3.10c) sets the bounds for the state and the control input. Eq. (3.10d) takes into account tire limit by setting the maximal and minimal lateral accelerations. An alternative approach is to consider a tire-friction circle mentioned in 2.2. Eq. (3.10e) is used to circumvent model singularities in the optimization problem where is a small positive constant. Eq. (3.10f) captures all collision avoidance constraints. Note that as discussed in 2.3, we soften the obstacle avoidance constraints to tolerate occasional violation of the constraints due to sensor noise or control imprecision by introducing slack variables k o ≥ 0. The term M ∀o ( k o ) 2 is used to penalize the violation of constraints with M a large constant.

  ) compactly written as ξc = f c (ξ c , u c ). where ξ c = [x, y, v, θ, φ] is the state vector. x and y are the coordinates of the center of mass. v, θ and φ are the speed, heading and the steering angle of the front wheel. The control inputs are u c = [a, ψ], with the rst component being the acceleration and the second one being the steering speed.

  ) where Q c and R c are two positive diagonal matrices with proper dimension. ξ c,ref is the reference trajectory obtained from the planner: ξ * → ξ c,ref . Note that the coordinate systems, the prediction horizons and the discretization steps between the planner and the controller are dierent. Thus we need to rst map ξ * from Frenet frame to Cartesian frame, interpolate the mapped trajectory(e.g. using cubic splines) and then re-discretize it with the discretization resolution of the controller. Chapter 3. Model Predictive Control for Autonomous Driving ξ = [-∞, -3 m, 0 m/s, -0.4 rad], ξ = [+∞, 3 m, 25 m/s, 0.4 rad], u = [-4.5 m/s 2 , -0.4 rad/s], u = [2.5 m/s 2 , 0.4 rad/s], a lat = 1.5 m/s 2 , M = 100000, T = 5.0 s, K = 25, Q = [0, 10, 2, 100], R = [5, 200], ξ c = [-∞, -∞, 0, -∞, -0.5 rad], ξ c = [+∞, +∞, 25 m/s, +∞, 0.5 rad], u c = [-6 m/s 2 , -0.5 rad/s], u c = [4 m/s 2 , 0.5 rad/s], T = 1.6 s, K = 8, Q c = [3, 2, 5, 2], R c = [0.8, 10],

Figure 3 . 5 :

 35 Figure 3.5: Illustration of the simulation setup for the static obstacle avoidance scenario. EV stands for Ego Vehicle.

Figure 3 . 6 :

 36 Figure 3.6: Perfect localization. (a) The trajectory of the ego vehicle as well as the predicted trajectories of the motion planner. (b) The vehicle speed and vehicle steering angle during the simulation.

Figure 3 . 7 :Figure 3 . 8 :

 3738 Figure 3.7: Imperfect localization with 0.5 m error. (a) The localization signal of the ego vehicle as well as the predicted trajectories of the motion planner. (b) The vehicle speed and vehicle steering angle during the simulation.

Fig. 3 .Figure 3 . 9 : 1 Figure 3 . 10 :Figure 3 . 11 :Fig. 3 .Figure 3 . 12 :

 33913103113312 Fig.3.11a illustrates the trajectory of the ego vehicle. We notice that the vehicle rst decelerate to synchronize its speed with the slow vehicle in the front. At t = 10 s,

Figure 4 . 1 :Fig. 4 .

 414 Figure 4.1: Illustration of multiple maneuver variants for the white car in an overtaking scenario. Adapted from Fig. 1 of [2].

  is one of the most relevant papers to our work. They tackle the problem of multiple maneuver variants during autonomous driving by explicitly enumerating all homotopy classes. For each homotopy class, they formulate and solve a MIQP based on a linearized bicycle model to get the local optimum, and they compare all local optima to nd the global one. Nevertheless, to the best of our knowledge, a generic MIP formulation considering realistic vehicle dynamics and capable of handling multiple on-road driving scenarios is not yet available.

Chapter 4 .Figure 4 . 2 :

 442 Figure 4.2: Overview of the control architecture.

Figure 4 . 3 :

 43 Figure 4.3: 2D linear point mass model.

Figure 4 . 4 :

 44 Figure 4.4: Illustration of the speed bump scenario with the speed bump region marked by gray color.

  the vehicle must decelerate to 10 m/s on the speed bump situated in the range x ∈ [40 m, 60 m] (Fig. 4.4). We dene three literals P 1 = [x k ≥ 40], P 2 = [x k ≤ 60] and P 3 = [v k x ≤ 10]; the rule can then be expressed in the form: ∀k ≥ 0, (P 1 ∧ P 2 ) ⇒ P 3 , i.e. if the vehicle's position is within [40 m, 60 m], then the vehicle's longitudinal speed should be lower than or equal to 10 m/s.

4 .

 4 If we must consider non-quadratic cost functions, non-linear vehicle dynamics or non-linear constraints, we can formulate a Mixed Integer Non-linear Program, which, however, is much harder to solve. ξ = [0, 0, -2 m, -3 m/s] T , ξ = [free, 25 m/s, 8 m, 3 m/s] T , u = [-3 m/s 2 , -1 m/s 2 ] T , u = [3 m/s 2 , 1 m/s 2 ]

Table 4 . 1 :

 41 Parameters used for the hMPC-based motion planner in applicative examples 4.4

(4. 10 )

 10 such that the vehicle tracks a constant desired speed prole v ref and a potentially time-varying desired lateral deviation y k ref , k ∈ [0, ..., K p ], while trying to minimize the control eort. We do not assume any desired binary state, and therefore the term ||σ kσ k ref || 2

3 . 2 .

 32 The speed bump conditions are given in(4.6). The initial speed of the vehicle is equal to its desired speed v ref = 15 m/s while on the speed bump (within the interval of[40 m, 60 m]) the maximally allowed speed is 10 m/s.

Figure 4 . 5 :

 45 Figure 4.5: Longitudinal speed prole with respect to the longitudinal oset. Green curves mark the predicted trajectories during the MPC iterations and the blue curve marks the actual trajectory of the vehicle.

Figure 4 . 6 :

 46 Figure 4.6: Illustrative example of the intersection crossing scenario. The ego vehicle(white) needs to cross the intersection without colliding with the non-controlled vehicles (red and blue cars). We do not consider road priority in this example.

Figure 4 . 7 :

 47 Figure 4.7: Illustration of the earliest arrival time and the latest departure time.

(4. 11 )

 11 where T (x, v, a) is a function that computes the time to traverse a distance of x if the current speed is v and the vehicle applies a constant acceleration of a. If the vehicle cannot traverse x in nite time, then the time is set to +∞. The reason of introducing āx,o instead of assuming constant speed of o is to add a reasonable error margin on the motion estimation of non-controlled vehicles.Similarly, we compute the latest departure time to as the latest time instant when the vehicle leaves the intersection, assuming a constant deceleration a x,o < 0 (Fig. 4.7). to = T (H ox o , v x,o , a x,o ).

(

( 4 .

 4 15b) The ego vehicle is required to either cross the intersection before o's earliest arrival time or stay out of the intersection at the end of the prediction horizon. If K ≤ k o ≤ ko , o is still far from intersection, thus we do not add any constraint on the ego vehicle. Eq. (4.14) -Eq. (4.15) are then integrated to the hybrid MPC framework following the procedure of 4.3.3. Note that āx,o and a x,o are two design parameters that can be chosen according to the required level of conservativeness: choosing large absolute values increases the conservativeness of the motion planner.

Figure 4 . 8 :

 48 Figure 4.8: Intersection simulation 1: (a) Longitudinal positions of three vehicles as functions of time. (b) Longitudinal speed prole of the ego vehicle as well as the predicted trajectories during MPC iterations. EV -Ego Vehicle, Veh 1 -Vehicle 1, Veh 2 -Vehicle 2.

Fig. 4 .

 4 Fig. 4.9a illustrates the result. The ego vehicle chooses to yield to both Vehicle 1

Figure 4 . 9 :

 49 Figure 4.9: Intersection simulation 2: (a) Longitudinal positions of three vehicles as functions of time. (b) Longitudinal speed prole of the ego vehicle as well as the predicted trajectories during MPC iterations. EV -Ego Vehicle, Veh 1 -Vehicle 1, Veh 2 -Vehicle 2.

Figure 4 . 10 :

 410 Figure 4.10: Illustration of the obstacle avoidance scenario. The obstacle is colored in red. The light-red area is used to take into account the size of the ego vehicle.

( 4 .

 4 16e) Note that the formulation allows both moving and still obstacles. The conditions (4.16) state that the vehicle must be separated from the obstacle, either by a longitudinal distance L o or laterally by W o .

Fig. 4 .Figure 4 . 11 :Figure 4 . 12 :

 4411412 Figure 4.11: Obstacle avoidance scenario: (a) Vehicle trajectory as well as predicted trajectories. (b) Speed prole and steering angle prole.

Figure 4 . 13 :

 413 Figure 4.13: Overtaking simulation 1: (a) the trajectory of overtaking as well as the predicted trajectories. We mark the positions of vehicles at six dierent time instants using natural numbers and color codes (lighter color means further time instant). (b) Speed and steering proles.

4. 4 .Figure 4 . 14 :

 4414 Figure 4.14: Overtaking simulation 2: (a) the trajectory of overtaking as well as the predicted trajectories, (b) speed and steering proles.

Chapter 4 .Figure 4 . 15 :Fig. 4 .

 44154 Figure 4.15: Illustration of the lane change scenario

(4. 21 )

 21 Constraints (4.19), (4.20) and (4.21) are enforced along with constraints on vehicle dynamics on the formulated MIQP problem. Remark that this formulation considers the vehicle as a point mass. It is thus necessary to have enough safety margin on L o to take into account the shape of the vehicle.

Figure 4 . 16 :

 416 Figure 4.16: Lane change scenario: (a) The trajectory of lane change as well as predicted trajectories. We mark the positions of vehicles at six dierent time instants using natural numbers and color codes (lighter color means further time instant). (b) Speed and steering proles.

Fig 4 . 17 .Figure 4 . 17 :

 417417 Fig 4.[START_REF] Gu | Toward human-like motion planning in urban environments[END_REF]. We observe that the motion planner is able to compute optimal trajectories within 100 ms for the scenarios of speed bump, intersection crossing and obstacle avoidance. For the scenarios of overtaking and lane change, the computation times for the rst 15 s hit regularly the upper-bound of 200 ms. The reason is two-fold: rst, the number of time steps for both scenarios is 30, which leads to a large number of continuous and discrete variables; second, there are many maneuver variants in both scenarios.

Figure 4 . 18 :

 418 Figure 4.18: Experiments: (a) The trajectory of overtaking as well as the predicted trajectories. We mark the positions of vehicles at six dierent time instants using natural numbers and color codes (lighter color means further time instant).(b) The speed prole and the steering prole.

Figure 5 . 1 :

 51 Figure 5.1: A three-vehicle formation with an obstacle on the road.

Figure 5 . 2 :

 52 Figure 5.2: Overview of the convoy control architecture.

Fig. 5 .

 5 Fig. 5.2 gives an overview of the proposed architecture for the convoy control of autonomous vehicles. The system adopts a hierarchical design with a centralized convoy supervisor that denes the structure of the formation and monitors the reconguration of the convoy, and local vehicle controllers that control individual vehicles to maintain the formation and avoid obstacles.

Example 5 . 1 . 2 .Figure 5 . 3 :

 51253 Fig. 5.1. The formation tree G is given as: 0 1 2   

( 5 .

 5 3c) where s d ji = s d js d i and r d ji = r d jr d i are the target relative position of vehicle j from vehicle i in the convoy. The above constraints denes a subspace of m={1...5} A m i . They are linear (and therefore dierentiable).

5. 3 .∪ A 3 0 ∪ A 4 0

 334 Fig. 5.1. The constraint for vehicle 1 can be computed from (5.3) as h 3 0 (s 1 , r 1 ) ≤ 0 such that the safe region (relative to vehicle 0) is A 2 0 ∪ A 3 0 ∪ A 4 0 , forcing vehicle 1 to stay behind vehicle 0. The constraint for vehicle 2 is h 3 0(s 2 , r 2 ) ≤ 0 ∧ h 2 1 (s 2 , r 2 ) ≤ 0, such that vehicle 2's safe region is (A 2 0 ∪ A 3 0 ∪ A 4 0 ) ∩ (A 3 1 ∪ A 4 1 ∪ A 5 1 ): vehicle 2 must

Figure 5 . 4 :

 54 Figure 5.4: A sequence of 1-step reachable isomorphic transformations.

4

 4 GHz Intel Core i7 CPU with 32GB of RAM. In all simulations, vehicles are equipped with noise-free localization systems and delay-free communication devices. The desired speed of the formation for both scenarios is given as v F = 6 m/s. Vehicle parameters are given as: 0 ≤ v i ≤ 10 m/s, |a i | ≤ 2.5 m/s 2 , |e θ,i | ≤ 0.4 rad and āi,lat = 2.5 m/s 2 . The parameters used for the leader are Q 0 = diag(0, 4, 2, 100), R 0 = diag(1, 200). The parameters used for the followers are

Fig. 5 .Figure 5 . 5 :

 555 Figure 5.5: First scenario: trajectories of three vehicles.

Figure 5 . 6 :

 56 Figure 5.6: First scenario: (a) vehicle speeds, (b) formation error, (c) computation time.

Figure 5 . 7 :

 57 Figure 5.7: Second scenario: trajectories of four vehicles.

Figure 6 . 1 :

 61 Figure 6.1: Illustration of an intersection

s 2 s 1 C 12 C 2 1 Figure 6 . 3 :Fig. 6 .

 1121636 Figure 6.3: Completed obstacle region for 2 1 of Fig. 6.2f.

Fig. 6 .

 6 Fig.6.6 illustrates the system trajectories of two scenarios in the position space.

94 6Figure 6 . 6 :Figure 6 . 7 :Figure 6 . 8 :

 94666768 Figure 6.6: Position space of three vehicles in the simulation. Subgure (a) shows the system trajectory as well as the obstacle region in the normal driving case, subgure (b) shows the system trajectory when the vehicle 1 performs an emergency brake.The interval of emergency brake is colored in red.

  understanding the impacts from noisy perception, unreliable communication and nonlinear vehicle dynamics on the autonomous intersection management. en sens inverse ou la prise de décision optimale. Les résultats de simulation et les expériences de terrain démontrent l'ecacité de l'approche et son applicabilité en temps réel.Cadre de contrôle pour convoiDans ce chapitre, on étudie le contrôle coopératif de la formation de plusieurs véhicules autonomes dans un environnement routier. On présente un cadre hiérarchique qui utilise une approche fondée sur la commande prédictive. Le cadre utilise un superviseur global de convoi pour gérer la formation et les contrôleurs de véhicules locaux pour suivre les trajectoires de référence qui maintiennent la formation et satise diverses contraintes. Le cadre proposé sera validé à l'aide de simulations de haute délité.Cadre de contrôle pour la gestion d'intersection automatiséeOn applique la commande prédictive pour coordonner des véhicules autonomes à une intersection sans feux de circulation. On propose une architecture de contrôle hiérarchique avec un contrôleur d'intersection et plusieurs planicateurs locaux pour les véhicules individuels. Le contrôleur d'intersection décide des ordres de traversée pour des véhicules tandis que les planicateurs de véhicules calculent des trajectoires qui respectent les ordres prescrits. Le système proposé maintient la propriété de sécurité à l'échelle du système même si un ou plusieurs véhicules freinent brusquement. Des simulations sont eectuées pour illustrer les avantages de notre approche.

  Chapter 3. Model Predictive Control for Autonomous Driving proximation method used in this chapter. For an arbitrary NBO, we rst adopt the heuristic to assign it to the nearest border of the road. Proper margins are then added to the NBO to take into account the size of the vehicle and convert the (potentially irregular) obstacle region into an enhanced obstacle region of rectangle shape. Finally the enhanced obstacle region is approximated using a continuous and dierentiable parabolic constraint that crosses two vertices of the enhanced obstacle region. For an obstacle aected to the lower lane boundary, the constraint is given as

	Obstacle	Margin	Parabola
	Figure 3.3: Approximation of obstacle region by a parabola.
	A vehicle is able to swerve to avoid NBOs. However, in many cases, NBOs cre-ate multiple maneuver variants, e.g., a vehicle may swerve in clock-wise or counter
	clock-wise manner. This problem is widely known as the combinatorial nature of
	autonomous driving [2]. Moreover, the shapes of obstacles are usually irregular

consider the obstacle modeling in the road-following Frenet frame. Dierent models apply for LBOs and NBOs. Let o denote an obstacle and p o = [s o , r o ] be the (potentially time-varying) Frenet coordinate of the obstacle. A vehicle must keep a safe distance from LBOs to avoid rear-end crashes unless it changes its lane. A simple linear constraint can be dened as s + vT gap ≤ s o , (3.1) where T gap is the desired time gap. and non-smooth. Therefore, approximations are required to incorporate obstacle avoidance conditions into the nonlinear MPC scheme. Fig. 3.3 illustrates the ap-

Table 3 .

 3 1 captures the parameters that are common in all test cases. Vehicle length is set to 4 m and width 3 m. The motion planner replans every 256 ms and the tracking controller recalculates the control input every 32 ms. Scenario-specic parameters will be introduced respectively in each case study.

	3.6.1 Static NBO avoidance
	Scenario description
	We consider the problem of avoiding static NBOs during high speed autonomous
	driving on a straight road segment. The goal is to illustrate the capacity of the

proposed architecture to handle static obstacles. The motion planner is congured to stay in the road centerline if possible and to track a constant speed prole of

  4.12) Since the dynamics of the ego vehicle is discretized with a time step duration τ , Integrating Logical Constraints we introduce two integer variables k o and ko as We formulate the safety constraints for the ego vehicle. For each non-controlled vehicle, we introduce a binary variable δ o ∈ {0, 1}. The planner of the ego vehicle runs with a horizon K, if k o ≤ ko ≤ K, we let

	k o = ko =	t o τ to τ	, ,	(4.13a) (4.13b)

  , s 2 ) : s 1s 2 ≤ d}, where d is the minimum separation of two vehicles on the same path. (b) is the crossing case and (f ) is the corresponding obstacle region given as[L 1 , H 1 ] × [L 2 , H 2 ], where [L 1 , H 1 ] is the interval on the path γ 1

		Chapter 6. Control Framework for Autonomous Intersection Management
				1
	2	1	2		2	1
			1	2
		(a)	(b)	(c)		(d)
	s 2	s 2		s 2	s 2
	C 12		C 12	C 12	C 12
		s 1	s 1	s 1		s 1
		(e)	(f )	(g)		(h)
	Figure 6.2: (a) -(d) are illustrations of interactions of paths that we considered in
	this paper. (e) -(h) are the corresponding obstacle regions. (a) is the case of two
	vehicles on the same path and (e) is the obstacle region corresponding to (a), given
	as {(s 1

where collision with vehicle 2 may occur. (c) is the merging case and (g) is the corresponding obstacle region given as

  )

	u k i ∈	j i	U j i (ξ k i , ξ k j , τ ), if the vehicle is assigned,	(6.10d)

u k i ∈ U prev i (ξ k i , ξ k prev , τ ) U ν i (ξ k i , ξ k ν , τ ), if

the vehicle is unassigned, (6.10e) Chapter 6. Control Framework for Autonomous Intersection Management

http://www.gcdc.net/en/

Chapter 4. Model Predictive Control for Autonomous Driving

Integrating Logical Constraints only consider the planner in this chapter, we omit p in the following paragraph to simplify the notations. The vehicle dynamics can be compactly written using the following linear dierential equation:

where 0 is a zero matrix with proper dimensions.

Discretizing the vehicle dynamics with a time step duration of τ leads to the following equation:

To take into account the dynamic limitations of the vehicle, bound constraints are enforced on the state and control signals as

with the bounds dened as ξ = [0, 0, y, v y ] T , ξ = [+∞, v x , y, v y ] T , (

u = [a x , a y ] T , ū = [a x , a y ] T .

(4.4b)

We enforce the following condition to couple the longitudinal and the lateral dynamics

Remark that this point-mass model is widely used at the planning stage [START_REF] Gao | Model Predictive Control for Autonomous and Semiautonomous Vehicles[END_REF][START_REF] Nilsson | Receding horizon maneuver generation for automated highway driving[END_REF] for normal on-road driving as it is conceptually simple while capturing most of the vehicle dynamics. In fact, if the constraints (4.3) and (4.5) are properly chosen, we can reconstruct all vehicle states of a bicycle model using the dierential atness theory [START_REF] Fliess | Flatness and defect of non-linear systems: introductory theory and examples[END_REF]. On the other hand, this model is no longer feasible if the vehicle is operating near its limit, for instance when executing an emergency maneuver.

A common workaround is to design an emergency controller which overrides the planner in this situation. Here we only consider normal driving situation where the Consider a vehicle i, recall the nonlinear point-mass model used for motion planning:

.

(5.4d)

] as the state vector. v i is the vehicle speed and e θ,i is the heading error with respect to the centerline γ. The control inputs are

with the rst component being the acceleration and the second one being the yaw rate. We compactly write the model as ξi = f (ξ i , u i ).

Let T be the prediction horizon of local planners and K be the number of steps.

Consider the following cost function in least-square form:

where Q i and R i are two positive diagonal matrices of proper dimensions. The reference trajectory ξ ref,i is calculated using the following procedure. Consider an arbitrary pair of vehicles (j, i) such that g ji = 1. A communication link can be established between j, i such that vehicle i periodically receives information on the planned trajectories of vehicle j. Assume that at time t the most up-to-date trajectory for vehicle j received by vehicle i is ξ j ([t 1 , t 1 + T ]), with t 1 < t: the trajectory ξ j ([t, t + T ]) can be simply estimated by simulating the trajectory from t 1 + T to t + T using the last value of the control. Under the assumption that the communication delay is small, we expect the estimated trajectory ξ j ([t, t + T ]) to remain close to the actually planned trajectory of vehicle j. The desired position of i relative to j can then be calculated as

(5.6a)

(5.6b)

Finally, the reference trajectory ξ i,ref can be obtained by osetting the position components of ξ j ([t, t + T p ]) by s d ij and r d ij and setting other components to zero.

The MPC for planning is formulated as 6.2. System model However, robots in this work are controlled using a simple "bang-bang" control law, which is fuel inecient and maybe uncomfortable, and as such, unsuitable for autonomous driving.

In this chapter, we combine the priority-based framework with model predictive control to smoothly coordinate autonomous vehicles at intersection. The proposed approach inherits the provably-safe and deadlock-free properties of the prioritybased framework, and produces smooth longitudinal trajectories for each individual vehicle.

The rest of the chapter is articulated as follows. 6.2 presents the system model of autonomous intersection. 6.3 provides an overview of the hierarchical control architecture. In 6.4, we present the design of the high-level intersection controller.

In 6.5, we present the motion planning for individual vehicles; 6.7 presents the simulation results. Finally, 6.8 concludes the chapter.

System model

Consider a collection of N = {1, ..., N } autonomous vehicles approaching an intersection as shown in Fig. 6.1. The proximity of the intersection is conceptually divided into two region: the coordination region and the intersection region. We assume that vehicles are coordinated within the coordination region. The intersection region is a subset of the coordination region, where only authorized vehicles can enter. For each vehicle i ∈ N , we make the following assumptions:

1. a predetermined path γ i ∈ R is given and is perfectly followed;

2. the velocities of vehicles are always non-negative;

3. perfect communication links can be established with other vehicles and with a roadside unit called the intersection controller;

4. the current states of vehicles can be acquired (through sensors and/or communication);

Let s i ∈ R be the curvilinear coordinate along the path for vehicle i. We ignore the lateral dynamics of the vehicle and use the double integrator model to describe the longitudinal dynamics: priority-preserving constraints into the formulation of the optimization problem to ensure the respect of priorities at the planning level. The detailed formulation will be provided in 6.5. The states of vehicles are observed by, or communicated to the intersection controller and other vehicles.

Intersection controller

The intersection controller coordinates vehicles inside the coordination region using priorities. Outside the coordination region, vehicles are assumed to drive safely by other means such as Adaptive Cruising Control (ACC), Cooperative Adaptive

Cruising Control (CACC), etc. A convenient way to encode the priority assignments is by using a priority graph, dened as an oriented graph whose vertices are the vehicles i ∈ N and where the directed edge i → j exists if and only if i j. It is shown in [START_REF] Gregoire | Priority-based coordination of robots[END_REF] that using a general oriented graph as a priority graph may cause deadlocks if the graph has cycles. One possible workaround is to rst design a priority graph, then use a strongly connected component detection algorithm like

Tarjan's algorithm [START_REF] Endre | Data structures and network algorithms[END_REF] to detect and remove the potential cycles. In this work, we use a simpler alternative approach to avoid cycles in the graph: instead of designing a priority graph from scratch, we choose a permutation of the vehicles of N and use it as an ordered list of the vehicles, O. Noting σ(i) and σ(j) the respective position of i and j in O, we then construct a priority graph by adding the edge i → j to the graph (and therefore the priority i j) if and only if σ(i) < σ(j). In other words, O encodes the order in which the vehicles are allowed to cross the intersection. Note that for the general case of N vehicles, there are N ! possible graphs that can be constructed using this algorithm, all of them acyclic.

The intersection controller maintains a simple nite-state machine for each ve- The priority assignment policy that enables the coordination of vehicles at an intersection is essentially the brain of the intersection controller. The design of an optimal policy, however, is still an open problem. In [START_REF] Altche | Analysis of optimal solutions to robot coordination problems to improve autonomous intersection management policies[END_REF], the time optimal priority assignment policy is found using Mixed Integer Linear Programming, assuming that we have full control of vehicle trajectories. However, the assumption is not valid in this work as each vehicle computes its own trajectory in a receding horizon fashion under the priority-preserving constraints.

In practice, we nd that carefully-designed heuristic policies provide satisfactory performance. We adopt the Fast First Service (FFS) policy adapted from [START_REF] Gregoire | Priority-based coordination of robots[END_REF].

For each unassigned vehicle, we simulate its behavior assuming it is added at the end of the priority list. If, in this case, the vehicle can cross the intersection without braking while respecting all priorities, then we consider this vehicle as a fast vehicle and eectively append it to the priority list. Note that it is possible that there is no fast vehicle at a given time; in this case, the controller simply waits without adding new vehicles to the priority list. The FFS policy favors the vehicles that require to stay in the intersection region for a minimum duration, thus increasing the eciency of the system.

6.5

Local vehicle controller

Priority-preserving condition

The priority-preserving condition is deduced in this section. For a pair of vehicles i, j ∈ N , j being in a state ξ 0 j at the current time t = 0, we dene a set-valued function:

Chapter 6. Control Framework for Autonomous Intersection Management

where J i denotes the cost function to be minimized by selecting a proper sequence of control input u k i , k = 0, ..., K. L i denotes is running cost during the interval τ .

We may select a quadratic running cost as

where the rst term is the eciency cost (gap between the current speed and the target speed) and the second term penalizes the control signal. We may consider other metrics to evaluate the cost rather than the one proposed here. (6.10a) and (6.10b) respectively dene the initial condition and the boundary constraints.

Equation (6.10c) is the state transition constraint that describes the time-dependent evolution of the system. We enforce the priority-preserving constraint at each time step k = 0, ..., K in (6.10d) and (6.10e).

Remark that it is not necessary to enforce the priority-preserving constraint at every time step in the prediction horizon, because only the constraint at the rst time step is required to guarantee the output to be priority-preserving. However, we opt to enforce constraints at the future time steps since it allows the vehicle to react earlier to possible priority violations in the future. Such enforcement requires the knowledge of ξ k j for k ∈ [0, ..., K]. We have assumed that each vehicle knows the current states of its prior vehicles. The future states of prior vehicles can be estimated by using prediction models, or simpler, by exchanging intentions (previously computed trajectories) using V2V communication.

Theoretic results for the proposed design

The proposed control architecture has the following properties: Theorem 6.1 (Recursive feasibility). The optimization problem (6.9) is recursively feasible if the current vehicle state is brake-safe with regard to prior vehicles.

The proof is intuitive. A vehicle in brake-safe state ensures that there exists a feasible control input at time τ regardless of other vehicles' maneuvers. Thus the MPC problem will also be feasible at time τ . Theorem 6.2 (Robust safety). Even if one or more vehicles fail to respect the computed trajectories and perform emergency brakes, the system is still safe.

The reason is that the maximal brake command u i is a brake-safe control. Thus the system remains safe. Detailed proof is available in [START_REF] Gregoire | Priority-based coordination of robots[END_REF].

Theorem 6.3 (Deadlock-freeness). All vehicles eventually quit the intersection.

Simulation

In [START_REF] Gregoire | Priority-based coordination of robots[END_REF], it is shown that if there is no cyclic priority relations, then the system is deadlock-free. Since we use a cycle-free priority list O to encode the priority relations, then no deadlock will occur.

Simulation

Scenario description We illustrate the approach through simulation with a simple intersection illustrated in Fig. 6.5. We consider a system of three vehicles labeled by numbers 1, 2 and 3. Vehicle 1, 2 and 3 drive respectively with the direction of West-East, South-North and North-South. We determine the priorities as 1 2 and 1 3. The priority relation between vehicle 2 and vehicle 3 are not relevant since their paths do not intersect. Vehicles start at position s 1 = s 2 = s 3 = 0. Their dynamics are supposed to be identical such that V i = [0, 8] m/s and U i = [-3, 2] m/s 2 . The initial velocities of three vehicles are set to 8 m/s. The parameters of the cost function are set to v d i = 8 m/s, q i,1 = 1 and q i,2 = 3.

Simulation results

In the simulation, the duration of a time step is set to τ = 0.2 s and the prediction horizon is set to T = 3 s.

We run simulations with two dierent scenarios: in the rst scenario vehicle 1 crosses the intersection at its desired speed; in the second scenario, vehicle 1 performs an emergency braking with u i = -3 m/s 2 between t = 2.6 s and t = 5.2 s.

We have presented a hierarchical control architecture for autonomous intersections.

The architecture is comprised of a centralized intersection controller for priority assignment and local motion planners for individual vehicles that are congured to respect the priorities. The integration of priority-based framework with MPC ensures individual vehicles to have smooth trajectories and the system to be deadlock-free and robustly safe. Commande prédictive pour la conduite autonome

Résumés en Français