Lateral Model Predictive Control for Autonomous Heavy-Duty Vehicles

Home , DMC DeLorean, Elon Musk

kth royal institute of technology

Licentiate Thesis in Electrical Engineering Lateral Model Predictive Control for Autonomous Heavy-Duty Vehicles

Sensor, Actuator, and Reference Uncertainties

GONÇALO COLLARES PEREIRA

Stockholm, Sweden 2020

Lateral Model Predictive Control for Autonomous Heavy-Duty Vehicles

Sensor, Actuator, and Reference Uncertainties

GONÇALO COLLARES PEREIRA

Academic Dissertation which, with due permission of the KTH Royal Institute of Technology, is submitted for public defence for the Degree of Licentiate of Engineering on Tuesday the 15th September 2020 at 10:00 a.m. in Harry Nyquist at KTH Royal Institute of Technology, Malvinas väg 10, Stockholm.

Licentiate Thesis in Electrical Engineering KTH Royal Institute of Technology Stockholm, Sweden 2020 © Gonçalo Collares Pereira

ISBN: 978-91-7873-580-8 TRITA-EECS-AVL-2020:38

Printed by: Universitetsservice US-AB, Sweden 2020 Abstract

Autonomous vehicle technology is shaping the future of road transportation. This technology promises safer, greener, and more efficient means of transportation for everyone. Autonomous vehicles are expected to have their first big impact in closed environments, such as mining areas, ports, and construction sites, where heavy-duty vehicles (HDVs) operate. Although research for autonomous systems has boomed in recent years, there are still many challenges associated with them. This thesis addresses lateral motion control for autonomous HDVs using model predictive control (MPC). First, the autonomous vehicle architecture and, in particular, the control module architecture are introduced. The control module receives the current vehicle states and a trajectory to follow, and requests a velocity and a steering-wheel angle to the vehicle actuators. Moreover, the control module needs to handle system delays, maintain certain error bounds, respect actuation constraints, and provide a safe and comfortable ride. Second, a linear robust model predictive controller for disturbed discrete-time nonlinear systems is presented. The optimization problem includes the initial nominal state of the system, which allows to guarantee robust exponential stability of the disturbance invariant set for the discrete-time nonlinear system. The controller effectiveness is demonstrated through simulations of an autonomous vehicle lateral control application. Finally, the controller limitations and possible improvements are discussed with the help of a more constrained autonomous vehicle example. Third, a path following reference aware MPC (RA-MPC) for autonomous vehicles is presented. The controller makes use of the linear time-varying MPC framework, and considers control input rates and accelerations to account for limitations on the vehicle steering dynamics and to provide a safe and comfortable ride. More- over, the controller includes a method to systematically handle references generated by motion planners which can consider different algorithms and vehicle models from the controller. The controller is verified through simulations and through experiments with a Scania construction truck. The experiments show an average lateral error to path of around 7 cm, not exceeding 27 cm on dry roads. Finally, the nonlinear curvature response of the vehicle is studied and the MPC prediction model is modified to account for it. The standard kinematic bicycle model does not describe accurately the lateral motion of the vehicle. Therefore, by extending the model with a nonlinear function that maps the curvature response of the vehicle to a given request, a better prediction of the vehicle’s movement is achieved. The modified model is used together with the RA-MPC and verified through simulations and experiments with a Scania construction truck, where the improvements of the more accurate model are verified. The experiments show an average lateral error to path of around 5 cm, not exceeding 20 cm on wet roads.

Sammanfattning

Autonoma fordon förväntas f˚aen stor inverkan p˚aframtidens transporter av gods och personer. En teknologi som lovar säkrare,grönareoch effektivare transporter till alla. Den typ av verksamhet som autonoma fordon förstförväntas f˚aett störregenomslag inom ärtransporter i avskilda omr˚aden,s˚asom gruvomr˚aden,hamnar och byggplatser. Aven¨ om forskning kopplat till autonoma system har exploderat under den senaste ˚aren kvarst˚arfortfarande ett flertal fr˚ageställningar. Denna avhandling fokuserar p˚alateral rörelsestyrning av tunga autonoma fordon med modellprediktiva regulatorer (MPC). Avhandlingen best˚arav fyra huvuddelar. I förstdelen introduceras det autonoma fordonets systemarkitektur, med fokus p˚aregulatormodulen. Regulatormodulen gener- erar hastighet och rattvinkel referenser till fordonets hastighetaktuator och rattvinkelak- tuator baserat p˚afordonets nuvarande tillst˚andsamt den givna referensbanan som fordonet skall följa.Regulatormodulen behöver dessutom hantera fördröjningari systemet, säkerställaatt systemet inte överskrider givna felmarginaler, hantera aktuator och system- begränsningar,och sist men inte minst framförafordonet p˚aett säkert och komfortabelt sätt. I andra delen presenteras en robust modellprediktiv regulator för ett tidsdiskret olinjärt system med störningar.I optimeringsproblemet inkluderas systemets nominella initialtill- st˚and,detta möjliggörgaranterad robust exponentiell stabilitet fördet tidsdiskreta olinjära systemets störningsinvarianta tillst˚andsmängd.Regulatorns prestanda visas genom simuleringar av ett autonomt fordon därregulatorn kontrollerar fordonets laterala rörelse. Begränsningaroch potentiella förbättringarav regulatorn diskuteras utifr˚anexempel med ökadebegränsningar. I tredje delen presenteras en referens medveten modellprediktiv regulator (RA-MPC), en regulator utvecklad föratt styra ett autonomt fordon längsen given referensbana. Regulator baseras p˚aen linjärttidsvarierande MPC och begränsningari fordonets styrdy- namik hanteras genom att beräknadessa baserat p˚ain insignalernas, referensbana, värden och derivator. Genom att beakta begränsningarnap˚adetta sätt möjliggörsen komfortabel och säker körning.En systematisk metod föratt hantera referensbanor som genererats av rörelseplanerarebaseras p˚aalgoritmer och modeller som skiljer sig fr˚ande som används i regulatorn presenteras ocks˚a.Den metoden äräven implementerad i regulatorn. Regu- latorn har utvärderatsmed s˚avälsimuleringar som tester. Testerna har genomförtsi en Scania lastbil av anläggningstyp. Experimenten visade p˚aen lateral avvikelse fr˚anrefer- ensbana p˚a7 cm i genomsnitt och en maximal avvikelse p˚a27 cm d˚afordonet kördes p˚a torr asfalt. I den sista delen studeras olinjärrespons i fordonets kurvaturreglering och hur detta kan hanteras i MPC’ns prediktions modell av fordonet presenteras ocks˚a.En prediktions modell baserad p˚aen standard kinematisk cykelmodell beskriver inte fordonets laterala rörelsetillräckligt bra fördet studerade systemet. Dock, genom att utvidga modellen med en funktion som mappar fordonets respons mot en given kurvaturbegärankan noggrannhet av fordonets rörelseförbättras.Modellen tillsammans med RA-MPC utvärderadesgenom simuleringar och tester. Testerna har genomförtsi en Scania lastbil av anläggningstyp. Utvärderingenvisade att den introducerade modellen gav en förbättradprecision. Exper- imenten visade p˚aen lateral avvikelse fr˚anreferensbanan p˚a5 cm i genomsnitt och en maximal avvikelse p˚a20 cm d˚afordonet kördesp˚av˚atasfalt.

To my parents and brother

Acknowledgments

There are many who I have to thank for the work presented in this thesis. First, I would like to thank Jonas M˚artensson,for giving me the opportunity to work at the Division of Decision and Control at KTH, for his guidance, and the endless discussions we have. A special thanks goes to one of my co-supervisors Henrik Pettersson, for all his advice and knowledge, and a never ending patience that puts up with me wanting to run just one more experiment. Not to mention the good taste in music that lightens the mood in the vehicles. I am also thankful to Bo Wahlberg, who always grounds me and makes me see the bigger picture giving suggestions and ideas. A big thanks goes to two of my colleagues Pedro Lima and Rui Oliveira. Not only have I collaborated academically with them, but also they are responsible for making my life in Sweden easier and better. A thank you also to Scania and all my colleagues. In particular, I would like to thank Lars Hjorth, Assad Alam, and Kalle Fagerberg for giving me the chance to work at Scania. Moreover, thanks to all the people of the autonomous transport solutions research department that have contributed and developed the vehicle used in the experiments. A special big thanks to the motion planning and control group with whom I had the pleasure of interacting daily. Together with Scania, I would like to thank the Wallenberg Artificial Intelli- gence, Autonomous Systems, and Software Program (WASP) funded by the Knut and Alice Wallenberg Foundation, for financing my project and this research. Furthermore, I would like to acknowledge all my friends and colleagues that have been with me for the past years. From the trips with WASP, to life in Stockholm, and to office work at KTH and Scania, I have always enjoyed the good company, the good mood, and the good discussions. I would like to thank Anamarija, Henrik, Jezdimir, Jonas, Manuel, Oskar, Pedro L., Pedro R., Rui, Sebastião,and Truls for proofreading parts of this thesis, and Henrik for helping me translate the Swedish abstract. Last but not least, I am forever grateful to my family and all my friends around the world that have constantly encouraged me and helped me throughout this journey. To all of you, Thank You!

Gon¸caloCollares Pereira June 2020.

Contents

Contents xi

Acronyms xv

Notation xvii

1 Introduction 1 1.1 Motivation ...... 2 1.1.1 Environment Motivation ...... 4 1.1.2 Health and Safety Motivation ...... 5 1.1.3 Economic Motivation ...... 7 1.2 Autonomous Vehicles ...... 10 1.2.1 Applications ...... 11 1.2.2 Challenges ...... 13 1.3 Problem Statement ...... 18 1.3.1 Thesis Outline and Contributions ...... 20 1.3.2 Other Publications by the Author ...... 22

2 Background 25 2.1 Autonomous Vehicle Modules ...... 25 2.1.1 Route Planning ...... 25 2.1.2 Perception ...... 27 2.1.3 Localization ...... 29 2.1.4 Situation Awareness ...... 30 2.1.5 Motion Planning ...... 33 2.1.6 Motion Control ...... 40 2.2 Control and Optimization Fundamentals ...... 76 2.2.1 Controllability, Reachability, Invariance, and Stability . . . . 77 2.2.2 Convexity and Sets ...... 80 2.2.3 Optimization Fundamentals for Control ...... 81 2.3 Summary ...... 84

xi xii CONTENTS

3 Control Module Architecture 87 3.1 Delay Prediction ...... 89 3.2 Path Sampling ...... 90 3.2.1 Circular Arc Projection ...... 90 3.2.2 MPC Horizon ...... 93 3.3 Constraints Setup ...... 95 3.4 Longitudinal Control ...... 97 3.5 Lateral Control ...... 97 3.6 Output Conversion ...... 97 3.7 Conclusion ...... 98

4 Linear Time-Varying Robust Model Predictive Control 99 4.1 Problem Statement ...... 100 4.2 Controller Formulation ...... 101 4.2.1 Terminal Weights Computation ...... 105 4.2.2 Assumptions Summary ...... 107 4.2.3 Controller Design Summary ...... 107 4.2.4 Stability Analysis ...... 108 4.3 Application: Autonomous Driving ...... 111 4.3.1 Vehicle Model ...... 111 4.3.2 Simulation ...... 114 4.4 Limitations of the Approach ...... 115 4.5 Conclusion ...... 123

5 Reference Aware Model Predictive Control 125 5.1 Problem Statement ...... 126 5.2 Controller Formulation ...... 128 5.2.1 LTV-MPC ...... 128 5.2.2 Controller Design ...... 129 5.2.3 Controller References ...... 132 5.2.4 Reference Aware MPC ...... 134 5.3 Vehicle Model ...... 136 5.3.1 Road-Aligned Spatial-Based Kinematic Bicycle Model . . . . 136 5.3.2 Vehicle Curvature Response Modeling ...... 138 5.4 Results ...... 141 5.4.1 Performance ...... 142 5.4.2 Scenario ...... 142 5.4.3 Controller Setup ...... 143 5.4.4 Simulations ...... 144 5.4.5 Experiments ...... 148 5.5 Conclusion ...... 153

6 Conclusions and Future Work 155 6.1 Conclusions ...... 155 CONTENTS xiii

6.2 Future Work ...... 157

Appendices 161

A MPC Feasibility and Stability 163

B QP Formulation 165

Bibliography 167

Acronyms

4WS 4 Wheel-Steering A* A-star algorithm ACC Adaptive Cruise Control ADAS Advanced Driver-Assistance Systems ARE Algebraic Riccati Equation CACC Cooperative Adaptive Cruise Control CAV Connected and Autonomous Vehicle CC Cruise Control CL-RRT Closed-Loop Rapidly-Exploring Random Trees algorithm EKF Extended Kalman Filter eMPC Explicit MPC EU European Union EV Electric Vehicles FOV Field of View GCDC Grand Cooperative Driving Challenge GDP Gross Domestic Product GHG Green House Gases GNSS Global Navigation Satellite System GPS Global Positioning System GPU Graphics Processing Unit HDV Heavy-Duty Vehicle HIL Hardware In the Loop IMU Inertial Measurement Unit LDV Light-Duty Vehicle LMPC Linear MPC LPV Linear Parameter-Varying LQ Linear Quadratic

xv xvi

LQE Linear Quadratic Estimation LQR Linear Quadratic Regulator LTI Linear Time-Invariant LTV Linear Time-Varying LiDAR Light Detection And Ranging MDP Markov Decision Processes MIQP Mixed-Integer Quadratic Programming MPC Model Predictive Control mRPI Minimal Robust Positively Invariant NMPC Nonlinear Model Predictive Control PID Proportional-Integrative-Derivative PPC Pure-Pursuit Control QP Quadratic Programming RC Car Radio-Controlled Car RPI Robust Positively Invariant RRT Rapidly-Exploring Random Trees algorithm RTI Real-Time Iteration RTK Real Time Kinematic RaDAR Radio Detection And Ranging SMC Sliding Mode Control SQP Sequential Quadratic Programming SoNaR Sound Navigation and Ranging US United States V2I Vehicle-To-Infrastructure V2V Vehicle-To-Vehicle WASP Wallenberg Artiﬁcial Intelligence, Autonomous Systems, and Software Program WHO World Health Organization Notation

z system states. u system control inputs. f(z, u) system dynamics. zr system states reference. ur system control inputs reference. z˘ z˘ = z − zr, system error with respect to the reference. ai|t variable a at time instant t + i predicted at time instant t. N number of prediction horizon points. T ut ut = [u0|t, u1|t, ..., uN−1|t] . T zt zt = [z0|t, z1|t, ..., zN|t] . N N0 set of all natural numbers from 0 to N.

N0, + set of all positive natural numbers including zero. R set of all real numbers.

R0, + set of all positive real numbers including zero. d(z, S) infimum Euclidean distance between point z and any other point in set S, i.e., d(z, S) = inf{|z − s| | s ∈ S}. ρ(A) spectral radius of matrix A. A ≺ 0 matrix A is negative-definite, equality means semi-definite. A 0 matrix A is positive-definite, equality means semi-definite. diag([A, A, A]) matrix with the diagonal filled with A matrices.

xvii

Chapter 1

Introduction

utonomous vehicles are becoming a reality and soon will be available to the A general public. In recent years, research for this technology has boomed, major vehicle manufacturers, suppliers, technology companies, and universities have started projects and collaborations with the objective of developing fully self-driving vehicles. This overwhelming interest comes from the endless possibilities and gains autonomous vehicles can bring to society and to planet Earth. This thesis focuses on the control of autonomous heavy-duty vehicles (HDV), such as trucks and buses (see Fig. 1.1). More specifically, it uses model predictive control (MPC), an optimization-based technique, to laterally control the vehicle to follow a path. HDVs present different challenges when compared to passenger cars, because of their big dimensions, slow dynamics, and abrupt dynamic changes during operation, e.g., a fully loaded truck which can weigh 40 t vs an empty one which weighs closer to 15 t. HDVs operate in highly constrained environments, e.g., trucks work in mining sites, which are highly disturbed and have tight maneuvering areas, and buses operate in city like environments, which often have badly parked vehicles and/or road construction, leading to very small clearance spaces. There- fore, the control for this type of vehicles needs to be accurate, i.e., tracking closely the path. Moreover, the steering actuation should be calm and smooth, since aggressive steering leads to higher wear and tear, decreasing the lifetime of the vehicle components, leading to higher operation costs. Additionally, fast steering can lead to unpredictable driving behavior and increased chances of rollover, which should be avoided at all times in order to achieve a stable and safe autonomous vehicle. This work is a cooperation between KTH - Royal Institute of Technology [1] and Scania CV AB [2], and is partially supported by the Wallenberg Artificial Intelli- gence, Autonomous Systems, and Software Program (WASP) funded by the Knut and Alice Wallenberg Foundation [3]. Scania and the Division of Decision and Control Systems at KTH have a long lasting research collaboration, where over the years many topics related to HDVs have been investigated. According to the European Commission there are four main vehicle categories: M - vehicles carrying

1 2

Fig. 1.1: Scania bus and trucks experimental and research platforms (courtesy of Scania CV AB). passengers; N - vehicles carrying goods; L - 2 and 3 wheeled vehicles and quadri- cycles; T- Agricultural and forestry tractors and their trailers [4]. Furthermore, vehicles that belong to the M and N categories are classified as light-duty vehicles (LDVs), which are passenger cars and vans, or heavy-duty vehicles (HDVs), which are trucks, buses, and coaches [4]. More specifically, HDVs are defined as freight vehicles with more than 3.5 t, or passenger vehicles with more than 8 seats [5]. The rest of this chapter is organized as follows. Section 1.1 details the global need for autonomous vehicles. A definition for autonomous vehicle is provided in Section 1.2, followed by the introduction of some specific applications and challenges in these vehicles. Finally, in Section 1.3 this thesis problem statement is presented, together with the research challenges and the thesis outline and main contributions.

1.1 Motivation

Climate change continues to happen endangering Earth life as we know it. The need for cleaner and more efficient means of transportation is now bigger than ever. Furthermore, safety is also a growing concern for road users. On the other hand, population and economic growth demand better, faster, more convenient and reliable transportation, which leads to an increase in the overall number of vehicles, in turn increasing pollution and decreasing safety. All in all, autonomous vehicles in conjunction with other new and emerging technologies, such as, e.g., connectivity and electrification, will help improve and reduce the harmful effects of road transportation. More specifically, new technologies in HDVs can improve operation in all mentioned aspects in many applications, consider the examples presented in Fig. 1.2. The following subsections shed some light on the impact and importance that autonomous vehicle technology has and will have globally, by presenting a three- pronged motivation: environment; health and safety; and economical. 1.1. MOTIVATION 3

(a) Three truck platoon. (b) A truck clearing snow.

(e) A truck transporting harvested logs. (f) An electric bus at a charging station.

Fig. 1.2: Examples of applications, where vehicle automation is beneﬁcial (courtesy of Scania CV AB). 4

1.1.1 Environment Motivation

The total number of vehicles in the world is hard to know precisely but a good estimate is 1320 M in 2016 for cars, trucks and buses [6]. This number has been increasing, in fact just recently, in 2017, the United States (US) (273 M registered vehicles [7]) has been surpassed as the country with most number of cars in the world by China with a staggering 300 M registered vehicles [8]. Moreover, the yearly compiled statistical analysis, done by the European Com- mission, shows that in 2016, 21.10 % of the total human-related CO2 emissions in the European Union (EU) were caused by road transportation. Looking at the reports from previous years it is clear this number has slightly increased from 20.46 % in 2014 and 20.77 % in 2015. The same trend is noticeable in green house gases (GHG), in 2016, 17.64 % of the total human-related emissions in the EU were caused by road transportation. Which has also increased from previous years, in 2014, 16.89 % and, in 2015, 17.13 %. [9–11] When comparing HDVs with passenger cars an interesting trend can be seen, in both GHG and CO2 emissions, the share of the emissions for HDV is growing in respect to the share of cars emissions. In the 2015-2016 time-frame, HDVs emissions share grew by ≈ 0.4 percentage points (to ≈26.2 % emissions share). At the same time, conventional cars saw their emissions share decrease from ≈61.1 % to ≈60.7 %. The share for the other means of road transportation has kept mostly constant. [9, 10] An important technology that will help reduce road transport pollution is electric vehicles (EV) (plug-in and battery electric). The number of EV worldwide is up from 3.4 M in 2017 to 5.6 M in 2018 [12]. However, when compared to the 1320 M total number of vehicles worldwide [6], the share of EV (0.4 %, vs the number in 2016) is still very small. Nevertheless, of all new car registrations, EV comprised around 1.5 % in the EU in 2017. The share of registrations is not the same for all countries, e.g., in Sweden, electric vehicle registration is 5.5 % of all new cars. Norway is the clear world leader with 39.2 % of new car registrations being electric vehicles. The share for new EV has been rising from circa 0.2 %, in 2012, to circa 1.5 %, in 2017, in the EU. [13] The expected impact of autonomous vehicles in the environment is not easy to predict. According to a study done by the National Renewable Energy Laboratory of the US Department of Energy [14], if only the energy benefits of self-driving vehicles occur, 90 % of fuel savings is expected to happen. On the other hand, if only the possible disadvantages are considered, the energy usage can increase to more than 250 %. All in all, the authors say that most likely the technology will lead to fuel savings. The concerns with the possible increase in energy use have been expressed by many (e.g., [15–17]) and are mostly related with use intensity increases, i.e., since riding a car will be cheaper and easier than ever, people might take more trips or do bigger commutes to work, leading to more kilometers driven. According to Fisher et al. [18], industrial automated systems (e.g., autonomous mining trucks) are more efficient and precise, not only increasing equipment up- 1.1. MOTIVATION 5 time, but also reducing resource consumption. This results in a more environmental friendly mining operation since energy consumption is more efficient and less waste is generated. Further, according to the North American Council for Freight Effi- ciency, a two-truck platoon can lead to nominal fuel savings of 10 % for the trailing vehicle and 4 % for the leading vehicle, when compared to the vehicles driving sep- arately by themselves. Moreover, in [19] van de Hoef shows platoon coordination is crucial to fully exploit the potential of HDV platooning for reduced fuel consumption and presents simulations to showcase the gains of having large number of HDVs platooning. All in all, autonomous vehicles have the potential and capability of helping the environment. The concerns about use intensity increase are not to be taken lightly, but further improvements in related technologies, and correct education of society, to continue increasing the usage of car-sharing and public transportation, are key to mitigate this problem. In particular for HDVs, the gains are clearer, since the use intensity is economically driven, the optimization of resources is set from the start, and the kilometers driven are automatically minimized.

1.1.2 Health and Safety Motivation Health concerns for drivers is an everyday topic, specially when it comes to truck driving. Long-haul truck driving is a demanding profession, not only because of the endless driving hours, but also because the driver is seldom at home and is always on the job. Sieber et al. in [20] presented the results of a health survey of U.S. long-haul truck drivers, and report that obesity (69 % vs 31 %) and current smoking (51 % vs 19 %) were twice as prevalent in long-haul truck drivers as in the US adult working population. Furthermore, 61 % of the interviewees reported having two or more of the following risk factors:

• Hypertension;

• Obesity;

• Smoking;

• High cholesterol;

• No physical activity;

• 6 h or fewer of sleep per 24 h period.

These risk factors are known to increase the risk of other health problems [21, 22]. Furthermore, these are not just US speciﬁc, e.g., a study made in Australia [23] shows that truck drivers may use more stay-awake drugs, such as smoking, to help combat fatigue. Moreover, the health problems experienced by truck drivers are not limited to physical conditions, according to Garbarino et al. [24], truck drivers also experience mental health problems such as, stress, anxiety and depression. 6

Health hazards in mines have been studied for a long time. In 1978, Cho et al. [25] described the situation in mines at the time with regard to their conditions, and their inﬂuence on diseases and disabilities. Some of the hazards mentioned are:

• Accidents; • Dust, including poisoning by certain ores; • High temperature and humidity; • Noise and vibration; • Toxic gases.

These hazards can result in multiple health problems, e.g., contusions, fractures, lung diseases, ore poisoning, and hearing impairment [25]. Overtime, the conditions in mines have improved but there are still health risks present, e.g., the US Na- tional Institute for Occupational Safety and Health reports [26] that coal workers’ pneumoconiosis (a lung disease) is the underlying contributing cause of death for 4118 workers from 2007 through 2016. Road transportation is the most dangerous means of transportation [27]. The World Health Organization (WHO) reports [28] that approximately 1.35 M people die from traffic crashes each year, that road traffic injuries are the 8th leading cause of death for people of all ages, and more specifically the 1st cause of death for children and young adults (aged 5 − 29 years of age). Moreover, in [29] the WHO states that between 20 M and 50 M more people suffer from non-fatal injuries, that can lead to disabilities. The WHO enumerates some risk factors for road traffic accidents [28, 29]:

• Speeding; • Driving under the inﬂuence (alcohol and drugs); • Not using motorcycle helmets, seat-belts, and child restraints; • Distracted driving (e.g., use of mobile phones); • Unsafe road infrastructure; • Unsafe vehicles; • Inadequate post-crash care; • Inadequate law enforcement of traﬃc laws.

Many of these risk factors are caused by human error, in fact a study [30], by the National Highway Traffic Safety Administration from the U.S. Department of Transportation, shows that 94 % of the times an accident occurs, the accident’s critical reason is attributed to the human driver (for data collected between 2005 1.1. MOTIVATION 7 and 2007). Removing the human from the driving task will improve overall health and safety [18, 31–34]. The health risks and problems for humans in the commercial applications mentioned (long-haulage and mining) are mitigated, since autonomous vehicles will perform the task without any physical human presence. Note, that to achieve this it is not a requirement to have fully autonomous vehicles, related technologies, such as tele-operation [35], can enable partially automated vehicles to be remotely controlled by an operator in necessary situations, that the automated vehicle is still not able to take care on its own. Once again, by removing the human from the equation the risk factors for road accidents are also reduced. Driving under the influence of alcohol, distracted driving, speeding, and inadequate law enforcement of the traffic laws, will be com- pletely mitigated, since the vehicle is programmed to follow the rules, and it cannot be distracted. Furthermore, these vehicles are equipped with the latest safety technologies, so even if they were human driven they would be automatically safer than older vehicles. Moreover, these vehicles will be able to detect faster that an accident is eminent, and also react faster to an accident, which will most likely lead to faster and more adequate post-crash care. On the other hand, autonomous vehicles might bring other safety risks, e.g., connected autonomous vehicles are more prone to be hacked and could possibly be forced to cause collisions and gridlocks [36]. All things considered, autonomous vehicles are bound to improve health and safety for users. New safety risks need also to be investigated and mitigated, which will be possible with the improvement of cybersecurity and other related technologies. For HDVs, the gains are even more straightforward than for normal passenger cars, since the health risk associated with commercial driving disappears automatically by the removal of the physical human driver.

1.1.3 Economic Motivation In 2019, the world population was 7.713 billion and is expected to be 8.548 billion by 2030 [37], so the need for goods and transport will also grow. The number of kilometers driven per year is higher than ever, the US Federal Highway Adminis- tration reports that the yearly average traveled vehicle kilometers for all US roads has increased 0.63 % in 2019 with respect to 2018 [38]. This trend is also noticeable in the EU, in 2016, each person inside the EU did 10 784 km by road transportation on average [9]. This is an increase of 1.96 % from the previous year (10 577 km in 2015) [10]. Moreover, there was also a 4.76 % increase in the t-km amount of goods transported by road inside the EU from 2015 to 2016 [9, 10]. Road transport is the main means of transport inside the EU. It accounts for 49.3 % of the total goods transported in 2016, coming in more than 15 % before the second most used mean, maritime transportation (32.3 %). In terms of passenger transport, the diﬀerence is even bigger, road transport accounted for 81 % (71 % - passenger cars, 1.9 % - powered two-wheelers, 8.1 % - buses and coaches) of the total passenger transport, the second most used mean is air, accounting for 10.5 %. [9] 8

The WHO [29] says that road traffic injuries cause economic losses not only to the individuals who suffer them, but also their families, and nations. These costs come from the treatment, and the loss of productivity caused for the individual and their families that need to take time off to take care of the injured. They also say that most countries spend 3 % of their gross domestic product (GDP) in traffic crashes. The World has seen an increase in truck driver shortage in recent years, and this problem is estimated to get worse [39–41]. For example, in the US there was a shortage of roughly 60.8 k drivers in the trucking industry in 2018, an increase of nearly 20 % from 2017 [42]. The same study [42] by the American Trucking Associations reports that if the same trend holds up, the shortage of drivers in the US can be bigger than 160 k by 2028. In [41], McKinnon et al. enumerate the reasons for the current driver shortage:

• Growth in the demand for road freight transport - in many countries the freight transport growth is closely coupled to economic growth;

• Relative unattractiveness of the occupation - unappealing professional to potential recruits;

• Low wage rates - low incentive to enter the profession;

• Working conditions - unsocial hours, poor amenities, and stress;

• Changing nature of the job - less job satisfaction because of performance monitoring, and less sense of adventure;

• Public image - is often poor and the drivers’ social status is considered low;

• Increased regulation - tightened controls on drivers hours;

• Demographics - it is an old sector that does not attract young people;

• Qualification requirements - high levels of qualification and certification deter potential recruits;

• Industry structure - almost half the drivers are also owners.

There is no single solution to this problem, and some solutions are proposed in the study, one such solution is platooning and automation. This offers technical fixes to the problem: in the case of platooning, allowing a convoy of trucks to be driven by a single driver while the rest of the drivers would be resting, and, in the case of full automation, replacing the driver. The demand for raw materials is also growing with the technological, population, and economical developments [18,43]. In [18], Fisher et al. explain that the decrease accessibility and/or quality of the mineral deposits, lead to increases in capital and labor needed to extract meaningful and useful quantities of resources. Fisher et al., 1.1. MOTIVATION 9 argue that ”automation essentially substitutes capital for labor and other inputs to achieve the same or a better production outcome”, particularly in the case of mine sites, it will not only move mine workers to a safer environment, but also reduce the number of employees, as machine operators undertake multiple tasks simultaneously and take less time in shift changes. Moreover, by moving the workers elsewhere, the costs of transportation and housing of employees at remote mine sites are reduced, as well as, environmental and social impacts. These changes in work environment and conditions, might also help reduce the shortage of skilled workers that are willing to work away from home in inhospitable environments. Finally, Fisher et al. enumerate some of the possible efficiency gains from automation in mine sites:

• Increased up-time and machine utilization - remote controlled and automated equipment can be better utilized, e.g., usually following blasting there are signiﬁcant delays, because humans cannot approach the site and these can be signiﬁcantly reduced;

• Increased predictability, control, and precision - this eliminates unnecessary ineﬃciencies, e.g., unnecessary wear and tear and fuel consumption, since the equipment will operate more eﬃciently;

• Better maintenance - diagnostic tools that are part of the automated equipment allow faults to be identiﬁed more quickly, reducing the equipment down- time and the risk of catastrophic failures. Increasing the lifetime of the equipment, and saving on parts and fuel, by, for instance, making sure tire pressures are maintained at the correct levels.

In general, it is estimated that connected and autonomous vehicles (CAVs) will have a strong impact in the overall economy. McKinsey & Company [44] estimates that autonomous vehicles can lead to economic gains ranging from 200 billion to 1.9 trillion US dollars per year by 2025. From which, 100 billion to 500 billion US dollars would come from self-driving vehicles in the trucking industry. Clements et al. [45], note that most of the savings for the trucking industry would come from cutting the driver cost. In fact, a study made by the American Transportation Research Institute [46] shows that 42 % of the total average marginal cost per mile is related to driver wages and benefits. Furthermore, in [45], Clements et al. study the effects of connected and autonomous vehicles in 13 different industries, where some industries will see economic gains and others will see losses. But, overall, the authors conclude that CAVs will potentially have a positive impact of 1.2 trillion US dollars per year on the US economy.

The next section starts by clarifying the diﬀerent levels of vehicle automation and their implications. Next, it presents some applications for autonomous vehicles. Finally, it introduces some of the diﬀerent challenges autonomous vehicle technology is still facing. 10

1.2 Autonomous Vehicles

An autonomous or self-driving vehicle is a motorized vehicle that is capable of sensing, planning, moving, and interacting with its environment safely, without the active intervention of a human, or at least with limited human intervention. The standard J3016 was created by the Society of Automotive Engineers [47] to establish a clear responsibility deﬁnition of the autonomous vehicle system vs the human. This is the de facto standard (i.e., it is widely adopted by the public, industry, and academia) for automation of vehicles, and it enumerates 6 diﬀerent levels of automation:

0. No Automation - The human driver is in complete control of the vehicle at all times;

1. Driver Assistance - The system helps the human driver with the control of the vehicle in some speciﬁc aspects, but in general the human driver is expected to perform all aspects of the driving task. Some examples of systems helping the vehicle are: adaptive cruise control; lane-keep assist; and electronic stability control. In this level, the system can only provide assistance in one steering or acceleration/deceleration at a time;

2. Partial Automation - The system is able to take control of both steering and acceleration/deceleration at the same time. An example of such an automation is platooning. The human is still fully responsible for monitoring the environment and taking over if needed;

3. Conditional Automation - The system is now in control of all safety-critical functions, and is responsible for monitoring the environment and acting accordingly. However, the human driver is still expected to respond if the system requests human intervention;

4. High Automation - The system is in complete control of the vehicle when requested by the human driver and under certain conditions, for which the system was designed. While in operation, the system initiates deactivation if the design conditions are no longer met, for example, a possible case would be extreme weather conditions. Furthermore, when the system requests deactivation, it needs to be able to handle situations where the driver does not take over immediately, by falling back to minimal risk conditions. When requested for control by the human driver, the system might delay deactivation if human control would compromise safety;

5. Full Automation - Given a destination the system is able to take the vehicle there. The system only deactivates if the human passenger/driver requests it (the system may delay disengagement for safety reasons), or if the vehicle has reached its destination. 1.2. AUTONOMOUS VEHICLES 11

This thesis considers the higher levels of autonomous vehicle automation, where the system is running by itself with no intervention from the human driver. How- ever, for safety reasons, in all experiments realized there is always a safety driver that is able to take over the vehicle at any time. Having deﬁned the automation level for self-driving vehicles, it is important to understand what this technology can be used for and the challenges it still faces.

1.2.1 Applications Nowadays, advanced driver-assistance systems (ADAS) have been simplifying and slowly replacing the human driver in everyday tasks, e.g.: • Adaptive cruise control (ACC) - allows a vehicle to follow a specific speed while avoiding collisions with vehicles in front; • Lane centering - allows a vehicle to steer into the center of the lane of travel; • Automatic parking - allows a vehicle to move from a traffic lane to a parking spot; These systems are commercially available and when combined they can provide already some level of autonomy. This is the case for the Tesla autopilot functionality which is classified as a level 2 automated system [48]. This functionality is designed to drive the vehicle to a destination, while suggesting and making (after human ap- proval) lane changes, navigating highway interchanges, taking exits, finally reaching the destination, and parking. The increasing presence of autonomous functionality on vehicles gives an idea of where the technology can go, and what can be done with it. There are many possible applications for autonomous vehicles. Consider, e.g., the following categories: long- haulage, industrial, and urban.

1.2.1.1 Long-Haulage Application One of the first applications that will start to appear in the upcoming years is platooning for highway driving. This technology represents partial vehicle automation (level 2). In fact, McKinsey and Company, in [49], predicts that autonomous trucks will roll out in four waves: 1- platooning with driver in all trucks; 2- platooning with driver in the leading truck; 3- drivers pickup and drop-off the trucks on highways, where the vehicles drive themselves; 4- full autonomous trucks. Platooning consists of a group of HDVs driving at close inter-vehicular distances, this leads to decreased air-drag, leading to increased fuel savings and road capacity [50]. Pla- tooning already happens to some extent with normal driven HDVs, but, to fully exploit its beneficial effects, the vehicles need to drive very close together, which is not safe for human drivers. Automated HDVs are able to drive at such distances, due to accurate sensor perception, information from vehicle-to-vehicle (V2V) and vehicle-to-infrastructure (V2I) communication, and precise automatic control. 12

1.2.1.2 Industrial Application Autonomous functionality will not only improve on-road driving applications, but also off-road. In fact, near-term autonomy is better suited for industrial applications such as, e.g., mining areas, construction sites, and loading terminals [51]. There are many advantages for a faster and simpler deployment of autonomous vehicles in industrial applications: • Well defined automation task with a clear and direct return on investment; • Fewer to no human-machine interactions, and the necessary interactions can be done by trained and specialized personnel; • Operation area is bounded and geo-fenced; • Safety requirements are more easily set, since the environment is controlled; • Legislation is simpler to uphold, because of the conditions and location where the vehicle is operating; In general, industrial applications, have more controlled environments that allow for targeted solutions to the problem, and do not need to generalize and work for all conditions. However, these applications also present their own specific challenges, since they are normally harsher environments than on-road driving. For example, mining areas, do not normally have smooth drivable surfaces, present dust clouds, and do not have a fixed map, i.e., the drivable area is in constant evolution with new drivable regions being created and previous ones being cut-off [52]. Autonomous solutions for industrial applications are already being tested and/or in operation. For example, Scania and Rio Tinto have already started testing an autonomous transport solution at Rio Tinto’s Dampier Salt mine [53]. Another example is the Yangshan deep water port in Shanghai, China, where the automation of many of the terminal’s functions reduces labor costs, increases handling efficiency, and reduces carbon emissions [54].

1.2.1.3 Urban Application Much of the recent public interest in autonomous vehicles is linked to one application of autonomous functionality, robo-taxis. As the name entails, a robo-taxi is a vehicle that provides the service of a taxi, but instead of having a driver it has an autonomous system making it a self-driving vehicle. This type of application opens up a lot of possibilities and is set to revolutionize not only the way society uses vehicles, but also related businesses, and even city design [55, 56]. A growing trend in the use of vehicles is mobility-as-a-service, i.e., instead of buying and owning a vehicle to move from point A to point B, the user pays for the service of being moved from point A to point B. However, with human driven vehicles, mobility-as-a-service was always very limited, because of the price associated with it. Self-driving vehicles mitigate part of this cost by removing the 1.2. AUTONOMOUS VEHICLES 13 driver, this leads to more aﬀordable prices for the consumer, which in turn allows for mass adoption of the technology [57]. Due to the complex scenes and interactions with other agents, this type of application is most likely one of the hardest to develop and be able to guarantee safety. The next subsection gives an overview of some challenges autonomous vehicle technology still faces.

1.2.2 Challenges Despite all the hype around self-driving vehicles, this technology, still faces challenges before it is widely available and accepted by the public. Among other challenges, not only the technology needs to mature, but also the infrastructure needs to be developed, consumer acceptance needs to be improved, and legislation needs to be implemented. [58]

1.2.2.1 Technology Maturity

Nowadays, ADAS systems seem to provide already some degree of autonomy, but they are still far from level 5 autonomy. ADAS combines the information collected by a set of sensors, such as, global positioning system (GPS), cameras, radio detection and ranging (RaDAR) systems, and even light detection and ranging (LiDAR) systems to perceive their current position and environment, allowing the vehicle to act and respond according to the human driver requests [59]. These systems are able to combine sensor information to perform specific tasks to a certain level, but for higher levels of automation, the system needs to be improved. On the one hand, the necessary hardware for fully autonomous vehicles is likely to be available in the upcoming years, both in terms of computational power and in terms of sensing capabilities. On the other hand, software is and will remain the critical bottleneck for the technology. [60] The software required for higher levels of automation is still not good enough [60]. One of the most challenging tasks is sensor fusion and perception, i.e., fusing the information of all sensors to be able to detect and interpret the environment [61]. Some of the hardest challenges are related with the deterioration of sensor performance under specific conditions, e.g., cameras are affected by strong, low- angle sunlight, while LiDARs are affected by fog and snowfall. So, to have a good perception and estimation of the environment it is necessary to have multiple sensor sources that use different technologies, minimizing the risk of the vehicle becoming blind [62]. Furthermore, after perceiving its environment the vehicle still needs to be able to decide how to reach its goal, negotiate its motion with other agents, and act on it. This is not easy to do and represents tasks where humans are inherently good at. For example, a human driver is able to perceive when it must violate a traffic rule to proceed or overcome an obstacle (e.g., an illegally parked vehicle). Moreover, a human-driver can understand when to cross an intersection or not by looking 14 at other drivers and understanding their intentions, i.e., humans negotiate and agree with other agents easily. Both these examples, showcase the complexity and difficulties of creating a set of rules that represent all possible scenarios. So, high level autonomous systems need to have algorithms that are capable of generalizing and handling unpredictable situations in a reasonable amount of time. [63] Moreover, even if the full system is working, is it safe? Does it have fail-safe mechanisms? For example, a human driver is able to handle a flat tire and perform a safe stop, a fully autonomous vehicle also needs to handle this. I.e., how can one prove the system is working well in all possible situations? Testing, certification, and validation of these systems is a key concern when it comes to autonomous vehicles deployment [64]. Another related and important topic is security. Not only safe autonomous behavior is important, but also the connectivity aspect of this technology exposes it to other risks, e.g., like hacking [65]. A recent study [36] shows that, for Manhattan, when 10 -20 % of vehicles are hacked at rush hour half the city became inaccessible to the rest. Furthermore, hacked vehicles can force collisions, posing a risk not only to the vehicle’s passengers but also other road users. All in all, an indicator that the technology is not mature yet and still experimental is its high cost. There is no exact price released for a fully autonomous vehicle, and the price tags are bound to change before these vehicles are commercially available. However, in general, the autonomous system hardware and software price can range from 70 k to 150 k US $ [66], other estimates are even more expensive, e.g., in [67], a simple analysis of hardware cost leads to a price tag of 250 k US $. How- ever, this cost is expected to decrease with the increase production of hardware components needed for these systems, e.g., Waymo started selling its LiDAR to other companies for exactly this reason [68, 69]. The combination of different sensor setups and software technology can also lead to different system costs, but there is still no clear winner when it comes to the sensor setup [60].

1.2.2.2 Infrastructure

The environment where an autonomous vehicle is deployed also plays an important role when it comes to automation of vehicles [62]. Nowadays, roads around the world do not look the same nor are they built in the same way. This makes the generalization of autonomous vehicles harder. Consider, e.g., traffic lights, road signs, lane markings, bus stops, and road material, these can change from country to country, and even from city to city [62]. These differences make the higher levels of automation a harder problem. If everything was standardized it would be much easier to program the vehicle to interpret and act on its environment. It is expected that new infrastructure being built becomes more standardized and smart, e.g., consider radio transmitters installed in traffic lights (and eventually replacing them) to inform the nearby vehicles of their current status [70]. This would not only simplify the task of sensing and interpreting the information from traffic lights, as it would also improve the range in which the information would be 1.2. AUTONOMOUS VEHICLES 15 available, since line of sight would no longer be a requirement to know the color status of the traffic light. Other examples of smart and connected infrastructure are, smart intersections [71] and smart roads [72]. Smart intersections, contain fixed sensing and computation hardware to detect and warn of eminent dangers to nearby vehicles, helping human-drivers (and eventually autonomous vehicles) approaching the intersection. Smart roads, can not only provide network connectivity to vehicles, but also real-time information about traffic, road conditions, and/or possible accidents. They can even act as a sensor input for autonomous vehicles, since they can provide information about vehicle position and velocity on the road. Autonomous vehicles are normally linked with connected vehicles, since connectivity brings new possibilities and simplifies the automation task. While there are already multiple connectivity options deployed and operational (2G-4G), which provide connectivity to multiple internet of things applications, the higher capacity, data rate, and lower latency, makes 5G the ideal choice to maximize safety and efficiency of road transportation [73]. The current network infrastructure presents multiple challenges, e.g., current roaming solutions are designed to support single users traveling outside the coverage of their home networks. This presents many difficulties, as the current architecture is designed to route all traffic to the home network first. This increases latency1 and it is a problem for the latency-critical and/or high data throughput automotive case. Another problem is that when switching networks after losing coverage of the home network, the mobile device loses connectivity for some time (up to 120 s), this is a serious problem for a safety- critical application like autonomous driving [73]. Furthermore, for vehicles to be able to communicate to other vehicles (V2V) or infrastructure (V2I), communication and message standards need to be set and agreed by all. This problem has been recognized by researchers and initial strides were already made to solve it, e.g., consider both Grand Cooperative Driving Challenges (GCDC) held in 2011 [74] and 2016 [75]. However the communication issue is still not solved, after GCDC 2016, Ploeg et al. in [76] conclude that the current standardized message sets did not support the cooperative maneuvers to a sufficient extent. A recent study done by KPMG [77] where 25 countries are compared on how prepared they are for autonomous vehicles, shows that in terms of infrastructure the most prepared countries are The Netherlands and Singapore, and the least prepared are Brazil and Russia. The study takes into consideration, among other factors, the quality and coverage of mobile network, the density of electric-vehicles charging stations, and road quality.

1.2.2.3 Consumer Acceptance and Awareness

One of the big barriers to autonomous vehicles is public acceptance and awareness of the technology. According to a survey made by Pew Research Center [78], 56 % of

1For networks, latency is the amount of time it takes a packet to travel from source to destination. 16

Americans would not ride in a driverless vehicle, and 87 % would favor a requirement that all driverless vehicles have a human safety driver that can take over in case of emergency. Furthermore, a recent study by Hewitt et al. [79] conducted with people living in the US, shows that users are less accepting of higher levels of autonomy vs the lower levels, and also displayed less intention of using highly autonomous vehicles. On the other hand, KPMG’s study [77] reports that both the US and the United Kingdom are the bottom two countries when it comes to consumer acceptance, so these numbers might be diﬀerent in other countries. According to KPMG’s study [77], Indians and Mexicans are the top 2 countries when it comes to consumer acceptance.

One of the key aspects influencing consumer acceptance is safety. A study done by Moody et al. on international perception of autonomous vehicle safety [80], shows that countries with higher motorization rates and lower road death rates tend to report worse perceptions of autonomous vehicles safety, which indicates that consumers in developed countries are less optimistic of autonomous vehicles than those in developing countries. Moreover, Moody et al. find that across all countries young males report higher current perceptions of autonomous vehicle safety and fewer years until they are safe enough to use, which the authors argue could lead to more rapid manifestations of road safety benefits, given that young males are the part of the population most linked to risky driving and pedestrian behavior. The perception of safety by the general public gets highly influenced by mass media coverage of road accidents involving autonomous vehicles, e.g., the Uber accident that killed a woman in 2018 [81]. But one most also not forget that on average 3698 people die per day due to road accidents, and a lot more people are injured or disabled [28]. The difference is that these accidents are already part of everyday life and, usually, do not get the same media coverage. Mass media coverage is a good thing, since it raises safety and technology maturity issues, however singled out examples of when this new technology failed, should not be taken as the ultimate safety analysis of the technology.

Safety is also linked to ethical and social dilemmas, e.g., consider the following one presented in [82]: An autonomous vehicle is driving on a road and a collision is eminent, either it kills several people that are crossing the road or it can swerve and kill a passerby. What should the vehicle do? What if the decision is between a pedestrian crossing the road and the passenger of the vehicle? What if it is between several pedestrians crossing and its own passenger? This dilemma can be endlessly modified with different nuances, e.g., are the pedestrians crossing the road in a designated area such as a crosswalk, or saving one or multiple children vs saving one or multiple elderly people. In [82], Bonnefon et al. found that study participants tended to agree that utilitarian autonomous 1.2. AUTONOMOUS VEHICLES 17 vehicles2 would be better for everyone, but they would prefer to ride in autonomous vehicles that protect their passengers at all costs. Although these ethical dilemmas are important and should not be ignored, Holstein et al. [83] argue that the focus should be on practical technological solutions and their social consequences, rather than on idealized unsolvable problems, that even humans have problem solving and that do not have a general solution under all circumstances. Another issue that influences consumer acceptance is the replacement of many jobs by autonomous vehicles and what happens to the people that got unemployed [84]. A prediction made by the International Transport Forum in 2017 [85], shows that of the 6.4 M potential driver jobs in 2030, between 3.4 to 4.4 (53.1 -68.8 %) would become redundant if driverless trucks are deployed quickly. Note that this does not necessarily mean all those drivers will be made redundant, in fact it is predicted that by 2030 about 5.6 M drivers will be available and willing to work under current conditions, so this also means that if nothing is done the truck driver shortage will continue. Moreover, not all is grim when it comes to jobs, because autonomous vehicles are also creating them [86,87]. Besides the obvious jobs related to developing, building, and selling these vehicles, other jobs are being created, e.g., special technicians, dispatchers, and general support staff at warehouses and delivery locations [87]. Furthermore, when it comes to the mining industry Cosbey et al. [88] predict that jobs will be created in the development, observation, service, and maintenance of remotely controlled autonomous equipment as well as in data processing, systems, and process analysis.

1.2.2.4 Legislation

Legislation for autonomous vehicles is one of the big challenges, not only in terms of deployment and liability, but also in terms of software development. A human- driver breaks the law while driving to avoid worst outcomes, e.g., consider a child running into the street in front of a car, a human-driver swerves over a double line to avoid the child, an autonomous vehicle should make the same decision (assuming there are no other consequences other than breaking the law) [89]. This means that the autonomous vehicle needs to be programmed to break the law when needed. The issue then becomes when should the system break the law, how to deﬁne what is acceptable or not. Note that the autonomous system will make its decisions based on probabilities and predictions, so there will be a need for hard number deﬁnitions on the software of what is acceptable or not. In contrast, legislation can also help alleviate the problem with ethical dilemmas such as the one presented in the previous subsection [82]. A period of transition where fully autonomous, semi autonomous, and tradi- tional driven vehicles will coexist is bound to happen. And this period of transition will also happen for the liability of a vehicle accident, i.e., nowadays when there

2An utilitarian autonomous vehicle makes the decision based on greater good, i.e., in the dilemma it would sacrifice its passenger and save several pedestrians crossing the road. 18 is a road accident the driver and its insurance are liable for the damages, and this is bound to shift to the companies behind the hardware and software that is driving the autonomous vehicle. In fact, e.g., Volvo already declared that it would pay for any injuries or property damage if its IntelliSafe Autopilot system causes a crash. [90] When it comes to deployment of such a system there are a lot of things to consider and legislation, many times, makes it harder. Different countries have different road legislation, e.g., in most countries the vehicle drives on the right side of the road, but in some, e.g., the United Kingdom, the vehicle drives on the left side of the road. Autonomous systems need to be prepared to cross borders and change and adapt to the local traffic laws. Furthermore, the current autonomous vehicle legislation varies from country to country and everyone is in a race to be the leader when it comes to autonomous technology leading to new laws and legal frameworks being proposed and approved by different countries regularly [91]. This race also leads to some lack of cooperation between countries, so even the new laws being created are not standardized [92], which will make the deployment of autonomous functionality harder, due to the different requirements in each country. KPMG’s study [77] on autonomous vehicle readiness, reports that the two countries most prepared in terms of legislation and policy are Singapore and the United Kingdom, while the least prepared are Mexico and Brazil. Singapore’s government is working hard to consolidate the city-state’s positions as a center for autonomous vehicles, and it has already announced that from 2022 on some areas will use driverless buses and shuttles for off-peak and on-demand commuting [77].

The next section presents the research challenges and questions this thesis addresses.

1.3 Problem Statement

The autonomous vehicle system objective is to complete missions while driving itself. For example, consider the mission of going from city A to city B. First, the vehicle needs to decide how to reach the destination by deﬁning a route (which roads to take, which exits, etc.). Second, the autonomous vehicle understands how its current environment looks like using sensors. Moreover, the vehicle localizes itself, other road users, and obstacles. Third, the system predicts how the environment will look like in the future, to understand what actions it can make safely. Fourth, the autonomous vehicle plans a local path to avoid obstacles and follow the overall planned route, taking into consideration the predicted obstacles and road-users. Finally, the system controls the vehicle to follow the local plan. All of this sub-problems can be viewed as building blocks that together compose the autonomous vehicle system. The considered autonomous vehicle architecture is detailed in Chapter 2. This thesis addresses the motion control problem for autonomous HDVs, in particular the lateral control problem. The objective of the control module is to make 1.3. PROBLEM STATEMENT 19

Planned Path Initial vehicle position Vehicle path [m] Y

X [m]

Fig. 1.3: Vehicle lateral control example.

the vehicle track the local plan received. The motion control module can be further partitioned in sub-modules (Chapter 3), where the two main sub-modules are longitudinal and lateral control of the vehicle. The longitudinal control module responsibility is to control the vehicle speed, making it follow a speed profile, braking at a desired point, etc.. Compared to a human driver it controls the brake and throttle pedals of the vehicle. The lateral control module of the vehicle is the main focus of this thesis and can be explained with the example presented in Fig. 1.3. In Fig. 1.3, the vehicle is represented by the red dot and the local plan is presented in black. Considering the vehicle is moving, the lateral controller makes the vehicle steer onto the path, achieving the path presented in red. Compared to a human driver it controls the vehicle steering-wheel angle to make the vehicle turn. The motion control module objective is to follow the path, while providing a safe and comfortable ride and respecting actuation constraints. Furthermore, the motion controller has to control the vehicle considering the current environment conditions and disturbances. For example, the vehicle can be driving over different surfaces, such as, snow, ice, gravel, dirt, asphalt, and can be disturbed by wind gusts or potholes. Additionally, the controller needs to be fast and light in order to be real-time, such that it can be implemented and experimented with real vehicles. Note that there are limited computation resources on the vehicles, and these are shared among all modules. Finally, the controller should also be general in order to be used with different vehicles, e.g., a truck and a bus, with minimum model and/or parameter changes. 20

There are still many open questions when it comes to motion control of autonomous vehicles. The following questions summarize the work presented in this thesis:

• How can robustness be guaranteed for a linear time-varying (LTV) controller formulation, when the system is subject to unknown but bounded input and state disturbances?

• How to achieve the desired smooth and calm vehicle behavior, while handling time-varying safety and actuation constraints, and maintaining tracking accuracy?

• How to handle interface disturbances between motion planning and control modules, e.g., diﬀerent vehicle models and diﬀerent computation methods?

• How to modify the common kinematic bicycle model, such that better closed- loop performance is obtained when the modiﬁed model is used in the controller?

The next section presents the outline, detailed chapter contents, and contributions of this thesis.

1.3.1 Thesis Outline and Contributions This section outlines the remainder chapters of this thesis and their contributions. Chapters marked with an * contain experimental evaluations.

Chapter 2: Background This chapter introduces the system architecture for autonomous driving and provides a literature review for each module. First, the long term goals of the system are planned with a route planner. Then, the vehicle sensors interface with the perception and localization modules for the system to be able to localize and understand its environment. This scene is then analyzed and evaluated by the situational awareness module that in turn decides the best action to achieve the desired goals. After, the motion planner module uses this action to create a feasible obstacle-free local plan for the vehicle to follow. Finally, the motion controller module makes use of the vehicle actuators to track the desired local plan. In the end of the chapter, the main concepts for discrete control and optimization used throughout this thesis are introduced.

Chapter 3: Control Module Architecture This chapter introduces in detail the architecture considered for the control module. This module receives the planned trajectory/path from the motion planner as 1.3. PROBLEM STATEMENT 21 well as the current vehicle position from the localization module. First, a future vehicle position is predicted from the current position and previous control requests to account for the actuation delay of the vehicle. Then, using the predicted position the motion plan is sampled and references for both the lateral and longitudinal control are obtained. The longitudinal control is done through a velocity request to the vehicle velocity controller. The lateral controller makes a vehicle curvature request that is transformed to a steering-wheel angle request using a vehicle spe- ciﬁc nonlinear mapping. The steering-wheel angle request is then tracked by a lower level controller that actuates a servo motor. Finally, throughout this chapter assumptions made by the controller are presented.

Chapter 4: Linear Time-Varying Robust Model Predictive Control This chapter presents a robust model predictive controller for discrete-time nonlinear systems. The controller prediction model uses a linearized time-varying version of the original discrete-time system. Moreover, the optimization problem includes the initial nominal state of the system, which is constrained to be in the vicinity of the measured real system states. This allows to guarantee robust exponential stability of the disturbance invariant set for the discrete-time nonlinear system that constraints the initial nominal position. The proposed controller is posed as a quadratic program and real-time capable. After, the proposed control method is veriﬁed through simulations of an autonomous vehicle application example. Fi- nally, the limitations of the proposed approach are discussed with the help of a more constrained autonomous vehicle example. The chapter is primarily based on the publication:

Chapter 5: Reference Aware Model Predictive Control* This chapter presents a path following controller for autonomous vehicles, making use of the linear time-varying MPC framework. The controller considers control input rates and accelerations to account for limitations in the vehicle steering dynamics and to provide a safe and comfortable ride. Moreover, the controller includes a method to systematically handle model references generated by motion planning algorithms that consider different vehicle models from the controller. Additionally, this chapter presents a vehicle specific model modification for lateral control of autonomous vehicles. The standard kinematic bicycle model yaw-rate is modified, introducing a curvature dependent function, to better predict the vehicle curvature response. The controller and model modification are verified and compared to 22 the SA-MPC [93] through simulations and experimentally with a modified Scania construction truck. The chapter is primarily based on the publications:

• G. Collares Pereira, P. F. Lima, B. Wahlberg, H. Pettersson, and J. M˚artensson. Reference Aware Model Predictive Control for Autonomous Vehicles. To appear in 2020 IEEE Intelligent Vehicles Symposium (IV);

• G. Collares Pereira, P. F. Lima, B. Wahlberg, H. Pettersson, and J. M˚artensson. Nonlinear Curvature Modeling for MPC of Autonomous Vehicles. To appear in 2020 IEEE 23rd International Conference on Intelligent Transportation Systems (ITSC).

Chapter 6: Conclusions and Future Work This chapter summarizes the work presented in the thesis and outlines future research directions.

1.3.2 Other Publications by the Author The following publications by the author inﬂuenced the work presented, but are not covered in this thesis:

• G. Collares Pereira, L. Svensson, P. F. Lima and J. M˚artensson. Lateral Model Predictive Control for Over-Actuated Autonomous Vehicle. In 2017 IEEE Intelligent Vehicles Symposium (IV), Los Angeles, CA, 2017, pp. 310- 316;

• S. Kokogias, L. Svensson, G. Collares Pereira, R. Oliveira, X. Zhang, X. Song, and J. M˚artensson.Development of platform-independent system for cooperative automated driving evaluated in GCDC 2016. IEEE Transactions on Intelligent Transportation Systems, 19(4):1277-1289, April 2018;

• P. F. Lima, G. Collares Pereira, J. M˚artensson,and B. Wahlberg. Progress maximization model predictive controller. In 2018 21st International Con- ference on Intelligent Transportation Systems (ITSC), pages 1075-1082, Nov 2018;

• P. F. Lima, G. Collares Pereira, J. M˚artensson,and B. Wahlberg. Experi- mental validation of model predictive control stability for autonomous driving. Control Engineering Practice, 81:244-255, 2018;

• R. Oliveira, P. F. Lima, G. Collares Pereira, J. M˚artensson, and B. Wahlberg. Path planning for autonomous bus driving in highly constrained environments. In 2019 IEEE Intelligent Transportation Systems Conference (ITSC), pages 2743-2749, Oct 2019; 1.3. PROBLEM STATEMENT 23

The publications are ordered by date. In the presented publications the thesis author has participated actively in the discussions and derivations of the theory and results, as well as, in the paper writing.

Chapter 2

Background

riving is a hard and complex task, which humans are used to and do without D significant difficulty. For an autonomous vehicle, this task is divided in many smaller sub-tasks, and each sub-task addresses a specific challenge. This chapter introduces the fundamental modules of an autonomous vehicle system. Namely, route planning, perception, localization, situation awareness, motion planning, and motion control. Section 2.1, presents the modules and a brief literature review of solutions developed for each. Special attention is given to the motion control module, since this is the main focus of this thesis. Then in Section 2.2, the necessary concepts of discrete optimization and control theory are defined. Finally, in Section 2.3 a summary of the chapter is presented.

2.1 Autonomous Vehicle Modules

This Section describes the main modules of an autonomous vehicle system. For each module it describes its objective and provides a short literature review. Fig. 2.1 presents an overview of the system architecture1. Six modules are highlighted in the ﬁgure: route planning; perception; localization; situation awareness; motion planning; and motion control. Both vehicle sensors and vehicle actuators are represented in the ﬁgure for completeness. However, they will not be introduced in this Chapter since it is focused mostly on software (i.e., methods, algorithms).

2.1.1 Route Planning The route planning module objective is to provide a high-level plan to achieve the mission goal. For example, a mission goal can be going from city A to city B, and the plan should specify which roads to take, which lanes to keep, which exits to take, and eventually provide alternative routes.

1Other modules could be considered for an autonomous system, but the ones presented represent a minimal and typical system representation.

25 26

Route planning

Vehicle sensors

Perception Localization Pedestrians GPS Other vehicles Lane-markings Traﬃc lights Features Free space 3D-map

Situation awareness Scene understanding and prediction Behavior selection

Motion planning Path planning Trajectory planning

Motion control Lateral control Longitudinal control

Vehicle actuators

Fig. 2.1: System architecture of an autonomous vehicle (adapted from Lima [94]). The software modules introduced in this Chapter are highlighted in gray.

The route obtained can be optimal with respect to different metrics, i.e., it could be the shortest route between two points, or the fastest, or the cheapest, etc. The decision and choice depends on the application and ultimately the user. This functionality is very similar to what is already commercially available in GPS navigation solutions. Consider Google maps [95], it already provides many of the aforementioned features, and takes it some steps further. It gives multiple means of travel, like public transportation, plane, and car. In the public transportation case it even lets the user know how crowded it normally is at specific times [96]. Route planning makes use of a road network (normally contained in a map), which is usually represented as a directed graph. The graph edge weights, i.e., the cost of going through a specific road segment, is where the specific application or user choice of the optimal route comes into play (this cost can also be a combination 2.1. AUTONOMOUS VEHICLE MODULES 27 of multiple objectives). There are many different algorithms to solve the routing problem, probably the most well known is Dijkstra [97]. Dijkstra’s algorithm is the standard solution to the one-to-all shortest path problem. The algorithm iteratively goes through every vertex and arcs starting from the initial vertex. It iteratively updates the cost of each connected vertex, by adding the current cost to the cost of traversing the arc to that vertex [98]. Many improvements have been proposed to Dijkstra’s algorithm, and other algorithms that, e.g., have goal-directed approaches, or hierarchical techniques. For a comprehensive study of this topic, the reader is referred to a survey by Bast et al. [98]. Although route planning is a well known problem and already has well known solutions, there are still some challenges. A typical situation in route planning, is to have an agent planning and optimizing individually its own route, which might not be optimal when considering other agents. This can happen for several reasons, such as unusual traffic congestion due to many agents wanting to go through the same road at the same time, or possible fuel savings for HDVs obtained by platooning. To solve such issues, the vehicles need to not only think about themselves, but also consider other agents and current traffic trends. For platoon planning and coordination the reader is referred to the review by Bhoopalam et al. [99], where the authors conclude that there are still open challenges related to optimization, uncertainty, sustainability, and network design. Being a high-level mission planner, route planning can be done locally on the vehicle or centralized by a global coordinator. Either way, it is mostly not affected by the immediate surroundings of the vehicle. For that reason, in Fig. 2.1, it is presented above the vehicle sensors. All other modules are periodically using information at high frequencies from the vehicle sensors, and, therefore, are presented below them.

2.1.2 Perception A human is able to drive a vehicle because he (she) is able to sense and perceive the environment through mostly his (her) eyes but also his (her) ears. Human drivers are able to see and hear their surroundings, and, from that information, perceive what is happening around them, acting accordingly. An autonomous vehicle needs to be able to do the same, in fact the equivalent to eyes and ears for an autonomous vehicle are vehicle sensors, and the equivalent to the human perception and interpretation capability is the perception module of the vehicle. The perception module is responsible for detecting, identifying, classifying, and tracking different objects in the environment surrounding the vehicle. Fig. 2.2 shows two examples of vehicle perception. The top image presents a camera image and the identified and classified road users with bounding boxes. The bottom image shows a 3D image of a LiDAR point cloud also with the identified and classified bounding boxes on the road users. This module receives real-time data generated by sensors and fuses them to create a map of the environment. To have an accurate map, multiple sensors are 28

(a) Camera based perception (courtesy of Neurala [100]).

(b) LiDAR based perception (courtesy of Popular Science [101]).

Fig. 2.2: Examples of perception using different sensors for autonomous vehicles. 2.1. AUTONOMOUS VEHICLE MODULES 29 used: RaDARs; LiDARs; monocular and stereo cameras; GPSs; inertial measurement units (IMUs); and sound navigation and ranging sensors (SoNaRs). Fusing information from multiple sensors allows for a better understanding of the surrounding environment, than using each sensor data-stream individually, since the shortcomings of one sensor can be mitigated by another and vice-versa. [61] A self-driving vehicle needs to be able to perceive lane-markings, traffic signs, other vehicles, pedestrians, and other static and dynamic obstacles. The perception module needs to perceive all the aforementioned agents, but also needs to handle measurement errors, sensor noise, and environment conditions. One of the oldest approaches of representing the environment is using an occupancy grid map [102, 103]. The environment map is represented as grid cells, and each cell stores the probability for existence of an obstacle, where the size of the cells is tuned based on the application and granularity necessary. For dynamic obstacle tracking there are other, more suitable, approaches, such as Kalman filters [104], particle filters [105], or even combination of different methods [106]. For a more detailed discussion of sensor fusion and perception for autonomous driving the reader is referred to the recent surveys by Van Brummelen et al. [107] and by Badue et al. [108].

2.1.3 Localization

Vehicle localization is the task of estimating the state of the vehicle (e.g., position, orientation, speed, acceleration), both with respect to its surroundings (locally) and with respect to a map (globally). This module is essential for safe, comfortable, and precise vehicle motion. For example, consider yourself in a dark room walking around virtually blind. Even if you know where the obstacles are, if you do not know where you are in respect to those obstacles (locally) or in respect to the room walls (globally), you are very likely to bump into obstacles and hurt yourself while trying to move inside the room. Nowadays, GPS2 sensors are widely available, in fact, most people are using them on a daily basis with their phones. This might lead to the erroneous idea that vehicles simply need to have a GPS to localize themselves, since this is basically what people use to drive/move from ”A” to ”B”. However, this is not true since the position provided by the readily available GPS sensors is not accurate enough for autonomous vehicle applications (accuracy of up to 2 m [109]). There are more accurate systems such as real time kinematic GPS, which makes use of model assumptions and extra communication with a base station to accurately estimate the vehicle position (accuracy of 1 cm [110]). However, even this more advanced, expensive, and accurate GPS systems, have reliability issues. For example, consider a vehicle in a city environment, or in a long tunnel, or an HDV operating inside a

2The correct term that should be used here is global navigation satellite system (GNSS), for ease of readability, the term GPS is being used interchangeably since it is more well known. 30 mine. The satellite communication will fail (or degrade) and the system will not provide a reliable and accurate position anymore. To mitigate these problems, multiple sensors are used and fused together to localize the vehicle. Thrun et al. in [111] use an unscented Kalman filter to fuse the data from GPS, IMU, and wheel encoders, which together with accurate vehicle modeling proved to be accurate and reliable even with GPS outages, since the Stanley vehicle ended up being the winning robot of the second DARPA Grand Challenge. Broggi et al. in [112] rely on GPS3 way-point following. When GPS fails (around 40 %-50 % of the distance covered on the tests) other functions are activated to localize the system, such as, lane following and barrier following. Lane following uses camera images to detect lane markings on the road. Barrier following uses guard rails and general barriers obstacles obtained via a laser-based obstacle detector. Other approaches where high-precision GPS is not used online, normally make use of a previously recorded high-precision map (which are most likely obtained using a more complete and complex sensor rig, that includes high-precision GPS). Ziegler et al. in [113] make use of a digital map, together with feature-based and lane-marking-based localization techniques that are enabled by front and rear facing cameras mounted on the vehicle. These localization techniques use online recognition to detect features and lane-markings in the current field of view (FOV) of the vehicle, and match these with the known map, allowing to position the vehicle in respect to the map. In [114], Ward and Folkesson use an extended Kalman filter together with an iterative closest point matching method on radar measurements to localize the vehicle against a recorded map. The map is obtained and created from a single pass through with the vehicle on the same route. Akai et al. in [115] implemented an extended Kalman filter using data from wheel encoders, a gyroscope, and a LiDAR point cloud scan matching against a pre-recorded map. For a more in depth discussion of the current state-of-the-art of vehicle localization techniques the reader is referred to the survey by Kuutti et al. [116].

2.1.4 Situation Awareness

The objective of the situation awareness module is to determine the vehicle action, as well as provide scene understanding and prediction of the vehicle’s surroundings. Examples of possible vehicle actions are: maintain lane; overtake; yield; park; start moving. A human driver is able to make decisions not only based on the current perception of the environment, but also on their expected behavior, and/or predictions of the other agents. For example, when overtaking on a highway a human driver considers multiple things: is there space to overtake, is it safe to change lane, does it break traﬃc rules (e.g., speed limit), is the vehicle being overtaken aware and maintaining their current course. All these considerations make use of human intuition and capacity to predict and anticipate movements.

3In fact the authors say GNSS, but to maintain text coherence the term GPS is used. 2.1. AUTONOMOUS VEHICLE MODULES 31

Fig. 2.3: Examples of motion prediction with diﬀerent types of motion model (courtesy of Lef`evreet al. [121]).

The first step to scene understanding is to know the surroundings of the vehicle. This task is shared with the perception module to a certain extent and it is not easy to define a clear boundary between perception and situation awareness. Nowadays, an approach that has proven successful is to use machine learning techniques, such as convolutional neural networks (e.g., [117, 118]), which allow to detect and track the scene. For a detailed survey of different methods and algorithms used, the reader is referred to Schwarting et al. [63] and Janai et al. [119]. Normally, an object can be classified by the way it moves (e.g., [120]) and/or the way it looks (e.g., [118]). Both these classifications help understand the scene and predict agents movement and intention. If all obstacles and agents are connected and autonomous, allowing them to broadcast their intentions and future predicted motion and actions, then the need for environment prediction would be very limited. However, autonomous vehicles are being designed to drive and interact with non autonomous non connected agents. So, for them to operate they need to predict and anticipate the environment. Lefèvreet al. in [121] classify motion modeling and prediction in three levels, physics-based, maneuver-based, and interaction-aware based motion; Fig. 2.3 depicts an example for each of these prediction models. Physics-based motion models are the simplest, since they assume agents motion only depends on the laws of physics. For example, consider the works by Ziegler et al. in [113] and Ferguson et al. in [122], where assumptions like constant speed are made and future states are predicted. These motion models can be as complex 32 as desired, e.g., Ammoun and Nashashibi in [123] assume Gaussian noise and use Kalman filtering to predict the future positions of other agents. Maneuver-based motion models are more advanced as they take into consideration the agents intended maneuver (e.g., turn left on the intersection). However, these models still represent the vehicles as independent maneuvering entities, i.e., the vehicle motion being predicted is not influenced by other agents. An easy example of such models, is to consider the more likely trajectories the vehicle can take. Consider Fig. 2.3, the normal behavior of the vehicle approaching the intersection is to turn left, turn right, or stop. Assuming there is prior knowledge of the possible cluster of trajectories (e.g., Atev et al. [124]), it is possible to predict the future vehicle motion by comparing the cluster trajectories to the current one being executed (e.g., Hu et al. [125]). Another possible prediction approach is to directly estimate the intended maneuver by using available information, such as, the road network, vehicle state (e.g., position, speed, heading, yaw rate), and even driver behavior (e.g., head movement, driving style). For example, Kumar et al. in [126] use a support vector machine and Bayesian filter for online lane change intention prediction. The more complex motion prediction models are the ones where interaction with other agents is taken into consideration. In [127], Lawitzky et al. compute the intention and collision probability of a maneuver. The scene prediction is achieved by doing a safety evaluation and using the assumption that drivers avoid collisions, which allows for the computation of a prediction of the probabilities for all possible actions for all agents. Agamennoni et al. in [128] use a Bayesian approach to infer the behavior of other agents. In particular, they apply their algorithm to an intersection scenario where a vehicle merges into a main road, and, as expected, the merging vehicle is predicted to yield to the traffic on the main road. Although, interaction-aware motion models seem like the obvious choice for prediction, as noted by Lefèvreet al. [121], these models and algorithms are computationally expensive and further work is needed for real-time implementation capability. For a more detailed discussion on vehicle prediction and estimation the reader is referred to the survey by Lefèvreet al. [121]. Once the current and future environment is mapped and estimated, the autonomous vehicle is able to make a decision.4 The easiest decision making (behavior generator) method for autonomous vehicles is hierarchical and concurrent finite state machines, these have been used in the DARPA challenges, e.g., [111,129,130], and continue to be used, e.g., [113,131,132]. On the one hand, state machines allow for a clear, comprehensible, and traceable top-down modeling of reactive systems, being easy to use and verify. On the other hand, state machines are specifically tailored to tasks, i.e., they switch between predefined behaviors, so they lack the ability to generalize to unknown situations and to deal with uncertainties. To ad-

4The system architecture depends on the approach, algorithms, and methods being used. Some approaches plan multiple paths/trajectories for multiple actions, and only after re-estimating and re-evaluating the effect on the environment, would the system make a decision. This is briefly mentioned in the next subsection 2.1.5. 2.1. AUTONOMOUS VEHICLE MODULES 33 dress these issues, several methods have been proposed: Gaussian based approaches (e.g., [133,134]); Markov decision processes (MDP) and generalizations approaches (e.g., [135–137]). The reader is referred to the survey by Schwarting et al. [63] for a more detailed discussion. Finally, it is important to mention that it is in the situation awareness module where safety starts to be taken into consideration. Decisions to take actions inevitably lead to situations where collisions and accidents can occur. The only way to avoid these situations would be to not have vehicles on the road at all, or have them all stopped. Traffic rules are not enough in order to guarantee safety, because many times they need to be broken in order to avoid an accident. On the other hand, if the vehicle is overly cautious it might just decide not to move. In order to address these issues, both academia (e.g., Althoff et al. in [138]) and industry (e.g., Shalev-Shwartz et al. from Mobileye in [139], and Nistéret al. from NVIDIA in [140]), are developing algorithms and methods to maintain safety distances while adapting to human driving behavior.

2.1.5 Motion Planning

The motion planning module responsibility is to compute a path or trajectory, while respecting vehicle dynamic constraints, being safe, and smooth, such that the motion is comfortable for passengers. The big difference between a trajectory and a path is that the first is parameterized in time, while the latter is not. A trajectory describes where the vehicle will be at a specific time. A path only describes where the vehicle will be, not when. Independently of computing a path or trajectory, the objective of the module is always the same, and it needs to compute a plan from the current vehicle state5 to a goal state, while satisfying constraints, such as, road boundaries, speed limit, actuation limitations, and obstacles. The following review for the planning module is significantly more extensive than for previous modules, because the planned path/trajectory is one of the main inputs to the motion controller. Making it important to understand how the planned references are created, what they consider, and their limitations. While the route planner plans to achieve the mission goal, taking into account the road-network and user preference, such as fastest or cheapest route. The motion planner plans for the next seconds or minutes, taking into account vehicle and local environment constraints, where, e.g., other vehicles and traffic lights/signs are considered. Motion planning can be considered the main responsible in decision making, because if the motion is planned, then the decision to make a maneuver is also made. Moreover, motion planning has been the focus of research for many decades, and different algorithms have been proposed depending on the structure of the environment, i.e., if it is structured or unstructured.

5This is not always the case, it could be from some other state, e.g., a previously planned point. 34

Structured environments are, e.g., roads and parking lots, environments where there are clear rules on how the vehicle and traffic should move. These guidelines are usually defined as road markings and road signs, that indicate road direction, number of lanes, allowed or prohibited overtaking/crossing, etc. . Another example, of a structured environment is a parking lot, where the vehicle follows the traffic rules to reach a parking spot, and where the vehicle needs to park with a specific pose occupying only one parking space. A planned path or trajectory in structured environments, provides a plan that, e.g., overtakes another vehicle and returns to the original lane, or takes the inner most lane of a roundabout to take the last exit, or keeps a safe longitudinal distance from the vehicles on the same lane on the highway. Due to the structure of the environment, most planning cases involve maneuvers in the vicinity of a known path. For example, the planner can access a highway map and use the lanes center lines to plan in their vicinity. Unstructured environments are the rest of the environments that do not present any clear structure on how a vehicle should drive. This is the case for off-road driving environments, such as, mining sites and harbors, particularly important applications for HDVs. This type of environment gives the vehicle a much wider range of motion, from which the motion planner has to select the most appropri- ate maneuver to reach the goal. On the other hand, since there are many more possible motions to achieve the same goal, the problem complexity and size can grow very fast, becoming hard to compute it in real-time6. Furthermore, in these environments it is more common to have drastic environmental changes, e.g., on a mine, piles of rock might be dumped in a specific spot and the vehicle has to re-plan around it. The algorithms used to solve these challenges are normally more exploratory in nature, allowing them to search for the best motion to achieve the goal. Following the presentation by Gonzálezet al. in [141], motion planning algorithms for non-holonomic vehicles can be divided in four types of methods: graph-based; sampling-based; interpolation-based; and optimization-based. Fig. 2.4 presents an example for each type of method. The following subsections provide a description and a small literature review of each type.

2.1.5.1 Graph-Based Methods

Graph-based methods discretize the environment as a graph, where the vertices correspond to possible conﬁgurations of vehicle states (e.g., position, and heading) and the edges represent transitions between those states. The desired path is then found by performing a search for a minimum-cost path in the graph. Handling static obstacles becomes trivial, since vertices and edges that collide with obstacles are ignored in the search, meaning that the solution (if found) is collision free.

6This is not to say it is easy to compute in real-time for structured environments. 2.1. AUTONOMOUS VEHICLE MODULES 35

(a) Lattice-based planning (courtesy of Piv- (b) Sampling-based planning (courtesy of toraiko et al. [142]). In light gray the Jeon et al. [143]). In red the road-limits lattice-graph, the resulting plan is shown are presented, in green the goal line, in blue with the thick black line, and in the top each of the expanded nodes (vehicle states), left corner the motion primitives used to in salmon each of the expanded branches generate the lattice are presented. (sub-plans) connected to the nodes, and the optimal trajectory is shown in black.

(c) Interpolation-based planning (courtesy (d) Optimization-based planning (courtesy of Gonz´alezet al. [144]). of Helmeier et al. [145]).

Fig. 2.4: Examples of motion planning for autonomous vehicles. 36

Creating the graph is not a trivial task and the ability to run the algorithms in real-time greatly depends on the graph used. Ideally, the graph should have a very large number of nodes and edges, such that every little spot of the free-space is covered by a node. However, the more nodes and edges are considered, the larger is the computational expense and the longer it will take to plan a path. Moreover, since non-holonomic vehicles are being considered, the transition between states is constrained by the dynamics of the system. To solve this issue, a common approach is to discretize the space-state using motion primitives computed offline that respect the vehicle dynamics (e.g., Ferguson et al. [122] and Cirillo [146]). When the motion primitives generate a graph that covers the state-space uniformly the graph is called a lattice (see Fig. 2.4a). Probably the most famous algorithm to search a graph is Dijkstra [97], which finds the shortest-path between two vertices. For example, in the 2007 DARPA Urban Challenge both Bohren et al. [147] and Bacha et al. [148] use this algorithm to solve part of the planning problem. An extension to this algorithm is the A-star (A?) algorithm proposed by Hart et al. [149]. The most important difference is the cost function decision, which defines the order in which the nodes are explored based on the weights attributed to each node. A? is significantly faster than Dijkstra, since it searches the graph in a smarter way, and, under certain conditions, it is still optimal and complete. Due to its computation efficiency, A? and extensions have been widely used in autonomous vehicles. In [150], Ziegler et al. propose the A? algorithm together with a Voronoi-based cost function to plan in unstructured environments and parking lots. Ferguson et al. in [122] make use of a lattice-based planner together with anytime dynamic A? to plan complex maneuvers for Boss, the vehicle that won the 2007 DARPA Urban Challenge. These methods have also been used for HDV motion planning. In [146], Cirillo implements a lattice-based planner using a time- bounded A? algorithm to ensure real-time operation. Ljungqvist et al. in [151] also propose a lattice-based planner, but for the task of reversing a general 2-trailer system, while maintaining real-time performance. Although it is not trivial to handle dynamic obstacles with these approaches, it is possible. For example, Ziegler and Stiller in [152] do trajectory planning on- road using a spatiotemporal lattice. This approach allows the vehicle to consider oncoming traffic while overtaking, as well as to merge into slower moving traffic.

2.1.5.2 Sampling-Based Methods

Sampling-based planners incrementally grow tree-like paths that have the root origin in the initial vehicles states. These planners explore the obstacle free space by sampling a point from it and adding it to the current tree while connecting the branch (path). The branch is created while respecting motion constraints and guaranteeing a safe collision-free movement. The algorithm ﬁnishes when a node is suﬃciently close to the desired goal state. The advantage of these algorithms vs graph-based is that they are capable of handling higher dimensional spaces 2.1. AUTONOMOUS VEHICLE MODULES 37

(Gonzálezet al. [141]), solving real-time constraints that deterministic methods cannot meet. The disadvantage is that these algorithms are only probabilistically complete, i.e., if a solution exists the probability of finding it approaches one as time increases (Katrakazas et al [153]). The most used sampling method for autonomous vehicles is likely to be rapidly- exploring random trees (RRT), originally proposed by LaValle [154]. RRT is known to generate sub-optimal paths, and undesirable oscillatory behavior (Gonzálezet al. [141], Katrakazas et al [153], Karaman et al. [155]). To address the issue of sub-optimality, Karaman and Frazzoli in [156] propose the RRT? algorithm, that is provably asymptotically optimal. RRT? has been used by Jeon et al. in [143] for optimal trajectory planning using the half-car dynamic model. The authors showcase their results with a trajectory for a U-turn maneuver (see Fig. 2.4b) and a trajectory for a circuit. Another RRT approach for autonomous driving is presented by Kuwata et al. in [157] called closed-loop RRT (CL-RRT). CL-RRT uses a controller and a system model to do forward simulation of the system states. Afterwards, instead of sampling randomly from the state-space (as RRT does), it selects a sample from the input space of the controller and adds the simulated dynamically feasible trajectory to the tree. Due to operational safety concerns, Evestedt et al. in [158] extend the CL-RRT algorithm with a sampling recovery method using brake profile regen- eration. Basically, when the algorithm detects an obstacle collision, a new speed profile for that branch (path) is computed in order to bring the vehicle to a safe stop. Then, the system is re-simulated and that trajectory is added to the tree, which leads to many more safe trajectories. The authors showcase their approach by implementing the algorithm on a modified Scania G480 mining truck in a simple constructed scenario.

2.1.5.3 Interpolation-Based Methods

Interpolation-based planners receive a set of waypoints (e.g., obtained from a graph or a sampling based method) and compute a smooth plan, by inserting new data points within the range of the received points. These methods use the original points to generate a new continuous and smooth trajectory that respects vehicle constraints and is safe (e.g., avoids obstacles) [159]. Obstacles can be easily avoided by using interpolation-based methods, since these generate a trajectory to go around the obstacle, re-entering the original plan after overcoming it [141]. Most likely, the first types of interpolation planners are Dubins [160] and Reeds and Shepp [161]. The first makes use of straight lines and circular arcs to construct the shortest path between two poses, and the second extends the first method by allowing vehicle reversing. These algorithms are simple, and consider instantaneous changes of curvature, i.e., they go from straight line to maximum (or minimum) allowed curvature, which leads to the shortest path, but are not actually feasible. Having this non-feasible path leads to big tracking errors when trying to follow it, since it is impossible to do maximum steer instantaneously. It is possible to 38 address these issues by considering more complex vehicle models. For example, Frachaird and Scheuer in [162] extend the Reeds and Shepp algorithm to consider continuous curvature paths, by including clothoid arcs for the allowed motions. In [163], Oliveira et al. extend previous works by considering sharpness (curvature rate) continuous paths and steering acceleration constraints. A possible classification of interpolation-based planners is by types of curve used, e.g., clothoid, polynomial, Bézier,or spline. Although clothoids do not have a closed-form expression, their continuous curvature property makes them easy to track by a controller; also they are used for road construction making them an obvious choice for on-road planning (e.g., Funke and Gerdes [164], and Brezak and Petrović[165]). Polynomial curves are used not only because of their smoothness properties, but also to meet interpolation constraints (e.g., position, heading, curvature) at specific points. Polynomials have been used in overtaking scenarios by Petrov and Nashashibi [166] and Cong et al. [167], and lane change scenarios by Glaser et al. [168]. Béziercurves are parametric curves based on control points to define their shape, their core is Bernstein polynomials. Gonzálezet al. in [144] used a Bézier based planner (see Fig. 2.4c) for urban environments, to perform intersections and handle obstacle avoidance. Similarly, Han et al. [169] implemented a Béziercurve planner to perform an overtaking scenario, and also to go around a small track, while avoiding obstacles. Finally, a spline is a piecewise polynomial parametric curve, i.e., it is a curve divided in intervals where each interval is described by a polynomial. For example, Berglund et al. [170] use a b-spline based planner for an autonomous mining vehicle, and Bacha et al. [148] use cubic splines to solve part of the planning task of the vehicle Odin which participated in the 2007 DARPA Urban Challenge.

2.1.5.4 Optimization-Based Methods Optimization-based planners minimize or maximize a function describing the planning criteria, while explicitly handling constraints. Recently, these approaches have been gaining attention, both due to the ever increasing computing power and the number of numerical optimization tools available. The planning problem is set up as an optimization problem with the vehicle model, actuator limitations, and free space as constraints, and the planning criteria (e.g., smooth plan) as the optimization objectives. A straightforward approach when defining the optimization problem is to directly use a nonlinear formulation and solver. Ziegler et al. in [171] formulate the on-road trajectory planning as a nonlinear optimization. The main objective is to track the center of the driving corridor, but to achieve a comfortable and smooth trajectory, acceleration, jerk, and yaw-rate are also penalized in the cost-function. Furthermore, the authors consider dynamic obstacles, by constraining each of the planned points individually depending on the prediction of the environment at that 2.1. AUTONOMOUS VEHICLE MODULES 39 time. Nonlinear optimization has two main disadvantages, it is difficult to implement in real-time, and the solution to the problem might only be a local optimum trajectory [63]. The motion planning problem, with all the constraints and objectives, is highly nonlinear, but it can be linearized in order to get a convex problem that is easier to solve in real-time. In [172], Bevan et al. do three convex optimization problems successively in order to obtain the solution (or approximate solution) to their original non-convex optimization problem, which includes vehicle dimensions and space-varying road boundaries. Oliveira et al. in [173]7, use a sequential quadratic programming (SQP) formulation to plan for an autonomous bus driving in constrained environments, where awareness of the bus overhangs is important. To solve the problem, the authors introduce a new classification for sweepable obstacles (where the overhangs can go through), and make use of the accuracy improvements of the successive QP problems solved, allowing to achieve a successful obstacle avoidance. Multiple objectives can be explicitly considered when setting up the problem as an optimization. Oliveira et al. in [173]7 minimize the deviation from the road centerline, the change in control input (steering actuation), and the amount the overhangs exit the drivable region. Achieving a smooth trajectory and minimum disturbance to other road-users due to forcing, as much as possible, the bus to be inside its lane. In [145], Helmeier et al. plan a minimum curvature trajectory around a racetrack (see Fig. 2.4d). This objective is not exactly the same as minimum time around the racetrack, but the authors consider it to be a good trade- off between computation time needed to solve the problem and loss of optimality on time around the racetrack. Andersen et al. in [174] formulate an optimal control problem that is in the boundary of planning and control. The optimization problem actively changes the trajectory in order to maximize the visibility of the autonomous vehicle, i.e., the problem is formulated in order to increase visibility of blind spots occluded by other road-users. The authors argue that this increases safety since it increases the vehicle’s gathered information to make decisions. To guarantee collision free and safe plans, there are many assumptions made about obstacles and vehicle body (see Fig. 2.5). It is important to simplify the vehicle body shape in order to able to solve the problem in real-time. In [171], Ziegler et al. decompose the vehicle geometry with circles in order to check for collision with obstacles (that are represented by polygons). Batkovic et al. in [175]8 model the vehicle body and other road users as ellipses. Oliveira et al. in [173]7, approximate the vehicle body of a bus by a fixed set of points. Each of these methods has their advantages and disadvantages, and it is hard to determine the best method for all scenarios and vehicle configurations. For example, Schwarting

7This thesis author is a co-author of this work. 8This work is in the border of control and planning, since both tasks are being done simultaneously. It is mentioned in this section, because it is a good example of possible vehicle and road users modeling. 40

Fig. 2.5: Modeling the vehicle body (adapted and courtesy of Oliveira [177]). Circles - Ziegler et al. [171], ellipses - Batkovic et al. [175], and points - Oliveira et al. [173]. et al. in [176] model the ego vehicle with circles, but the other road-users with ellipses to describe the Gaussian uncertainty of their position. Finally, it is important to mention that optimization planners, as interpolation- based ones, can also be used after another planning method, to achieve a more optimal trajectory with respect to some criteria. Oliveira et al. in [178], use a greedy path optimization approach to minimize length of the path that is originally generated by a lattice-based planner. Dolgov et al. in [179], use a nonlinear problem formulation to increase the smoothness of the trajectory generated by an A?-based method.

The previous presented work focuses on single maneuver planning. Some works consider planning multiple maneuvers evaluating all trajectories and deciding which one is best to take according to a set of criteria. Evestedt et al. in [180], compute multiple trajectories for merging scenarios and based on feasibility, safety, comfort, and progress each trajectory leads to, the best is chosen. Note that this approach allows the authors to also take into account how the planning vehicle would aﬀect the other road-users for each plan it can do, allowing it to take into consideration the other vehicles’ limitations and comfort. In [181], Heinrich et al. parallelize the planning task with a graphics processing unit (GPU), allowing them to obtain a set of collision-free plans. Afterwards, these plans are evaluated to pick the safest trajectory, considering the uncertainty of the vehicle executing each trajectory and the uncertainty of other road-users.

For a more detailed discussion on motion planning methods the reader is referred to the surveys by Gonz´alezet al. [141], Katrakazas et al. [153], and Paden et al. [182].

2.1.6 Motion Control The motion control module responsibility is to stabilize and guide the vehicle towards a given reference path or trajectory. As explained before, a trajectory is parameterized in time, so tracking it is more restrictive than tracking a path, since it needs to track a reference pose at a specific time. Independently of the reference used, the motion controller has to handle disturbances to the vehicle, correcting its 2.1. AUTONOMOUS VEHICLE MODULES 41 states (e.g., position, yaw, velocity) back to the desired reference while maintaining the actuation limits and achieving a smooth and comfortable ride. The vehicle disturbances can be divided in external and internal. External disturbances are, e.g., potholes, rain, snow, or gravel. Internal disturbances are, e.g., model uncertainty or unmodeled actuator dynamics. Furthermore, smooth actuation is important not only because of possible passengers, but also to reduce wear and tear of the vehicle, increasing the vehicle components lifetime. The motion controller receives the vehicle states estimated by the perception and localization module, as well as, a feasible reference path or trajectory from the motion planning module. Usually, the path (or trajectory) and its vicinity are assumed safe and collision-free, i.e., if the vehicle tracks the plan with a sufficiently small error9, no accident will occur. The controller is responsible for actuating the steering wheel (or wheel angles), throttle, and brakes (or individual wheel torque) of the vehicle. The control design can be divided into two sub-tasks, longitudinal and lateral control, or treated as one problem, combined control. Longitudinal control is responsible for tracking a longitudinal position in a trajectory, by minimizing the longitudinal displacement, speed, and acceleration errors to it. This task is usually divided in two loops, an inner faster loop that takes care of the throttle and brake control, and an outer slower loop that performs a higher level tracking on velocity and acceleration. A basic form of longitudinal control is a cruise controller (CC), where the driver sets a desired speed and the vehicle tracks it. Nowadays, a more advanced form is also widely available, where the vehicle tracks not only a desired speed, but also maintains a safety distance to other road- users, called adaptive cruise control (ACC) (consider the review by Björnander and Grunske [183], and more recently the review by He et al. [184]). Cooperative ACC (CACC) is a more advanced type of longitudinal control, that assumes communication between nearby vehicles, and is of special interest for vehicle platoons (consider the review by Wang et al. [185]). Lateral control is responsible for tracking a desired path by minimizing the lateral and heading errors to it. The more common approach is actuating on the steering wheel in order to change the wheels angles and make the vehicle turn. Another approach is torque vectoring and differential braking, where different wheels are rotating at significantly different speeds changing the heading of the vehicle (e.g., Vignati et al [186], or the recent video by Rivian performing a tank turn [187]). An- other approach is 4 wheel-steering (4WS), also known as all wheel steering, which is already available on commercially available cars (e.g., Audi’s A8 [188]) enhancing vehicle handling and stability to some extent. 4WS taken to its limits can allow lane changes without changing the direction of the vehicle (e.g., Collares Pereira

9Suﬃciently small error is open for interpretation. A more robust and safe system would also provide a safe collision free tube which the vehicle cannot exit. Another approach is for the controller to also have access to the obstacles and road information, allowing it to actively avoid collisions. The disadvantage is that this approach requires a more complex controller. 42 et al. [189]10), or parallel parking with sideways motion (e.g., DLR’s ROboMO- bil [190]). Combined longitudinal and lateral control can allow for higher maneuverability and enhanced stability of the vehicle. This approach is in general more complex, since it makes less assumptions about the current states of the vehicle. For example, decoupled lateral control of the vehicle is usually computed under the assumption the vehicle will keep constant speed. During normal operation the vehicle speeds up and down and assuming constant velocity may lead to problems, e.g., the same steering at a higher speed leads to a higher lateral acceleration, which can cause drifting. This type of controller can be easily designed in a model predictive control (MPC) framework, but in practice the computation power needed to solve the optimization problem makes it hard to implement (e.g., Rosalia et al. [191]). Since this thesis focuses on lateral control of autonomous vehicles, an in-depth review is made for the motion control module, and in particular for lateral control applications. There are many diﬀerent approaches to solve the motion control problem of an autonomous vehicle, the following subsections are divided by control methods: geometric, proportional-integrative-derivative (PID), fuzzy, sliding mode, linear quadratic (LQ), H2 and H∞, and model predictive control (MPC).

2.1.6.1 Geometric Control Geometric controllers are methods that make use of geometry analysis to derive a control law. The most famous geometrical controller is the pure-pursuit. Another, also well known, geometry based controller is the Stanley controller, named after the Stanford team vehicle that participated in the 2005 DARPA Grand Challenge.

Pure-Pursuit Control Pure-pursuit control (PPC) is one of the first known methods to address the path following problem (Amidi and Thorpe [192] and Wallace et al. [193]). The pure- pursuit control law is simple and based on a geometric approach inspired by human driving. Humans look at the road and focus on a point some distance ahead making the vehicle drive in the direction of the point in focus. In PPC the goal position is chosen on the reference path based on a user-defined look-ahead distance. Then, a circular arc is used to connect the current vehicle position to the goal, see Fig. 2.6 for an illustration. The circular arc defines a specific curvature (inverse of its radius) which is used to compute the required steering angle. The PPC method has been applied in numerous instances due to its versatility and ease of implementation. The 2005 DARPA Grand Challeng had two vehicles with PPC [195] and the 2007 DARPA Urban Challenge had three [196]. Snider in [194] implements the PPC method together with the simulator CarSim [197] and showcases the capabilities and shortcomings of different look-ahead distance tuning

10This thesis author is a co-author of this work. 2.1. AUTONOMOUS VEHICLE MODULES 43

Fig. 2.6: Pure-pursuit controller illus- Fig. 2.7: Stanley controller illustration tration (courtesy of Snider [194]). From (courtesy of Snider [194]). The wheel the goal position (gx, gy) and the look- angle δ is computed from the lateral ahead distance ld, the radius R of the (efa) and heading (θe) errors to the path circular arc necessary to steer the vehi- point (cx, cy). cle to the goal point is computed. of the controller. The main disadvantage of this method is that, by design, it can never obtain null error to a path in curvy roads. In other words, the PPC cuts corners since it always follows a point ahead. Over the years, the original PPC has been modiﬁed to address some of its shortcomings. For example, in [198], Shan et al. modify the PPC to use a clothoid curve instead of a circular arc, and use a fuzzy logic controller to decide on the look- ahead distance. The authors present results comparing the proposed method with the controller used by MIT’s team in the 2007 DARPA Grand Challenge [199] and with the Stanley controller [111] (presented in the next subsection). The proposed method performs better, showing smaller lateral error to path and maintaining a higher average speed on the experiments.

Stanley Controller

The Stanley controller is a geometric based controller that got famous for being the steering controller used in the vehicle that won the 2005 DARPA Grand Challenge [111]. This controller implements a nonlinear control law based on the lateral and heading errors to path from the vehicle’s front axle center to the closest point on path, see Fig. 2.7 for an illustration. Thrun et al. in [111] show that the Stanley controller makes the lateral error to path converge exponentially to zero (for initially small errors). The tuning of the controller is done by scaling the lateral error to path with a gain, forcing the controller to actuate more (or less) to a value of error. Later, in [200], Hoffmann et al. extend the method to stabilize the vehicle yaw dynamics at higher speeds by 44 adding a negative feedback term based on yaw-rate. Furthermore, the authors add a servo motor steering response damper term in order to avoid instability caused by the servo motor. Finally, the authors guarantee the lateral error correction term in the control law is not oversensitive to noise at low-speeds, by adding a tunable velocity offset. As for the PPC, in [194] Snider implements the Stanley method together with the simulator CarSim [197] and showcases the capabilities and shortcomings of different tunings of the controller. As expected, higher controller gains lead to lower lateral error to path. However, higher gains also lead to more instability, specially at high-speeds. The author compares the Stanley controller to the PPC and concludes that the Stanley does not cut corners and achieves smaller lateral error to path. On the other hand, the Stanley is less robust to disturbances and requires a continuous curvature path, i.e., it does not handle well discontinuities on the path. The Stanley controller as been widely used and modifications to it have also been proposed. Amer et al. in [201] propose an adaptive modified Stanley controller for trajectory tracking of an autonomous armored vehicle. The authors modify the controller by adding extra tuning parameters, which are modified online. The online tuning is done with a fuzzy logic supervisor, allowing the controller to adapt to different trajectories and improving the controller performance. Fleischmann et al. in [202] modify the Stanley controller by adding a look-ahead distance, using a mean of recent past errors, and adding an integrative term on the lateral error to path part of the control law. The authors are able to successfully implement their approach on an agricultural tractor with a round baler, achieving a lateral error to path no greater than 25 cm following a pre-recorded GPS track. Cibooglu et al. in [203] propose an hybrid method using the PPC and Stanley controller methods. The authors method weighs both the PPC and Stanley control laws at the same time, where the weighing is changed to account for very sharp turns in the path. The approach is tested and compared using simulation, and the authors conclude that their hybrid approach performs better than both the PPC and Stanley controllers individually.

2.1.6.2 PID Control

The proportional-integrative-derivative (PID) control is the most common control method used in the world [204, 205]. What makes the PID control so widespread is its simplicity and versatility. The PID control law continuously computes the difference between a desired set-point and the current measured value of the process variable. Then, it computes a control signal taking into account the error variable, its integral, and its derivative, multiplying each term by a respective gain. The gains can be tuned and fixed, or alternatively can be varying by using a gain- schedule approach based on a scheduling variable (e.g., longitudinal velocity). To learn about PID control and its tuning, the book by Aström[204]˚ is a good start. 2.1. AUTONOMOUS VEHICLE MODULES 45

Fig. 2.8: Block diagram of PID with anti- Fig. 2.9: PD control architecture for windup (courtesy of Kolb et al. [209]). ACC and CACC (courtesy of Lidstr¨om et al. [208]).

PID control has been widely used for longitudinal control of vehicles. Hima et al. in [206] implement a PID with anti-windup11 for cruise control, see Fig. 2.8. The controller requests a desired acceleration or deceleration to track the desired velocity for the vehicle, and assumes that this reference is tracked by lower level controllers already implemented on the vehicle. In [207], Osman et al. propose a PID with a feed-forward PD controller for a cruise control system. The authors showcase their proposed method vs the normal PID and vs the normal PID with a standard feed-forward term, showing that their approach is more robust to disturbances. Lidstr¨omet al. in [208] present Halmstad’s University entry to GCDC 2011. The longitudinal control architecture designed allows the vehicle to perform both ACC and CACC, by taking into consideration the platoon leader, see Fig. 2.9. The authors propose two PD controllers: one tracks the position of the preceding vehicle; the other tracks the desired acceleration. If in ACC mode, the desired acceleration is always from the preceding vehicle. Otherwise, if in CACC mode the desired acceleration is the minimum between that of the preceding vehicle and the platoon leader. PIDs have also been used for lateral control of vehicles. Marino et al. in [210] propose a nested PID steering lateral controller. First, a reference yaw-rate is designed with PID control techniques. Then, the steering angle is designed to track the generated yaw-rate using a PI controller. Kolb et al. in [209] propose a control method for path tracking, where a PID and a PPC are combined. The PID corrects the lateral error to path. The PPC tracks the next path point. The authors conclude that by combining these two simple methods the overall path tracking performance is improved, since the shortcomings of each control strategy are partly addressed by the other. Finally, the combined controller is implemented

11Integral windup occurs due to limitations in physical systems, i.e., when the process is satu- rated. Anti-windup approaches detect the saturation and prevent the integral error from growing. 46 in a 1 : 32 scaled truck, where a maximum lateral error of 1 cm is achieved while following a path. Zakaria et al. in [211] address both the longitudinal and lateral control problems of an autonomous vehicle with PIDs. The longitudinal control is done with a PID to track velocity. The lateral control is done with a P controller on heading error and a PID on lateral error to path. To increase the accuracy of path tracking, the authors propose a curvature-varying velocity reference profile, such that the vehicle slows down for turns. Furthermore, the authors use a velocity dependent look-ahead distance, meaning that it decreases with lower velocities and vice-versa. When it comes to PIDs it is hard to define a set of tuning gains that work well for all operating conditions. A common way of tackling this challenge is different gains at different times. In general, the idea is to improve system performance, by changing the response of the feedback loop and adapting to the current operating conditions. For example, Broggi et al. in [212] implement a P controller for steering an autonomous vehicle, where the controller gain is inversely proportional to the vehicle’s velocity. Zhao et al. in [213], implement a path tracking adaptive-PID controller. The authors design the control gains based on the sampling time and previously measured lateral errors to path. PID techniques have also been used to design more complex control loops, where a reference variable is designed with a PID. Then, this reference is used in nonlinear mapping functions, or other controllers, to compute the desired control. Menhour et al. in [214] propose a combined longitudinal and lateral flatness-based12 nonlinear controller. This controller makes use of a PI and a PID both with feed-forward terms to design the flat output references, which are used to compute wheel torque and steering angle for the vehicle. Goh et al. in [216] design a path tracking controller with sideslip stabilization. First, system error dynamics are imposed with P, PD, and feed-forward terms. After, using a nonlinear model the steering and rear thrust angles are computed. The steering angle is directly actuated on the vehicle, while the rear thrust angle is sent to another nonlinear mapping to obtain a desired wheel speed. This reference is then tracked by a faster feedback control loop that actuates through individual torques on the rear wheels of the vehicle. Recently, the authors have extended their work and have made the vehicle transverse a more difficult path while drifting (see a video of the experiments in [217]), showcasing the impressive capabilities of MARTY (a modified 1981 DMC DeLorean) and their methods [218].

2.1.6.3 Fuzzy Control

Fuzzy logic has its origins in a 1965 paper by Zadeh [219]. The term fuzzy means that the logic involved can deal with concepts that are not necessarily deﬁned as true or false, but rather as partially true, i.e., instead of deﬁning a boolean variable

12This type of controller assumes fictitious outputs for the system, called flat outputs, and expresses the system as a function of these flat outputs and their time-derivatives [215]. 2.1. AUTONOMOUS VEHICLE MODULES 47

0 or 1 it allows for a continuous range of numbers in between [220]. Fuzzy control is a method based on fuzzy logic. An interested reader is directed to a recent article by Zadeh [221] written to celebrate the 50th anniversary since the original paper, where the author elaborates on the name fuzzy and describes some of his contributions since then. Usually, fuzzy control is applied in systems where no mathematical model is known or where accurate models are diﬃcult to obtain and use [222]. The control input variables are transformed into linguistic variables by using membership functions. The control request is chosen based on fuzzy rules of the form if-then. Consider the simplistic example presented by Rupp et al. in [222],

if lateral error to path is left then steering angle is right, the lateral error to path is the linguistic input variable and the steering angle is the controller output. The idea behind fuzzy control is to make the system behave as a human would control it. This has the advantage of allowing for some good initial guesses by experienced engineers. However, the controller tuning is not straightforward and is done manually during tests. Another advantage of these controllers is that they are easy to read and understand. On the other hand, if the number of variables is too large, the system becomes unmanageable and unreadable. Furthermore, stability analysis cannot be done without mathematical models. Probably the ﬁrst fuzzy controller for a vehicle was proposed by Sugeno and Nishida in [223], where the front wheels of a model car are controlled to traverse a double corner scenario. Since then, fuzzy controllers have been extensively used for lateral control of vehicles. In [224], Hessburg and Tomizuka implement and compare a fuzzy logic controller to a PID and a frequency shaped linear quadratic controller, showing comparable performance between controllers up to 50 km h−1 on a test-vehicle. In [225], Naranjo et al., propose a simple steering fuzzy logic controller with only four rules to perform lane-change and overtaking maneuvers, showcasing the controller capabilities with an experiment.

Chaib et al. in [226] use simulations to compare four different controllers (H∞, adaptive, proportional, and fuzzy control) over a test track circuit while being perturbed with wind, and with varying speed and road friction coefficient. The authors conclude that the self regulating adaptive control scheme obtains the best performance, and the proportional controller the worst. While the H∞ and fuzzy controllers perform similarly. More recently in [227], Wang et al. implemented two fuzzy logic controllers, one for steering and another for velocity, verifying the proposed controllers with simulations and experiments. The authors also compare the fuzzy controllers to a PID (with constant gains), and conclude that the fuzzy controllers have a better performance. Fuzzy control has also been used for longitudinal control of vehicles. In [228], Asere et al. implement a fuzzy cruise controller. Using simulations the method is compared to a PI controller, where the fuzzy controller is able to reach steady-state faster than the PI controller. Naranjo et al. in [229] implement a fuzzy controller 48 that uses throttle and brake request to achieve both ACC and stop and go functionality. The authors showcase their method with experiments up to 55 km h−1. In [230], Tsai et al. implement a fuzzy controller for ACC and stop and go functionality, which tracks the desired distance to the front vehicle. The proposed method is verified with both simulations and experiments for speeds ranging from 0 to 120 km h−1. Pérez et al. in [231] propose a fuzzy controller for cooperative ACC and implement it in a CitroënC3. The vehicle and controller are then used to participate in the first GCDC competition, where the system was able to handle failures and finish all heats.

2.1.6.4 Sliding Mode Control

Sliding mode control (SMC) makes use of a variable structure controller to be robust to model uncertainties and disturbances [222]. In general, SMC is a nonlinear control method that uses a discontinuous control law to alter the dynamics of the controlled system. The system dynamics are altered to follow a sliding surface. This sliding surface is defined in the state-space of the system and designed to make the system become asymptotically stable under certain conditions. The control law is then derived to achieve the desired dynamics of the sliding variable. For a more detailed introduction to SMC the reader is referred to the book by Shtessel et al. [232]. In the book [233], Ferrara presents work with SMC for longitudinal and lateral control of vehicles. Moreover, the author presents the main advantages for SMC: (1) theoretically exact compensation of bounded matched uncertainties; (2) reduced system order dynamics when the sliding surface has been reached; and (3) finite- time convergence to the sliding surface. The author also presents the disadvantages: (1) chattering, due to discontinuities on the control input and the switching between laws; (2) system insensitivity only for perturbations that affect the same state as the control input; (3) the finite-time convergence is only guaranteed for the sliding variables, the state variables converge asymptotically; and (4) non-ideal closed-loop performance in the presence of parasitic dynamics, discretization, and noises. The biggest disadvantage is most likely the chattering, a possible way of handling this issue is by using a higher-order SMC [222]. For example, a well-known second-order SMC is the super-twisting algorithm [234]. Autonomous vehicles longitudinal control has seen applications of SMC. In [235], Rajamani et al. present a cascaded control loop with SMC for longitudinal control of a vehicle. The first SMC of the cascade defines a desired acceleration for each car of a platoon ensuring string stability. The second SMC tracks the desired acceleration by making throttle and brake requests. Furthermore, the authors also consider how to create space between vehicles for safe merging and splitting of platoons in the presence of communication constraints. The effectiveness of the control method is showcased with experimental results of an eight car platoon drive, consisting of continuous 6-8 h driving per day during 3 weeks. 2.1. AUTONOMOUS VEHICLE MODULES 49

Ferrara and Pisu in [236] present a second order sliding mode controller for CC and ACC of a vehicle. The controller capabilities are verified through simulation where the robustness against sensor noise and wind gusts up to 10 m s−1 is demonstrated. In [237], Nouvelièreand Mammar also implement a second order SMC for ACC, more specifically they implement the super-twisting algorithm [234]. The authors showcase their method with experimental results, drawing conclusions of the controller’s impact to increase road capacity and safety. Ferrara and Vecchio in [238] present a first and second order SMC for a platoon of vehicles. The authors consider two operation modes: a cruising mode for normal platooning; and a collision avoidance mode for when a collision is detected. The proposed methods are verified in simulation where both the distance to pedestrians and to other vehicles are considered. Driver assistance systems for lateral control have also seen SMC applications. In [239], Canale et al. present an active stability system based on yaw control using a second order SMC. The control design considers a nonlinear static map for reference generation, as well as, a feed-forward term to improve the yaw-rate transient response. The approach is successfully verified in simulation for three different scenarios with speeds up to 100 km h−1. Imine et al. in [240] design an active steering assist system with SMC for rollover avoidance of HDVs. The rollover avoidance is based on the control of the vehicle’s lateral acceleration. The approach is verified with an instrumented Renault truck on a zigzag and a ramp test. SMC has also been proposed for lateral control of higher autonomy level vehicles. In [241], Tagne et al. use the super-twisting algorithm to control an autonomous vehicle to follow a path while minimizing the lateral error to it. The proposed controller is verified experimentally with a Peugeot 308, where the robustness to high lateral acceleration, and uncertainties in vehicle parameters (such as tire cornering stiffness and vehicle mass) is highlighted. Dominguez et al. in [242] propose a proportional controller based on desired lateral velocity and a kinematic vehicle model. More interestingly, the authors compare their approach to PPC, Stanley, and SMC controllers performing experiments on a Renault Zoe ZE. The authors conclude that the SMC and the lateral velocity controller perform the best, and also that the lateral velocity controller is smoother than the SMC. In [243], Alcaláet al. propose two sliding mode controllers, one for lateral and another for longitudinal control of the vehicle. The authors compare their approach to a Lyapunov based control law. Both, controllers are designed to track desired lateral and longitudinal velocities and yaw-rate of the vehicle. The SMCs achieve better tracking errors, but the Lyapunov method provides smoother steering control. Rupp and Stolz in [222] compare five different controllers (Stanley, Fuzzy, SMC, LQR, and MPC) on a simple simulated example. The SMC provides the smoothest and smallest lateral errors, while the LQR provides the smoothest and smallest an- gular errors. On the other hand, the SMC is the most aggressive with the control input requests. The approaches are summarized as follows: the Stanley controller is simple and provides a smooth performance; the LQR has low computational com- 50 plexity, is robust, and simple to tune; the SMC is very robust, but its discrete-time performance needs to be evaluated; the MPC has high computational complexity, but good performance and the ability to handle constraints.

2.1.6.5 Linear Quadratic Control

Linear quadratic (LQ) control methods are concerned with controlling a system with minimum cost. The most famous is the linear quadratic regulator (LQR) controller, which is a feedback controller that minimizes the following problem, Z ∞ z(t)TQz(t) + u(t)TRu(t) dt, (2.1) 0 where,z ˙ = Az + Bu are the system dynamics, and Q 0 and R 0 are the optimization objectives for the state and control input cost, respectively. This problem has a unique solution13 and the optimal state feedback is u(t) = Kz(t), with the feedback controller K = −R−1BTP , where P 0 is the unique solution to the algebraic Riccatti equation (ARE),

ATP + PA − PBR−1BTP + Q = 0. (2.2)

The optimal cost for (2.1) is V (z(t)) = z(t)TP z(t) and the resulting closed-loop systemz ˙ = (A + BK)z is exponentially stable [244,245]. The discrete time version of this control method is used in Chapter 4 for disturbance rejection and the ARE solution is used as a terminal cost for the finite-horizon control problem to guarantee stability. Another important LQ method is the linear quadratic Gaussian (LQG) controller. This method assumes that the disturbances to the system and the measurement noise are stochastic. The objective of the controller is to minimize the expected value of the quadratic cost in (2.1). The solution to this problem is given by the feedback law u(t) = Kzˆ(t), where the controller K is the LQR controller for the system without noise or disturbances, and the system statesz ˆ(t) are estimated using a Kalman filter14 on the measured states and control input request. [246] These control methods have the advantage of guaranteeing stability, being optimal with respect to the designed cost function, and easily tuned due to the direct penalty of control and states of the system. Furthermore, these methods are easy to use due to the wide availability of tools to design them (e.g., MathWorks Control System Toolbox [247]). On the other hand, these methods require good mathematical models of the system, this is specially true for the LQG where there are no guaranteed stability margins [246, 248]. Moreover, LQR requires that all system states are measured and available for feedback which is normally not the case for real systems, this is solved with the Kalman filter on the LQG.

13Given that the pair (A, B) is controllable and (A, Q1/2) is observable. 14Also known as linear quadratic estimation (LQE). 2.1. AUTONOMOUS VEHICLE MODULES 51

As expected, LQ control methods have been applied to longitudinal control of autonomous vehicles. In [249], Alam et al. present a decentralized LQR controller for a platoon of HDVs. The proposed controller is string stable, and simulations with a platoon of six vehicles showcase its effectiveness. Alam et al. extend their work in [250] by considering bumpless switching between the brake and motor controllers, as well as designing two LQRs to generate references for these controllers. Experimental results with three Scania tractor-trailer HDVs are presented. The platoon is able to stay together, since the third vehicle does not undershoot more than the preceding vehicle, i.e., the distance error between vehicles is being atten- uated throughout the platoon. Once more, the authors extend their work in [50], where the decentralized LQR based CACC is tested using three Scania tractor- trailer HDVs on Swedish public highways. The controller is able to maintain the desired inter-vehicular spacing. However, small disturbances, caused by varying road grade, are observed in both velocity and spacing for the follower vehicles. Moreover, the fuel consumption is evaluated and the following vehicles are shown to save up to 6.5 % fuel and 8.6 % energy. Stankovićet al. in [251] present a longitudinal LQ controller for a platoon of vehicles. The method is string and asymptotically stable, while tracking given reference signals and the preceding vehicle in the platoon. Simulations of a ten vehicle platoon, show how different velocity profiles and changes in inter-vehicular distance are tracked. Stankovićet al. extend their work in [252] by proposing an LQG control method that makes use of the previous LQ together with a Kalman filter. The robustness to stochastic noise is shown through simulations of an eight vehicle platoon. As expected, the LQG controlled platoon vehicles track the desired velocities and distances without noticeable oscillations, while the LQ controlled platoon has small oscillations. Lateral control of autonomous vehicles has also seen applications of LQ methods. In [253], Hsiao and Tomizuka propose a lateral position LQR controller. The controller is experimentally tested around a test-track and compared to a look-ahead controller. The LQR has slightly better performance, presenting slightly smaller error to path and a smoother steering control input. Kim et al. in [254] propose a LQR for lateral control of a vehicle together with a PID for longitudinal control. The controllers are verified in simulation, where a test-track following scenario is presented. Jeon et al. propose an LQG for front steering wheel control in [255]. The authors propose two LQG controllers, one with road curvature information and one without. The controllers are experimentally tested using a Hyundai IONIQ on a path following scenario with speeds up to 36 km h−1. The controller with road curvature information has a path tracking performance more than two times better. Snider in [194] implements three different LQ methods together with the simulator CarSim [197]: the first only considers feedback control; the second considers both feedback and feed-forward actuation; and the third considers also a look-ahead distance. As discussed for other control methods, higher weight on lateral error to path leads to smaller tracking errors, but it might also cause instability at high speeds. The author concludes that the LQR with look-ahead provides some ro- 52 bustness to disturbances. However, it cuts more corners and overshoots more than the second feed-forward one. Comparing to other methods (PPC and Stanley) the LQR with feed-forward seems to be the best approach. More specifically, in the road course simulation it is able to stabilize the vehicle for all velocities simulated, and provides the smallest lateral error to path. Reversing a truck-trailer vehicle is one of the hardest tasks a human driver has to do with HDVs. To address this problem Ljungqvist et al. in [256], propose a LQ control method based on a kinematic model for the system. The authors prove closed-loop stability for a set of paths and showcase their method in simulation. Ljungqvist et al. extend their work in [257], by considering a lattice based planner that creates a kinematically feasible and obstacle free path for the hybrid LQ controller to follow. The controller is hybrid, due to the two different feedback gains for forward and reverse motion. The approach is verified both with simulations and experiments using a Scania tractor-trailer HDV. Experiments are successfully run for figure eight following while reversing, for a two-point turn maneuver, and for a T-turn maneuver.

2.1.6.6 H2 and H∞ Control

H2 and H∞ control are robust control methods that model uncertainties and parameter variations to obtain an optimal control law for the system [222,246]. As the name implies H2 control minimizes the H2-norm, and H∞ control the H∞-norm, for the transfer function of the system. These controllers are able to handle colored disturbances and measurement noise, as well as frequency weighting of the input and controlled output [258]. Following Skogestad and Postlethwaite book [246] notation, consider the state- space realization of a system P , the controller K, the closed-loop transfer function of the system given by the linear fractional transformation Fl(P,K), and for simplicity F , Fl(P,K). Then, the H2 optimal control problem is to ﬁnd the stabilizing controller K that minimizes,

s Z ∞ 1 H kF k2 = tr F (jw) F (jw) dw, 2π −∞ where tr(·) is the trace of the matrix, and the notation F H is the conjugate trans- pose of F . It is hard to understand what is being minimized, since it is deﬁned in the frequency domain. For ease of understanding, note that the time domain LQG control method mentioned previously is a special case of H2 control where white noise is considered. Moreover, H∞ control was initially motivated by the shortcomings of the LQG controller. In 1981 Zames published [259], where he argues that the poor robustness of the LQG can be attributed to the integral criterion of the H2 norm, as well as the unrealistic representation of uncertainties by white noise. 2.1. AUTONOMOUS VEHICLE MODULES 53

The H∞ optimal control problem is to find all stabilizing controllers K that minimize, kF k = max σ¯ (F (jw)) , ∞ w whereσ ¯ is the maximum singular value of the matrix. In general, H2 control is minimizing the root-mean-square value of the errors being controlled, and H∞ control is minimizing the worst case `2-norm of the errors being controlled. [246] H2 and H∞ control methods have advantages over other methods, as these are, by design, stable and robust to disturbances and uncertainties, and are readily applicable to multivariate systems with cross-coupling between channels. On the other hand, they do not handle well nonlinear constraints (e.g., actuator saturation). They require a high level of mathematical understanding and are not easy to apply. They require a reasonably good model of the system and, depending on the weighing/shaping transfer functions, they might result in high-order dynamic controllers that are complex to realize. [222, 260] Both H2 and H∞ methods have been proposed for vehicle longitudinal control. 15 In [261], Morand et al. compare PI, H∞, and CRONE controllers. The authors present frequency response analysis for all controllers to conclude that the CRONE controller is better, since it does not break the desired constraints, while the PI and H∞ do so in some cases. However, the authors also present step responses and wind disturbance rejection for all controllers, and the H∞ has the best performance, since it does not present overshoot and has a faster rise-time than the PI and CRONE. Xu et al. in [263] consider a decentralized robust H∞ controller for a platoon of vehicles. The authors take into consideration varying road-slopes, aerodynamic drag, and wireless communication delay. The robust controller is compared against a regular controller that can ensure string-stability if disturbances are not considered. Not surprisingly, the proposed controller is significantly better. Morbidi et al. in [264] consider decentralized CACC comparing an LQR with mixed H2 and H∞ control designed via compensator blending. Through simulation, the authors showcase that both approaches are able to achieve a string stable platoon, concluding that the mixed H2 and H∞ controller is considered better, because the same scenario is performed with smoother and less control input. In [265], Schmied and Colaneri extend [264]. However, instead of considering control of a full platoon, only one vehicle is controlled on multi-lane scenarios, where lane changing and merging scenarios are considered. The authors showcase their approach by studying how the controlled vehicle behaves and also how it affects the other traffic participants. The mixed H2 and H∞ controller is able to maintain string-stability of other traffic participants by making lane changes and adapting the longitudinal behavior of the ego vehicle. When it comes to the lateral control problem, H2 and H∞ methods have also been used. In [206], Hima et al. present an H∞ controller that uses the front wheels steering angle to perform lane following and changing scenarios. The approach

15This controller is a specific method based on regulator design with robust loop shaping [262]. 54 achieves a good performance at high speeds for the straight parts of the path. Shimakage et al. in [266], present an H2 method for lane-keeping. Torque control of the steering-wheel is used to turn the front-wheels of the vehicle. Robustness against disturbances and parameter variation is shown by µ-analysis (see definition in [246]) and a step response simulation with parameter variation. A straight lane following experiment shows effectiveness and smoothness of the approach, when compared to human driven results. As mentioned before, in [226], Chaib et al. use simulations to compare four different controllers (H∞, adaptive, proportional, and fuzzy control) over a test track circuit with varying speed, while being perturbed with wind and different road surfaces. Although the adaptive controller is concluded to be the best one, it is also the most complicated to implement. On the other hand, the H∞ controller has good performance and it is simpler to implement. Hwang and Chang in [267] propose a mixed H2 and H∞ control method for path following. The approach is verified with both static and dynamic obstacle avoidance while following a path. Additionally, the authors compare their approach with a PID where they conclude mixed H2 and H∞ method is superior at handling uncertainties. In [268], Xu et al. propose a mixed H2 and H∞ control method for a 4WS vehicle. The authors compare the approach with both a P and a H∞ controller for step response, sinusoidal, fishhook, and double lane change scenario simulations. The mixed H2 and H∞ controller has a better yaw-rate tracking performance, and achieves less overshoot, smoother control requests, and smaller side-slip angles.

2.1.6.7 Model Predictive Control Model predictive control (MPC) methods were originally developed in the 70s to control power plants and petroleum reﬁneries [269, 270]. Decades ago, MPC could only be used for processes with slow dynamics, because of the high computational cost and the low computational power available. Nowadays, MPC is widely used and studied for multiple applications. Both due to the much higher computational power available and the improvement in solving methods for the optimization problem [271]. The trajectory tracking optimization problem can be written as,

Z tg min L(¯z(t), u(t)) dt u tc s.t.z ˙(t) = f(z(t), u(t)), (2.3) z¯(t) = z(t) − zr(t), z(tc) = zc, z(t) ∈ Z(t), u(t) ∈ U(t).

The objective is to compute the necessary control input, u, from the current time, tc, to the goal time, tg, to track a desired trajectory, zr. The minimization is done according to the stage-cost function L, that penalizes the difference from 2.1. AUTONOMOUS VEHICLE MODULES 55 the reference trajectory and the control input used. Furthermore, the tracking starts in the current position, zc, and it needs to respect the system dynamics, f(z(t), u(t)), as well as the allowed state-space and actuator constraints, Z(t) and U(t). Equivalently, the path following problem can be written by neglecting the time dynamics of the system and only considering the state of the system in space. In order for a computer to solve (2.3), the continuous optimization problem is discretized and the integral is changed to the sum of points [272]. Then, a receding horizon control (RHC) approach is used. Where the optimal control inputs are computed over a fixed number of prediction horizon points, N, and where only the first control input is applied to the system at each time step. On the next controller iteration, the procedure is repeated for the shifted horizon reference points and the updated system states [271]. The number of horizon points, together with the number of constraints, and the complexity of the system model, influence the problem complexity. Higher complexity makes the optimization problem harder to solve, making it harder to apply the control method in practice. The difficulty to solve the problem in real- time is the major disadvantage of MPC. Other disadvantages compared to other controllers are: • it requires an accurate model (or good-enough model) - the problem complexity grows with the accuracy of the model; • it requires an optimization algorithm - normally, an optimization problem is solved online at each iteration and the solver does not guarantee convergence to the optimum in bounded time; • guaranteeing stability and feasibility for the system is not simple and usually increases problem complexity; • increasing robustness to disturbances for the controller usually also increases problem complexity. The biggest advantage of MPC is its ability to explicitly handle both input and state constraints, which can represent: physical, economical, safety, and/or comfort constraints. Other controllers, are usually not able to handle all constraint types, and, even if it is possible to introduce constraints, this is normally done in an ad-hoc fashion making it much more complex to implement and interpret. Other important advantages are: • simple control policy for all types of systems - linear and nonlinear, single or multiple input with single or multiple output; • optimal controller with respect to a designed cost, which can be related to tracking performance, comfort, and/or economical reasons (usually suboptimal due to the RHC approach); • well understood tuning parameters; 56

• straightforward, ﬂexible, intuitive, and easy to read problem formulation - where the control objectives can be explicitly optimized in the cost function, and the system constraints directly taken into consideration; • takes into consideration future dynamics of the system and reference changes, acting accordingly; • development time is much shorter than other advanced control methods; • easy maintenance, i.e., changing system model or performance requirements does not require complete redesign and can sometimes be done online. The MPC method advantages, from a theoretical point of view, far outweigh the disadvantages. On the other hand, from a practical (implementation) point of view, the advantages outweigh the disadvantages if the method is implementable in real-time. Thus, for these reasons, MPC is the main control method studied in this thesis, and considerations on practical implementation capabilities are always done throughout Chapters 4-5. The following literature review for MPC methods applied to autonomous driving is more complete than the ones presented before for other methods, since MPC is the main method studied in this thesis. The rest of this subsection is divided into two main areas, nonlinear MPC and linear MPC.

Nonlinear MPC Nonlinear MPC (NMPC) as the name entails does not explicitly16 linearize neither the model nor the constraints and solves a problem of the form,

min J(¯zi|t, ui|t, z¯N|t) ut

s.t. z˙i+1|t = f(zi|t, ui|t), gm(zi|t, ui|t) 6 0, z¯i|t = zi|t − zr,i|t, z0|t = zt = zc, N−1 M ∀i ∈ N0 , ∀m ∈ N1 , where, J is the desired function to optimize and is usually of the form,

N−1 X J(¯zi|t, ui|t, z¯N|t) = jf (¯zN|t) + j(¯zi|t, ui|t), i=0 where j can be non-convex. Furthermore, all the state constraints are expressed in inequality constraint form making use of the real-valued functions g1, g2, ..., gM .

16Note that the solving method might do successive linearizations to ﬁnd the solution for the problem. However, if it converges to some solution the nonlinear problem is solved (it might be a local solution). 2.1. AUTONOMOUS VEHICLE MODULES 57

NMPC has been studied for longitudinal control of vehicles. In [273], Matute et al. present an NMPC based CC, where the objective is to track a desired speed profile while maintaining velocity, acceleration, and jerk constraints. Simulations showcase the advantages of this approach in adapting the velocity profile to account for future changes, achieving a smoother and more comfortable behavior than initially planned. Shakouri et al. in [274] use a NMPC method for ACC. The controller does distance and velocity tracking by controlling the brake and throttle of the vehicle. This approach is compared to a gain-scheduled LQ controller for throttle and a PI controller for the brake, a control architecture that had been proposed by the authors in [275]. The control approaches are compared in simulation in an ACC scenario at high-speeds and in a stop & go scenario. Overall, the NMPC controller is far smoother and more comfortable than the LQ-PI controller. Moreover, in the stop and go scenario the LQ-PI controller does not react/brake enough and causes a potential collision with the front vehicle, while the NMPC is able to stop safely. Lang et al. study the fuel benefits of MPC applied to CACC in [276]. The authors study and compare nonlinear and linear MPC to solve the control problem. The controllers objective is to minimize the fuel consumption, while maintaining a certain distance from other vehicles and keeping the velocity and acceleration within certain intervals. First, the full nonlinear problem is solved offline where the fuel benefits of allowing more or less distance between vehicles is studied. The authors conclude that bigger spacing between vehicles allows for smoother and calmer accelerations leading to higher fuel savings (in this study air-drag reduction by forming a platoon was not taken into consideration). However, the trade-off is that higher inter-vehicle distance leads to diminished road throughput. Second, the optimization problem is solved with RHC NMPC, where the fuel savings of different allowed distances and prediction horizon lengths are studied. The authors conclude a prediction horizon of 20 s and a control horizon of 10 s give a very similar result when compared to the full offline optimization. Finally, the nonlinear approach is compared to a linear MPC previously proposed by the authors in [277]. The authors conclude that the linear MPC obtains very similar results, with the benefit of being easier to implement. Schmied et al. in [278] propose NMPC for CACC of a following vehicle on a two vehicle platoon. The authors consider interactions with a traffic light, which allows the controller to better plan its acceleration and deceleration maneuvers. Hardware- in-the-loop (HIL) experiments on a dynamic test bench are used to measure fuel consumption and pollutants emissions. The scenario consists of two vehicles with multiple stop & go maneuvers caused by traffic light interactions. The NMPC is able to maintain an acceptable longitudinal distance with smooth and anticipatory behavior. Which results in a 13 % fuel consumption reduction and up to 28 % pollutants emissions reduction, when compared to the front vehicle. The use of MPC for lateral control of autonomous vehicles continues to be widely studied. Topics range from lane-keeping in highways, to city driving, racing, 58 and off-road. Furthermore, vehicle handling, stability, and robustness are still open problems to solve. Borrelli et al. in [279] propose a nonlinear MPC approach for active-steering control. The method uses the front steering angle to stabilize the vehicle along the desired path, while obeying to physical actuation limits. The controller is verified in simulation performing an obstacle avoidance double lane change maneuver on snow, with different entry speeds. The nonlinear solver takes 0.1 − 1.7s, which makes it hard to implement on a real system. This work is extend by Keviczky et al. in [280], where side wind rejection and controller stability are studied on a similar scenario. Both works, discuss the length of the prediction and control horizons. The necessary horizon lengths for stability depend on the speed of the vehicle, and, as expected, high speeds with short horizons lead to instability. Falcone et al. in [281] extend [279, 280], where an additional convex constraint bounding the control effort and predicted states is computed to guarantee stability. Furthermore, the authors solve the optimization problem using SQP, i.e., the controller solves multiple QP problems sequentially until it converges to a solution for the nonlinear problem. In simulations, this approach is compared to a linear MPC, which only uses an ad-hoc constraint on the front tire slip angle to stabilize at high speeds. Although, the authors conclude the performance of both controllers is identical, the nonlinear controller is more general and provides a better guarantee of stability than the linear one. On the other hand, the nonlinear controller requires more computation time. Falcone et al. in [282] use their previous control formulation [279] to study the difference between using a complex (tenth-order) model or a simplified bicycle model in the controller. The models have different computational complexities, and also different control inputs. The complex one uses front steering angle and individual wheel braking torques. The simpler one uses front steering angle and braking yaw moment. Simulation of double lane change shows similar performance with both controllers for entry speeds of 14 m s−1. However, for entry speed of 19 m s−1 the lower order model is not able to stabilize the vehicle (note that there are no stability constraints in either controller). Kong et al. in [283] also compare vehicle models for MPC design. Specifically, the authors compare kinematic vs dynamic vehicle models and verify those results by implementing an NMPC on different scenarios. Depending on the discretization method and time, the kinematic model can give accurate results in comparison to the dynamic model. More interestingly, comparing open-loop prediction of the models with real data, the authors conclude that errors obtained with the kinematic model discretized at 200 ms are better than at 100 ms, when compared to the dynamic model errors discretized at 100 ms. Yoon et al. study NMPC for obstacle avoidance in [284]. The authors propose a modified-parallax-based obstacle avoidance approach, and compare to a distance based and potential field based approaches. The parallax and distance based controller provide much smoother paths avoiding the obstacles than the potential field one. The parallax method is better, because it has less computational time than 2.1. AUTONOMOUS VEHICLE MODULES 59

Fig. 2.10: Experimental results obtained with 1:43 scale RC cars (courtesy of Roso- lia et al. [191]). Nonlinear MPC ideal trajectory in red, measured tracked trajectory in black, desired path (center line of the road) in dashed-gray, and the obstacles represented by the blue shapes. the distance based controller. In [285], Leman et al. propose NMPC for obstacle avoidance and compare their approach to the Stanley controller. The authors simulate a scenario with different speeds, showing that the NMPC is able to track better the path and be more stable than the Stanley controller. In [191], Rosolia et al. propose a two-stage controller, with a NMPC in the outer-loop, doing the obstacle avoidance, and a PI with feed-forward controller tracking the optimized trajectory. Moreover, the authors propose a solving method for the optimization problem and compare it to a common solver (IPOPT). The proposed solver is significantly faster (<0.15 s vs <0.9 s), but it has the disadvantage of needing a slack variable to guarantee feasibility. The approach is verified with experiments using 1 : 43 scale RC cars around a track see Fig. 2.10, where the desired trajectory by the NMPC controller17 is tracked by the PI controller using torque and steering input.

17This is one of those cases where it is a gray area whether the NMPC is a controller or a planner. The approach is considered a controller due to the knowledge of errors to the reference path by the outer-loop. 60

Fig. 2.11: Experimental results obtained with a passenger car on snowy/icy conditions (courtesy of Gao et al. [287]).

Gao et al. in [286] propose two different NMPC controller schemes to achieve obstacle avoidance on slippery roads. The first solves a single NMPC problem penalizing the cost of being close to obstacles in order to avoid them. The second is a planner controller scheme: the planner NMPC uses a simple point-mass vehicle model to avoid obstacles; the controller NMPC uses the more complex model as in the first approach to track the received reference path. Experiments are run for a double lane change obstacle avoidance scenario on icy/snowy conditions. The two stage approach is better than the one stage, because , not only it maintains stability for higher entry speeds, but also reduces significantly the computational time of the controller. In [287], Gao et al. extend their work by using a spatial bicycle model on the planner NMPC. This allows to more easily setup the obstacles as constraints simplifying the cost function. Moreover, the authors use SQP with linear time-varying MPC (LTV-MPC) to solve the NMPC controller problem. The more accurate model advantages on the higher-level planner are clear when compared to previous work. Fig. 2.11 presents experimental results obtained with a passenger car on snowy/icy conditions at 14 m s−1. As expected, the controller does not track perfectly the planned path, specially when avoiding obstacles. In related work [288], Gray et al. also propose a planner controller scheme, where motion primitives are used to plan and avoid obstacles, and an NMPC follows the planned path. The approach is verified by avoiding an obstacle and track the path on a double lane change scenario with low road friction as in [286]. The advantage over the previous approach is that selecting motion primitives is faster than running another optimization. In [289], Carvalho et al. use NMPC to track the lane center-line while avoiding obstacles. An SQP approach is used to sequentially 2.1. AUTONOMOUS VEHICLE MODULES 61 solve LTV-MPCs until convergence of the control input or a maximum number of iterations is reached. Both static and moving obstacles are considered in performed experiments on packed snow. One obstacle is moving in a straight line at 10 m s−1 and, initially, the ego vehicle is behind it, approaching at 22 m s−1. The ego vehicle is able to brake and steer, performing the double lane change and the overtake. MPC has also been combined with other methods to achieve better control performance. One such combination, is to apply learning to better handle disturbances on repetitive tasks. For example, consider a truck always traversing the same path to load and unload at the same places. If the disturbances can be modeled and accounted for the vehicle will be able to track the path more accurately. Ostafew et al. in [290] propose a learning based NMPC to follow the same path multiple times. A kinematic model with disturbances modeled as a Gaussian process is used. Fig. 2.12 presents a robot and the results obtained with it at 0.4 m s−1. Road and model disturbances are learned over the runs, modifying the Gaussian process. The learning is mostly done over the first five runs, reducing both the lateral and heading errors to path by 50 %. The authors extend their work in [291], where they compare a P controller and a NMPC to the learning based NMPC. As expected, the learning controller is able to anticipate reference changes and disturbances, the NMPC is only able to handle reference changes, and the P controller acts only after the changes or disturbances. Furthermore in [291], the authors extend the experiments to a car-like vehicle. As for the smaller robot, the algorithm and controller quickly reduce the errors to path by over 50 %. The NMPC optimization problem is difficult to solve in real-time and guarantee convergence to the minimum. An approach to address this is to use explicit MPC (eMPC) [292], where the MPC problem is solved offline and a piece-wise control law is derived for each region of the state-space. Online, the controller evaluates convex polytopes to determine in which region the system is on, and computes the control input accordingly. In [293], Di Cairano et al. propose an eMPC method to track the driver’s requested yaw-rate using differential braking and active front steering. The authors note that eMPC can be executed with automotive-grade electronic control units and showcase it with experiments on a passenger car. An experiment at 14 m s−1 doing a double lane change on a icy/snowy condition, shows that the driver is not able to stabilize the vehicle alone. With the control method the vehicle stabilizes and the tire slip angles are maintained at the desired saturation points. Some nonlinear systems can be described by linear parameter-varying (LPV) systems, which are defined as linear systems, where the system matrices are not constant but depend on some time-varying parameters [294]. eMPC is interesting for such systems, because by bounding the LPV system parameters, the number of regions and control laws are also bounded. Besselman et al., in [295] and [296], applied explicit MPC to a discrete-time LPV system, where the scheduling parameter is measured online and known to the controller, but future values of it are only known to lie in a simplex. More recently, in [297], the rate of the parameter variation is considered to be bounded, which allows for improvements in the performance of the controller. Finally, the authors in [298] provide a procedure to 62

(a) Robot used.

(b) Errors to path evolution over multiple runs.

Fig. 2.12: Learning based NMPC example (courtesy of Ostafew et al. [290]). bound the solution suboptimality. In [298], the authors also gather previous results on constraint satisfaction, recursive feasibility, and asymptotic stability. Another way of reducing/limiting the computation time taken by NMPC approaches is to solve the problem by using an SQP approach and stopping it before convergence. For each iteration of the LTV-MPC the accuracy of the linearizations is improved, and gets closer to the optimum of the nonlinear problem. The solution time can be bounded18 by stopping after a number of iterations, and using the solution at that time. Lima et al. in [299]19 explore this idea to implement the progress maximization MPC (PM-MPC). The objective of the controller is to maximize the progress along a road, i.e., have minimum traveling time (e.g., a racing application). Furthermore, a polytopic approximation of the g-g diagram is proposed to respect the lateral and longitudinal acceleration constraints of the vehicle. The vehicle is modeled as a

18This is only valid if the solver used for each QP is able to provided a bounded solution time. 19This thesis author is a co-author of this work. 2.1. AUTONOMOUS VEHICLE MODULES 63 road-aligned kinematic bicycle model, and obstacles are considered by constraining the allowed error to path. Fig. 2.13 depicts the simulation results for the PM- MPC, with two successive iterations, traversing an S-shaped road with obstacles. As expected from a racing application, the vehicle makes use of the road width to speed up while turning. When compared to the globally solved solution of the same scenario, the PM-MPC takes around 5 % more time to traverse the path, but it is significantly fast to compute making it real-time implementable. Ultimately, the idea explained before leads to what is called real-time iteration (RTI) MPC. In RTI-MPC, only one QP problem is solved per controller iteration using a linearized system model and constraints, and using the previous controller iteration shifted solution as linearization points [300]. More details can be found in [300], where Gros et al. clarify the similarities and differences between linear MPC and NMPC based on a RTI scheme. The authors give simple examples to showcase the differences between linear MPC, RTI MPC, and fully converged SQP MPC. There is a significant difference between the first two, but very small difference between the last two (with the advantage of only solving one QP per iteration). These results are further verified experimentally, where the importance of shifting the solutions between iterations is highlighted. RTI-MPC has also been used for vehicle applications. For example, Duijkeren et al. in [301] use it for highway driving trajectory control of long HDVs. The authors consider both steering angle rate and longitudinal jerk as control inputs for the vehicle. A lane merge scenario with other traffic participants is presented, where the results showcase the safety of the approach, maintaining longitudinal distance from other agents. Furthermore, the RTI scheme is compared to solving the full problem until convergence, showing that RTI is significantly better, since the optimality loss is very small, but the solving time per controller iteration is up to 90 times faster.

Linear MPC

Linear MPC (LMPC) solves a convex problem, with a linear system and linear constraints. A common formulation is,

s.t. z¯i+1|t = Ai|tz¯i|t + Bi|tu¯i|t, Ctzt + Dtut + ht 6 0, (2.4) z¯i|t = zi|t − zr,i|t, u¯i|t = ui|t − ur,i|t, z0|t = zt = zc, N−1 ∀i ∈ N0 , 64

20 20

0 18

[m] 20 16 y −

40 − 14

60 − 0 20 40 60 80 100 120 12 x [m] 4 10 2

[m] 0 y e 2 8 − 4 − 6 0.5

[rad] 0

ψ 4 e 0.5 −

0 50 100 150 200 250 300 350 2 1 s [m] Optimal path speed [m s− ]

Road boundaries Centerline

Fig. 2.13: PM-MPC simulation results (originally presented in [299]). The top plot shows the trajectory in the global frame and the bottom two plots in the road- aligned frame. The road boundaries vary to represent obstacles on the road, and the trajectory color represents the speed of the vehicle. 2.1. AUTONOMOUS VEHICLE MODULES 65 where, the quadratic cost of the error to the trajectory and the control input are minimized20. Furthermore, all state and control input constraints are expressed linearly in inequality constraint form making use of the matrices C and D, and the vector h. In fact the formulation presented in (2.4), is that of an LTV-MPC because of the time dependency21 of matrices A and B. The original linear MPC formulation has matrices A and B constant. Linear MPC has been widely applied to both lateral and longitudinal vehicle control problems due to its real-time capabilities. In [302], Bageshwar et al. propose LMPC for ACC. The authors use simulations to compare different formulations (with and without constraints) on vehicle following scenarios. As expected, when both the acceleration limits and collision avoidance constraints are implemented, the vehicle is able to anticipate and avoid collisions that a simpler controller would not. Li et al. in [303] also propose an LMPC for ACC of an HDV. The controller takes into consideration the ego vehicle’s acceleration response, as well as, the velocity and acceleration of the preceding vehicle. The LMPC approach is compared to an LQR ACC. Over multiple simulations the MPC approach is better, avoiding collisions, having a better tracking performance, and saving more fuel than the LQR. Bu et al. in [304] implement linear MPC for CACC on a Infiniti (Nissan group) passenger vehicle. The approach makes use of the factory installed ACC by modifying the measurements and reference requests sent to it. Experiments comparing the factory ACC to the proposed CACC, show that the CACC far outperforms the ACC. Furthermore, the authors raise concerns about string stability if only the factory ACC is used. In [277], Stanger and Re present a LMPC for CACC directly minimizing the fuel consumption of the vehicle. The MPC is compared to a PI ACC using simulation, where its advantages are clear. Not only, safety distances to other vehicles are maintained, but also there is a clear improvement in fuel consumption and ride comfort. Furthermore, the authors study different lengths of prediction horizon, and how they influence fuel consumption and road capacity, showing that bigger horizons lead to bigger gains. As mentioned before, in a more recent publication by the authors [276], this approach is compared to NMPC. The authors conclude that the linear MPC is better, because of faster computation times and small loss of optimality. Falcone et al. in [305] introduce the LTV-MPC framework applying it to vehicle lateral control, and showcasing its capabilities against the NMPC proposed in [279]. The linearization of the system and constraints is done around the desired path to be followed. The controller makes use of the front tire steering angle to control a coasting vehicle during a double lane change on a snowy road. The NMPC cannot be implemented in real-time while the LTV-MPC can, and, by comparing the results obtained at 15 m s−1, the LTV-MPC only has a small loss of performance, so its

20Other cost functions can be considered in the context of LMPC, given that they are convex functions. The quadratic cost function is presented, for its wide use in previous literature. 21Some formulations consider other dependencies (e.g., space), but the formulation is still called linear time-varying to express the varying nature of the system model. 66 advantages are clear. Falcone et al. extend their work in [306], where higher entry speeds and experimental results are presented. Even constraining the number of iterations the solver could do, the NMPC could only be tested up to 7 m s−1. On the other hand, the LTV-MPC is able to stabilize the vehicle up to 21.5 m s−1 for the scenario considered. In [307], Falcone et al. further extend their controller by considering active braking and active differentials. The authors compare their approach with other LTV-MPCs that only consider steer and braking intervention, or only steering intervention. In simulation, compared to the steer and braking controller, the proposed approach is able to handle higher velocities, but it presents slightly more error to path. Compared to the steering only LTV-MPC, the proposed controller is overall better, with significant less error to path. In [308], Falcone et al. propose a planner-controller scheme for the lateral control, where the controller used is the LTV-MPC proposed in [306]. The authors showcase the advantages of having a planned reference path that takes into consideration vehicle stability limits, showcasing how it allows to have a smaller prediction horizon and still achieve a slightly better tracking performance of the original path. Many other problems have seen linear MPC solutions. In [309], Timings and Cole use a successive linearization RTI LTV-MPC to optimize the steering and torque input of a racing vehicle, while minimizing maneuver time. The controller is verified by simulating a 90◦ bend, where tire force saturation levels are verified and the controller is seen to perform a racing line. The controller is also compared against an NMPC, which achieves a slightly higher apex speed, because it is able to use more tire lateral capacity by increasing steering oscillation. Turri et al. in [310] propose LTV-MPC to perform lane keeping and obstacle avoidance on low curvature roads. The approach solves multiple independent QPs per iteration, where each QP uses a different longitudinal velocity profile. Then, out of all solutions, the minimum cost inputs are chosen and applied to the vehicle at each iteration. The controller is implemented in a Jaguar passenger vehicle, and is able to perform a double lane change at 50 km h−1 to avoid an obstacle on a snowy road. In [189]22, Collares Pereira et al. present an LTV-MPC for lateral control of an over-actuated vehicle. The research concept vehicle (RCV) is a 4 wheel-steered (4WS) electric vehicle. This vehicle can move sideways by turning all wheels in the same direction, which is called crabbing motion. The controller makes use of an extended kinematic bicycle model to control both curvature and the crabbing angle, which are then translated to individual wheel angles using Ackerman geometry. Fig. 2.14 shows experimental results obtained for a lane change maneuver in snowy/icy conditions. It is clear the vehicle is moving sideways using the maximum allowed crabbing angle during the maneuver. Kim et al. in [311] propose MPC for path tracking taking into consideration the steering actuator dynamics. The formulation includes constraints on maximum and

22This thesis author is a co-author of this work. 2.1. AUTONOMOUS VEHICLE MODULES 67

(a) Position and orientation of the vehicle. Note how the vehicle is pointing front and moving sideways making use of the crabbing motion.

0.15 0.2

0.04 0.1

) 0.02 0.1 0.05 -1 0 (rad) (m β κ 0 -0.02 0

-0.04 -0.05 Lateral Error (m) -0.06 -0.1 -0.1 0 20 40 0 10 20 30 40 X (m) X (m) (b) Curvature, κ, in blue, and crabbing an- (c) Lateral error of the vehicle to the path. gle, β, in red.

Fig. 2.14: Experimental result for a lane change maneuver with a 4WS vehicle (originally presented in [189]).

minimum steering angle and rate, where the maximum angle is quadratically inverse to the velocity of the vehicle, bounding its lateral acceleration. Simulations are used to verify different controller aspects. First, different model orders for steering dynamics are studied, showing the second order model is good enough. Second, the prediction horizon length is studied without varying the control horizon, where the computational times are measured. Finally, a double lane change scenario at different velocities is used to verify the controller. In [312], Ji et al. propose an MPC for tracking paths generated with potential fields considering road obstacles and boundaries. The controller considers constraints on lateral and yaw errors to path, sideslip angles, and steering actuation. The proposed MPC is compared to a simpler version, without state and sideslip constraints. Only the more constrained version is able to perform a double lane change at 20 m s−1 on a low friction road. 68

Graf Plessen et al. in [313] propose a spatial-based LTV-MPC for path tracking and obstacle avoidance. The controller actuates on throttle, brake, and steering inputs of the vehicle. Simulation results for multiple scenarios, considering static and dynamic obstacles are presented. The most interesting results are presented in Fig. 2.15 where multiple obstacles are considered while tracking a winding road. Fig. 2.15 presents snapshots in time of the scenario, where the ego vehicle (in blue) is able to overtake multiple obstacles, while taking into consideration oncoming traffic (red shapes). In literature is common to assume that the obstacles positions are known and/or that the feedback-loop is able to address system disturbances. However, in reality, there are always uncertainties, disturbances, and unmodeled system dynamics. If they are accounted for in the control design, most likely a better closed-loop performance is achieved. Stochastic MPC makes use of probability distributions to model uncertainties and chance constraints to account for these uncertainties, while penalizing the expected value in the cost-function. Gray et al. in [314] propose a stochastic MPC for lane keeping and obstacle avoidance, while taking into consideration driver intervention. The driver intervention is modeled with a normal distribution around a designed estimate using the reference path and current state. Two obstacle avoidance scenarios are simulated. In the first, the actuation of the driver would lead to a collision, but with the help of the controller the vehicle is able to avoid it. Fig. 2.16 presents the results of the second simulation, where ten randomly disturbed runs and the worst-case disturbance (δˆ ± 3Σ) runs are considered. The vehicle is able to maneuver around the obstacles and avoid collisions. Liniger et al. in [315] propose a RTI LTV-MPC for longitudinal and lateral control of RC cars. The objective is to maximize progress along the track in order to win a race. Both the motor and steering are controlled, while taking into consideration other vehicles and track limits. The approach is compared to a NMPC, that cannot have the same prediction horizon due to the computational power it would require. Experiments with 1:43 scale RC cars show that the linear MPC achieves a better lap time, both with an empty track and overtaking static obstacles. In [316], Liniger et al. extend their work by considering stochastic MPC and disturbance feedback. The vehicle model considers additive disturbance to provide robustness. Different stochastic approaches are compared, more importantly a smaller lap time is achieved when compared to the previously proposed approach in [315]. Carvalho et al. in [317] propose stochastic MPC to handle uncertainty aris- ing from other road participants. The approach starts by estimating the other vehicle’s trajectories using a multiple model Kalman filter, which considers typical maneuvers, such as, lane keeping, and lane changing. The control problem optimizes steering and braking inputs for the vehicle, while taking into account moving obstacles (posed as chance constraints), lane boundaries, and control inputs constraints. The first simulation results compare risk vs conservativeness of the controller by changing the risk parameter. Showing that with it at 0.5 (equivalent to the undisturbed nominal model) the vehicle makes a lane change in the 2.1. AUTONOMOUS VEHICLE MODULES 69

Fig. 2.15: Simulation time lapse of LTV-MPC avoiding static and dynamic obstacles on a double lane winding road (courtesy of Plessen et al. [313]). All road participants have their velocities indicated next to them in km h−1. In blue the controlled vehicle and the predicted horizon, in red the obstacles, in green the ego vehicle sensing capabilities, and in black the road boundaries. 70

Fig. 2.16: Ten randomly disturbed simulations using stochastic MPC (courtesy of Gray et al. [314]). The randomly disturbed trajectories are in red, and the worst- case disturbed trajectories are marked in dashed-blue. middle of two other moving vehicles, but, with it at 0.998, the controller decides to slow down avoiding a possible collision. The second simulation results are presented in Fig. 2.17, where snapshots in time are presented. The ego vehicle (denoted by E) slows down when vehicle T1 merges into its lane, and then waits for T2 in order to start overtaking T1. For an interested reader, in [318], Carvalho et al. present the previous approach as well as other related work, putting the stochastic MPC approach in a broader context for autonomous vehicles. Robust MPC is a thoroughly researched topic (see [319] and references therein for a complete survey on the topic) and is able to explicitly handle disturbances. Mayne et al. in [320–322], present a robust MPC approach for control of disturbed linear time-invariant23 (LTI) systems. The nominal system is subject to tighter constraints in order to guarantee exponential stability of a disturbance invariant set. In particular, [320] accounts for state disturbances, [321] extends the results to include output disturbances, and [322] includes the dynamics of a state estimator convergence. The controller makes use of a feedback gain to handle disturbances and to maintain the system inside an invariant set. The invariant set position, i.e., the nominal system position, is optimized online, and the control law is the sum of the feedback term and the optimal input for the nominal MPC problem. Bumroongsri in [323] extends the work done by Mayne et al. to LPV systems. The proposed controller steers the nominal LPV system to the origin and stability is guaranteed assuming that the time-varying parameter can be measured. However, this controller does not optimize the nominal state of the system, instead the time- varying parameter is measured at each step and the nominal system state computed.

23LTI means that the matrices A and B of the system are constant. 2.1. AUTONOMOUS VEHICLE MODULES 71

Fig. 2.17: Time snapshots of stochastic MPC simulation (courtesy of Carvalho et al. [317]). The red boxes depict the current positions, while the gray boxes depict the future positions. For each target vehicle, the ground truth is shown by the dashed red line.

These works are better introduced and extended for LTV-MPC in Chapter 4 of this thesis. The great advantage of the approach proposed by Mayne et al. is the low complexity of online computations, which allows for real-time implementation. Nev- ertheless, there are other approaches. In [324], Richards et al. propose a linear MPC approach for LTV systems, capturing the system linearization error in the disturbances. This approach makes use of a very constrained MPC problem in order to be robust. Each optimized state is constrained to be inside shrinking tubes obtained with a feedback control law (usually the LQR one). Comparing to Mayne et al. [320], all states are constrained by tight sets, while in Mayne et al. only the current nominal state has a tight set constraint, and it is an optimized 72 variable making it easy to solve online. When it comes to robust MPC applied to autonomous vehicles it is harder to find work with linear MPC. It is easier to find work for robust nonlinear MPC, e.g., [325–327]. Some of the mentioned approaches are robustifying work already discussed in this literature review. The reader is welcomed to read and dive into the topic of robust nonlinear MPC. One of the concerns when developing controllers is their ability to stabilize the closed-loop system. Falcone et al. have considered this extensively (e.g., [305,306]), and that is why the considered scenarios always include slippery roads. In [305], Falcone et al. note that the controller stability is achieved by careful tuning the cost function weights and by including vehicle dynamic constraints. This is an ad-hoc solution to the problem, and does not provide guarantees. However, this type of solution has been widely applied. Katriniok and Abel in [328] propose an RTI like LTV-MPC. The front steering angle is used to maneuver the vehicle, while taking into consideration maximum and minimum sideslip angles for the wheels, maintaining the tires in a good operating region. Simulation results of a double lane change in snowy conditions are used to compare successive vs reference model linearization. Concluding that using the predicted horizon to linearize improves both the tracking accuracy and the smoothness of the control request. In [329], Katrioniok et al. extend previous work to control also braking of the vehicle and propose an extended Kalman filter (EKF) to obtain offset free tracking. The controller is evaluated with a Volkswagen passenger vehicle on a dry rural road performing a double lane change. The vehicle starts the scenario at 20 m s−1 and is able to perform the evasive maneuver, while maintaining a lateral error to path smaller than 0.3 m. Controllers that consider the bounds of vehicle capabilities, maintaining its stability, can also be referred to as envelope controllers [330]. Beal and Gerdes in [330] propose an envelope controller for driver assistance. The envelope is done on yaw- rate and rear wheel sideslip angle. The optimized front wheel lateral force necessary for stability is used, together with the driver requested steering angle, computing the necessary steering angle to stabilize the vehicle. The controller is tested on the P1 (a research vehicle built at Stanford University) on varied road surfaces (mixed dirt, gravel, and asphalt). The controller is able to prevent the vehicle from entering unstable regions, for different velocity profiles. Erlien et al. in [331] propose an envelope controller to achieve both stable vehicle dynamics and obstacle avoidance. A noteworthy detail of the implementation is the use of varying discretization time-steps throughout the horizon. Which allows for the prediction of a long horizon, without adding more points and increasing problem complexity. Simulation shows controller capability in correcting the driver’s steering input, while avoiding obstacles and maintaining a stable behavior. Furthermore, a comparison between using or not a RTI scheme concludes that successive linearizations allow for higher entry speeds on the double lane change scenario. Erlien et al. extend their work in [332], considering the possible path decisions to avoid obstacles ahead and experimentally testing the controller. To formulate 2.1. AUTONOMOUS VEHICLE MODULES 73 the MPC as a QP, different paths are considered to avoid obstacles (e.g., go left or right). Then, a QP MPC is solved per possible path and the optimum is chosen. Fig. 2.18 shows experimental results obtained with the X1 (research vehicle built at Stanford). The scenario conforms with the ISO standard 3888-1 [333], consisting of a double lane change at more than 60 km h−1 on a low friction road. Interest- ingly, during the experiment the envelope is broken (which is allowed in the MPC formulation through the use of slack variables), because the collision avoidance constraints take priority over the stability envelope constraints. Still, the controller is able to recover in time, achieving the desired behavior. Another concern when solving constrained optimization problems is their feasibility, i.e., the ability to find a sequence of control inputs (optimized variables) for which all constraints are satisfied. As discussed before, constraints influence each other and, sometimes, it is not possible to fulfill all of them (consider Fig. 2.18). To tackle this, many implementations consider soft constraints (e.g., [328–332] ). The constraints are modified and a slack variable is added to allow the optimization to not fulfill the constraint. The slack variable is then heavily penalized on the cost function in order to only be used if breaking the constraint is really necessary. These feasibility issues have been studied and problems with different approaches highlighted and solved by Scokaert et al. in [334]. Furthermore, in [335], Kerrigan et al. present a method to compute a lower bound for the constraint violation penalty, allowing the guarantee of an exact penalty function when using soft constraints. This is important to ensure the constraints are only broken when really necessary, i.e., when there is no other feasible solution to the problem. For linear time-invariant systems, guaranteeing feasibility and stability for MPC has been extensively studied [319]. Probably the most common approach is to use a terminal cost and a terminal set constraint, which guarantees that the cost-to- go decreases over time, and using Lyapunov arguments, exponential stability is proven. Note that no closed form expression can be found for the constrained infinite-horizon cost-to-go function. However, it is often possible to provide a good estimate of it in a region of the space, which is called the terminal region (set), Zf . The idea is to find a feedback control law that stabilizes the unconstrained problem inside an invariant terminal set. Then, the closed-loop is stable if the cost function accounts for the infinite-horizon cost, Qf . So, the cost Qf is added to the finite-horizon cost and the last horizon point is constrained to be inside the terminal set Zf (where the unconstrained problem and infinite cost-to-go is valid), making the system stable. A common choice for the weight Qf is P , the solution 24 of the ARE (2.2), and for the terminal constraint set Zf is the maximal positive invariant set (see Definition 2.6) for the closed-loop system. When it comes to lateral control of HDVs with linear MPC, previous published work is much more scarce. Still, Scania CV AB, KTH - Royal Institute of Tech- nology, and LinköpingUniversity, have been key players in improving and applying MPC to path following problems for HDVs.

24In fact, the discrete time version of the ARE (2.2) is used. This is presented in Section 2.2.3. 74

Fig. 2.18: Experimental results obtained with Stanford’s X1 vehicle performing a double lane change on a low friction road (courtesy of Erlien et al. [332]). 2.1. AUTONOMOUS VEHICLE MODULES 75

Lima et al. in [336] present an LTV-MPC path following controller for HDVs called the smooth and accurate MPC (SA-MPC). The controller minimizes the errors to path, and the first and second derivatives of curvature to obtain a smooth driving behavior, while respecting the dynamics of spatial kinematic bicycle model and actuation constraints. This approach is experimentally evaluated with a Sca- nia truck, and is shown to behave two times better than a pure-pursuit controller. The authors extend this work in [93], where further results are presented for the controller. The controller is tested in both a highway like track at high speeds, and also in a precision track with a more complex curvature profile. First, simulations are run comparing the SA-MPC to a pure pursuit controller and a standard MPC that only considers the errors to path and requested curvature on the cost function. As expected, both MPCs perform better than the PPC, and the SA-MPC is smoother than the standard MPC, although it has slightly higher lateral error than the standard MPC. Experiments are run with the SA-MPC and PPC for both tracks. Not only the lateral error to path of the SA-MPC is much better, but it also increases much less than the PPC at higher speeds. In Chapter 5, a comparison to the SA-MPC is made, using the formulation presented in Lima’s PhD thesis [94, Ch.7]. For LTI systems controlled with MPC it is possible to guarantee stability with a terminal set and a terminal gain. Lima et al. in [337] extend this idea to LTV-MPC obtaining a terminal set that is invariant to multiple LTI systems and proposing a terminal cost. The approach is applied to lateral control of a vehicle using a spatial road-aligned bicycle model and LTV-MPC. Simulation of a straight path with an initial lateral error of 1 m, shows that the controller without terminal gain and set does not stabilize the system for some specific tuning weights. However, with terminal gain and set, the closed-loop system is always stable. This work is extended in [338]25, where Lima et al. propose an improved formulation for the terminal cost, consider input rate constraints, and verify their approach experimentally. The experiments are run with a Scania truck at constant velocity, where the vehicle is to follow a path that suddenly jumps 1 m to the side. Without taking into consideration input rate constraints the closed-loop system is not stable. However, when taking in consideration input rate constraints, terminal cost, and the terminal set, the system is stable. This work is extended in Chapter 4 of this thesis, where a more general formulation is presented for LTV systems which can be bounded by LPV systems. Furthermore, the rate of the LPV parameter variation is taken into consideration in order to stabilize all systems. For the autonomous vehicle example, this is done to handle changes in velocity, for which previous work did not guarantee stability for. Ljungqvist and Axehill in [339] have extend their work with LQR reversing truck-trailer systems by changing the controller to an MPC. The authors consider a multi-steered two trailer system with a car-like tractor, where the last trailer has steerable wheels. The MPC optimizes both the tractor and the last trailer

25This thesis author is a co-author of this work. 76 steering inputs, while minimizing the errors to path and control used. Moreover, the problem considers input and input rate constraints, and constraints on joint angles between trailers and tractor, to avoid undesired behaviors. Simulation is used to compare the proposed MPC to the LQ method proposed in [257]. Two reversing scenarios at −1 m s−1 are considered, a straight and a figure-eight path. The MPC advantages of having the joint-angle constraints are clear, since the controller is able to start from more difficult initial positions and stabilize the vehicle, without causing jackknife states. Furthermore, the advantage of having a steered trailer is also verified, showing that the vehicle converges faster and tracks the path better. Ljungqvist et al. in [340] extend their previous work by considering sensing capabilities of the system, and verifying the approach experimentally with a Scania truck-trailer system. The same figure-eight and straight paths scenarios as before are used. The experiments highlight even more the importance of the constraints on joint-angles, which for this controller not only avoid jackknifing but also allow for a good estimation of the trailer’s position. In [341], Ljungqvist et al. present more details on the previous controller, and compare it to a nonlinear mixed integer QP (MIQP) formulation that is less constrained on the joint-angles. Simulations show better tracking performance of the MIQP formulation, but the worst-case computation times are more than 7 times higher than the QP formulation, not allowing implementation in real-time.

This literature review of controllers and their application to autonomous vehicles is not complete. The reader is advised to dive into the references mentioned throughout the section, and, if interested, look into other applications where much more literature can be found. Consider the following subset of books and survey papers that were consulted to do this review: [108, 182, 204, 222, 232, 233, 246, 271, 272, 319, 342–347].

The review of the system architecture and related work for each module is also far from complete. The reader is advised to read further on these topics. Probably a good start is to look into the literature referenced and the references therein. Furthermore, the architecture presented is not unique, there are other valid system representations, where the tasks are split diﬀerently for multiple reasons. In fact, the extreme architecture assumes everything is done by one module, as in Schwarting et al. end-to-end planning section [63].

2.2 Control and Optimization Fundamentals

In this section the main concepts for discrete control and optimization that are used throughout this thesis are recalled. First, controllability, reachability, invariance, and stability concepts are presented. Afterwards, convexity and set operations are deﬁned. Finall the concepts of optimization, feasibility, quadratic program formulation, discrete LQR, and MPC feasibility and stability are introduced. The 2.2. CONTROL AND OPTIMIZATION FUNDAMENTALS 77 material presented below follows closely the book Predictive Control for Linear and Hybrid Systems by Borrelly, Bemporad, and Morari [271].

2.2.1 Controllability, Reachability, Invariance, and Stability Consider the following discrete-time system,

zt+1 = f(zt, ut), (2.5)

n m where, the function f(·) describes the system dynamics, zt ∈ R and ut ∈ R are the state and input vectors, respectively, which belong to state and input constraint sets, n m zt ∈ Z ⊆ R , ut ∈ U ⊆ R , ∀t > 0. (2.6)

When system (2.5) is subject to the feedback control law ut = F zt where F is feedback control gain, the system dynamics become,

zt+1 = f(zt, F zt).

Deﬁnition 2.1 (One-step controllable set) For system (2.5), the one-step controllable set to set S is deﬁned as,

n Pre(S) = {z ∈ R : ∃u ∈ U, f(z, u) ∈ S}

The one-step controllable set is the set of states that can be driven into the target set S in one time step.

Deﬁnition 2.2 (One-step reachable set) For system (2.5), the one-step reachable set from set S is deﬁned as,

n Suc(S) = {z ∈ R : ∃z0 ∈ S, ∃u0 ∈ U, z = f(z0, u0)}

The one-step reachable set is the set of states that can be driven from set S in one time step.

Deﬁnition 2.3 (N -step controllable set, KN (S)) For system (2.5) subject to constraints (2.6), the N-step controllable set to set S ⊆ Z is deﬁned recursively as,

N Kj(S) = Pre (Kj−1(S)) ∩ Z, K0 (S) = S, ∀j ∈ N1

The N -step controllable set is the set of states that can be driven into the target set S in N steps, while respecting the system constraints.

Deﬁnition 2.4 (N -step reachable set, RN (S)) For system (2.5) subject to constraints (2.6), the N-step reachable set from set S ⊆ Z is deﬁned recursively as,

N Rj(S) = Suc (Rj−1(S)) ∩ Z, R0 (S) = S, ∀j ∈ N1 78

The N -step reachable set is the set of states that can be driven from set S in N steps, while respecting the system constraints.

Deﬁnition 2.5 (Positive invariant set) For system (2.5) subject to constraints (2.6), a set O ⊆ Z is said to be positive invariant if,

z0 ∈ O ⇒ zt ∈ O, ∀t > 0.

Set O is invariant for system (2.5), if the system starts in O it stays in O for all futures states, while respecting the constraints.

Deﬁnition 2.6 (Maximal positive invariant set) The set O∞ ⊆ Z is the maximal positive invariant set for system (2.5) subject to constraints (2.6), if O∞ is invariant and contains all invariant sets in Z.

Theorem 2.1 (Geometric condition for invariance) A set O ⊆ Z is a positive invariant set for system (2.5) subject to constraints (2.6) if and only if

O ⊆ Pre (O) .

A proof can be found in [271, Theorem 10.1] by Borrelli et al. .

For ease of understanding while discussing system stability, consider the discrete time autonomous system, zt+1 = fa(zt), (2.7) n where, the function fa(·) describes the system dynamics, zt ∈ Z ⊆ R , ∀t > 0. Furthermore, consider the disturbed system,

zt+1 = fa(zt, wt), (2.8)

n where, wt ∈ W ⊆ R , ∀t > 0. A disturbance invariant set for a system, is a set that is invariant for that system even considering the system disturbances, i.e.,

Deﬁnition 2.7 (Disturbance invariant set) For system (2.8), a set O ⊆ Z is said to be disturbance invariant, or robustly positively invariant set, if,

zt ∈ O ⇒ zt+1 = fa(zt, wt) ∈ O, wt ∈ W, ∀t > 0.

Before defining stability, consider the following definition of distance of a point to a set, Definition 2.8 (Distance of point z to set S) The infimum Euclidean norm distance of point z ∈ Rn to set S ∈ Rn is defined as,

d(z, S) = inf{kz − sk : s ∈ S}. 2.2. CONTROL AND OPTIMIZATION FUNDAMENTALS 79

Deﬁnition 2.9 (Origin stable) For system (2.7) the origin is Lyapunov stable n for region ZA ⊆ R , if for each > 0, there is a δ > 0 such that any solution with initial state zt ∈ ZA satisﬁes,

kztk < δ ⇒ kzt+jk < , ∀j > 0.

Deﬁnition 2.10 (Origin asymptotically stable) For system (2.7) the origin is n asymptotically stable with a region of attraction ZA ⊆ R , if it is stable and

lim zt+j = 0, ∀zt ∈ ZA. j→∞

Deﬁnition 2.11 (Origin exponentially stable) For system (2.7) the origin is exponentially stable (Lyapunov stable and exponentially attractive) with a region of n attraction ZA ⊆ R , if there exists c > 0 and τ ∈ ]0, 1[ such that any solution with initial state zt ∈ ZA satisﬁes,

j kzt+jk 6 c τ kztk, ∀j > 0.

Showing Lyapunov stability of the origin is usually done by constructing a Lya- punov function that satisﬁes the conditions of the following theorem, Theorem 2.2 (Lyapunov stability) Consider the equilibrium point z = 0 (ori- n gin) of system (2.7). Consider the region of attraction ZA ⊂ R a closed and bounded set containing the origin. Assume there exists a function V : Rn → R continuous at the origin, ﬁnite for every z ∈ ZA, and such that

V (0) = 0 and V (z) > 0, ∀z ∈ ZA \{0}, (2.9) V (zt+1) − V (zt) 6 −τ(zt), ∀z ∈ ZA \{0}, (2.10) where τ : Rn → R is a continuous positive deﬁnite function. Then the origin is asymptotically stable in ZA. A proof can be found in [342, Appendix B] by Rawlings and Mayne.

Deﬁnition 2.12 A function V (z) satisfying conditions (2.9) and (2.10) is called a Lyapunov function.

When the system is disturbed the origin might not be stable anymore. In this case, a set around the origin can be stable. The following stability definition follows Mayne et al. [320]. Definition 2.13 (Set robustly exponentially stable) For system (2.8) set S n is robustly exponentially stable with a region of attraction ZA ⊆ R , if there exists c > 0 and τ ∈ ]0, 1[ such that any solution with initial state zt ∈ ZA and admissible disturbance sequence wt, ...., wt+j (wt+i ∈ W, ∀i ≥ 0) satisfies,

j d(zt+j, S) 6 c τ d(zt, S), ∀j > 0. 80

2.2.2 Convexity and Sets

The state and input sets (Z and U, respectively), are usually convex polytopes.

Deﬁnition 2.14 (Convex set) A set S ∈ Rs is convex if

βz1 + (1 − β)z2 ∈ S, ∀z1 ∈ S, ∀z2 ∈ S, β ∈ [0, 1].

Deﬁnition 2.15 (Convex function) A function f : S → R is convex if S is convex and

f(z1β + z2(1 − β)) > f(z1)β + f(z2)(1 − β)), ∀z1 ∈ S, ∀z2 ∈ S, β ∈ [0, 1].

Deﬁnition 2.16 (Convex hull of set S) The convex hull of a set S ⊆ Rn is the set of all convex combinations of points in S and it is denoted as,

( k ) k X Conv (S) = z1β1 + ... + zkβk : zi ∈ S, βi > 0, i ∈ N1 , βi = 1 . i=1

Deﬁnition 2.17 (Polyhedron) A polyhedron P in Rn denotes an intersection of a ﬁnite set of closed half-spaces in R,

n P = {z ∈ R : Az 6 b} , where Az 6 b is a system of inequalities.

Deﬁnition 2.18 (Polytope) A polytope is a bounded polyhedron.

Operations with sets are the basis for many control analysis.

Deﬁnition 2.19 (Minkowski sum) The Minkowski sum of two sets P, Q ⊆ Rn is deﬁned as, P ⊕ Q = {p + q : p ∈ P, q ∈ Q} .

Definition 2.20 (Pontryagin difference) The Pontryagin difference (also known as Minkowski difference) of two sets P, Q ⊆ Rn is defined as,

n P Q = {z ∈ R : z + q ∈ P, ∀q ∈ Q} . 2.2. CONTROL AND OPTIMIZATION FUNDAMENTALS 81

2.2.3 Optimization Fundamentals for Control In general, an optimization problem is formulated as,

min J(z) z (2.11) s.t. z ∈ Z ⊆ ZD, where the vector z is the decision variable, ZD is the optimization problem domain, and Z is the set of feasible or admissible decisions, more commonly known as constraints in MPC literature. The cost function J : ZD → R assigns a cost to each decision variable. The problem is said to be feasible if there is a pointz ¯ that belongs to set Z, i.e.,z ¯ ∈ Z. The problem is said to be infeasible if Z is empty. The objective is to ﬁnd the optimal solution to (2.11). Consider z? ∈ Z the argument that minimizes (2.11), then,

? ? J(z) > J(z ) = J , ∀z ∈ Z,

The value J ? is also called the optimal value of problem (2.11). The optimization problem (2.11) is said to be convex if the cost function J is convex on ZD and Z is a convex set. A fundamental property of convex optimization problems is that local optimizers are also global optimizers (Borrelli et al. [271, Theorem 1.1]). When the cost is a convex quadratic function and the constraints are aﬃne, the optimization problem is called a quadratic program (QP):

min 1 zTHz + hTz z 2 (2.12) s.t. Gz 6 g, Ez = e, where G ∈ Rq×n, g ∈ Rq, E ∈ Rp×n, e ∈ Rp, H = HT 0, H ∈ Rn×n, and h ∈ Rn. Numerical methods for solving QPs are a well studied problem and in this thesis they are assumed available and working as expected. For example, consider two open-source QP solvers, qpOASES [348] and OSQP [349].

Discrete-Time Linear Quadratic Regulator In 2.1.6.5, the LQR controller is introduced for a continuous-time system. Here the LQR for discrete-time systems is introduced in more detail, since it presents relevant results that are used in MPC problems. Consider the LTI system,

zt+1 = Azt + But, ∀t ≥ 0, (2.13) 82

n m where zt ∈ R and ut ∈ R are the state and input vectors. Let the LQR control problem be deﬁned as

∞ X T T min J∞(zt, ut) = zi|tQzi|t + ui|tRui|t ut i=0 ∞ (2.14) s.t. zi+1|t = Azi|t + Bui|t, ∀i ∈ N0 , z0|t = zt, where zi|t denotes the state vector at discrete-time t+i predicted at time t obtained i−1 by applying an input sequence {uj|t}j=0 to the model zi+1|t = Azi|t +Bui|t starting from the current state z0|t = zt. The state penalty matrix is semi-positive definite Q = QT 0 and the input penalty matrix is positive definite R = RT 0. In this ∞ case the number of points in the horizon is infinite, so ut = {uj|t}j=0. Using dynamic programming it is possible to solve (2.14) in a recursive manner. The solution to the problem is a feedback control law as function of the current state of the system (for details check Borrelli et al. [271, Chapter 8]). To obtain the feedback control law the discrete-time Riccati equation,

T T T −1 T Pt = A Pt+1A + Q − A Pt+1B(B Pt+1B + R) B Pt+1A, (2.15) is solved recursively backwards, i.e., for t → −∞ starting with P0 = Q. Given that it converges to a solution P∞ (check Goodwin and Sin [350, Appendix E]) satisfying the discrete-time algebraic Riccati equation (DARE),

T T T −1 T P∞ = A P∞A + Q − A P∞B(B P∞B + R) B P∞A, (2.16) the optimal feedback control law is written as,

? T −1 T ut = F∞zt = −(B P∞B + R) B P∞A zt, (2.17) and the optimal inﬁnite horizon cost as,

? T J∞ zt = zt P∞zt.

To guarantee convergence of (2.15) and to guarantee that it yields a stabilizing feedback law consider the following theorem. Theorem 2.3 (LQR stability) If (A, B) is a stabilizable pair and (A, Q1/2) is an observable pair. Then, the discrete-time Riccati equation (2.15) with P0 0 converges to the unique positive deﬁnite solution P∞ of the DARE (2.16) and all the eigenvalues of (A + BF∞) lie strictly inside the unit circle. A proof can be found in [351, Theorem 2.4-2].

? The theorem’s first condition ((A, B) is stabilizable) is necessary for J∞ (zt) (and P∞) to be finite. To understand the second condition consider the objective function T T 1/2 1/2 1/2 written as zt Qzt = (zt Q )(Q zt). Thus, not the state but the output Q zt is 2.2. CONTROL AND OPTIMIZATION FUNDAMENTALS 83 penalized in the objective. Therefore the second condition ((A, Q1/2) observable) 1/2 requires that this output captures all system modes, such that convergence of Q zt implies convergence of the state to zero. Thus the optimal control law (2.17) guarantees asymptotic stability of system (2.13). However, when the optimization problem (2.14) includes state and control input constraints, the optimal control law is difficult to find. Hence, solving the optimal problem online becomes interesting - this approach is called model predictive control (MPC).

Model Predictive Control - Feasibility and Stability

In 2.1.6.7, linear MPC is introduced. In this subsection feasibility and stability of the controller are discussed. For ease of readability and understanding, once again, consider the LTI system,

zt+1 = Azt + But, ∀t > 0, (2.18)

n m where zt ∈ R and ut ∈ R are the state and input vectors, which belong to the state and input constraint sets,

n m zt ∈ Z ⊆ R , ut ∈ U ⊆ R , ∀t > 0. (2.19)

The linear MPC formulation to control the system (2.18) to the origin is written as,

s.t. zi+1|t = Azi|t + Bui|t, ui|t ∈ U, (2.20) zi|t ∈ Z, zN|t ∈ Zf , z0|t = zt, N−1 ∀i ∈ N0 , where Z and U are state and input constraints, respectively, and are defined as convex polytopes containing the origin. The cost matrices Q and R, and the system matrices A and B, follow the same conditions as in the LQR optimal problem (2.14), so Theorem 2.3 applies for the unconstrained problem. The terminal state penalty T matrix is semi-positive definite Qf = Qf 0, and the terminal set Zf is the region of the state space where the terminal cost is valid, which is also defined as a convex polytope. As explained before, receding horizon control (RHC) is used, so only the first optimal control input of the N predicted points is applied to the system at each time step. 84

LQR Let Qf = P∞ be the solution to the DARE (2.16), and let Zf = O∞ the maximal positive invariant set (2.6) for the closed-loop system

zt+1 = (A + BF∞) zt, ∀t > 0, where F∞ is the feedback gain deﬁned in (2.17). Finally, let the region of attraction LQR of the controller be KN O∞ , the N-step controllable set (2.3) of the maximal positive invariant set. Then, the MPC controller is asymptotically stable. Two conditions need to be shown to prove stability, ﬁrst the problem needs to be recursively feasible, and second the cost function needs to be a Lyapunov function.

Theorem 2.4 (MPC feasibility) Problem (2.20) is feasible ∀t > 0 if Qf = P∞, LQR LQR Zf = O∞ , and if z0 ∈ KN O∞ . A proof can be found in Appendix A.

Theorem 2.5 (MPC stability) Consider system (2.18), constrained by (2.19), under closed-loop RHC with the linear MPC (2.20) as deﬁned above with all the as- LQR sumptions for A, B, Q, R, Qf = P∞, and Zf = O∞ . Then, the origin is asymp- LQR totically stable for the closed-loop system with region of attraction KN O∞ . A proof can be found in Appendix A.

To solve problem (2.20) online eﬃciently, it is of interest to formulate it as a QP to match the formulation (2.12). The derivation of this formulation is presented in Appendix B. This thesis addresses problems with LTV systems of the form,

zt+1 = Atzt + Btut, ∀t ≥ 0, where both matrices At and Bt are time-varying. To guarantee stability for these systems extra considerations need to be made, which is addressed in Chapter 4. Furthermore, the QP formulation setup follows the same procedure presented in Appendix B, but the correct system matrices that change for each step are used, making the setup slightly more complex.

2.3 Summary

An autonomous vehicle is a system composed of many modules. This chapter presented a six module architecture: route planning, perception, localization, situation awareness, motion planning, and motion control. The route planner produces and keeps track of the plan achieving long-term goals. The perception and localization modules receive information from the vehicle sensors, to estimate the environment as well as the ego vehicle position in it. The situation awareness module receives all previous information, predicting and understanding the environment scene, and, together with motion planning, chooses an action to take. Motion planning creates a detailed local plan to reach a short term goal. Finally, the motion control module 2.3. SUMMARY 85 is responsible for making the vehicle track the desired plan, by using the vehicle actuators. This chapter started by presenting a literature review for the autonomous system modules, with special emphasis on the main topic of this thesis, motion control. Afterwards, control and optimization fundamentals were deﬁned. First, controllability, reachability, invariance, and stability concepts were introduced. Then, convexity and set operations were deﬁned. Finally, optimization for control was presented, where concepts of optimization, feasibility, discrete-time LQR, MPC stability, and QP formulation were introduced.

Chapter 3

Control Module Architecture

ontrolling a vehicle is not a simple task. Humans excel at it and can adapt C their driving behavior depending on conditions and disturbances, avoiding holes on the road, accounting for wind, moderating velocity on icy conditions, etc.. Moreover, humans are able to drive the vehicle at the limits of handling (e.g., on racing and drifting applications), where controlling it is a diﬃcult task. The control module objective is to replace the physical actuation of the driver. Consider Fig. 3.1, where a simpliﬁed autonomous vehicle architecture is presented. Comparing to the architecture presented in Fig. 2.1: the mission control block represents both the route planner and the user interface; the localization block represents both perception and localization; and the planner block represents both situational awareness and mission planning. The controller receives feedback from the vehicle through the perception and localization modules, and receives a path or trajectory from the planner module. Two control input requests are used: steering-

Mission Control

Planner zp Localization Controller z

vreq SWAreq

Vehicle

zp - Planned Path z - Vehicle States vreq - Velocity Request SWAreq - Steering-Wheel Angle Request

Fig. 3.1: Simple closed-loop system architecture.

87 88

zp Delay Prediction

zˆ

Path Sampling

zp zp - Planned Path z - Vehicle States Constraints Setup zˆ - Predicted Vehicle States

zp - Planned Path Sampled zp zp - Constraints for zp

Longitudinal Lateral κr - Curvature Request Control Control vreq - Velocity Request κreq SWA - Steering-Wheel Angle

SWAreq - SWA Request Output Conversion

vreq SWAreq

Vehicle

Fig. 3.2: Control module architecture.

wheel angle (SWAreq), and velocity (vreq). Finally, the vehicle, is assumed to include low-level controllers implemented to track received requests, which for the sake of this work cannot be modiﬁed and are considered as a black-box. The objective of the control module is to follow the path and velocity proﬁle references received by the planner (zp), while maintaining certain error bounds, respecting the actuation constraints, and providing a safe and comfortable ride. Requested values are not instantaneously actuated, i.e., there is a delay between the request being made, the actuators achieving the requested value, and the vehicle behaving as intended. Moreover, requests are not followed exactly, because the vehicle is subject to both external and internal disturbances. External disturbances are, e.g., potholes, rain, snow, or gravel. Internal disturbances are, e.g., model uncertainty or unmodeled actuator dynamics. To achieve the desired control behavior consider the controller architecture presented in Fig. 3.2. Note that all blocks have access to previous block outputs, but, for simplicity, only the outputs from the preceding block are drawn. The following Sections in this chapter introduce each block. This introduction to the controller architecture is mostly focused on lateral control, since it is the main focus of this 3.1. DELAY PREDICTION 89

zp Delay Predicted Path z zˆ

Fig. 3.3: Illustration of delay prediction. The arrows indicate the pose yaw. thesis. The lateral and longitudinal control blocks are not extensively introduced and the reader is referred to Chapters 2, 4, and 5.

Remark 3.1 Most Figures presented in this Chapter have highly exaggerated curvature and radius changes for easier understanding. In practice if the path points are 1 or 2 meters away from each other, the changes in curvature are small for the path to be feasible. This makes the assumptions valid and the approximations to be good.

3.1 Delay Prediction

Real systems have delays in computation, actuation, communication, and feedback. Sometimes, these delays are neglectable and the control loop is able to stabilize and control the system without explicitly accounting for them. Other times, as is the case here, these delays need to be addressed and accounted for. A possible approach to handle delays is to actuate on a future position of the system (Findeisen et al. [352,353] and Lima [94]). Where, the future system states are predicted using the system model and previous control requests. Then, instead of using the current system states (z), the predicted states (ˆz) are used to control the system, a simple illustration of the idea is presented in Fig. 3.3. In other words, the idea is to compute the necessary control request for states the system will have in time Td. This way, when the system reaches those states the actuation is the desired one. For Chapter 5, the delay is assumed to be known and the model being used for prediction is the same as the one used in the controller. The prediction is done by forward simulating the system model with the control inputs previously requested and the vehicle’s velocity. 90

3.2 Path Sampling

The path sampling block is responsible for computing the correct path points for the controllers to track. The path sampling starts on the predicted vehicle states, zˆ. In practice, the objective is to control the vehicle starting from where it will be at when the control input is being applied. The first step of path sampling, is to do the perpendicular projection of the states to the path. The errors for the controller to correct are computed from the projected and predicted states. After, starting on the projected point, the rest of the MPC horizon points to be tracked are computed. The planner module sends a path for the controller to follow and, this path, can be described in multiple ways. A common approach is to send a set of points containing information about the desired vehicle poses, curvatures, velocities, accelerations, etc. . This general interface allows more flexibility for testing different planners and algorithms, since it is only necessary to fulfill a generic communication interface. On the other hand, this general interface has the disadvantage of not providing a description between the received points, meaning that if the predicted vehicle states are in between two given points there is not an exact projection of the vehicle states to the path. To solve this problem two simple assumptions are made. The first is that the path has continuous poses. This means that the position (x and y) and the yaw (ψ) are continuous between points. The second assumption is that the center of rotation is constant between received path points. These assumptions lead to a circular arc approximation between path points.

3.2.1 Circular Arc Projection For better understanding the idea behind the circular arc approximation, consider first a path defined by 3 points and two circular arcs, presented in Fig. 3.4. The two circles are defined by C1,R1 and C2,R2, the circles centers and radii, respectively. The orthogonal projection to path is easily defined by radius R1 and the line connecting C1 toz ˆ. Which leads to the green pose on the path, pproj. The state errors of the vehicle are then defined by the difference between pproj andz ˆ. The example presented in Fig. 3.4 is a specific case of the path sampling problem where the path is actually defined by circular arcs. However, there are many methods, algorithms, and vehicle models that can be considered by the motion planner, as presented in Chapter 2. So, the case presented in Fig. 3.4 seldomly occurs. Normally, the path is not defined by circular arcs, and the poses received are only known to be continuous. Consider now a more general example where the curvature between path points varies, presented in Fig. 3.5. Between two poses the path is assumed to have a constant turning center, which is a reasonable assumption since the length of a normal HDV (> 5 m) is significantly bigger than the distance between received planned poses (< 2 m). This leads to two centers of rotation in Fig. 3.5, C1,2 and 3.2. PATH SAMPLING 91

zˆ p1 eψ C ey 2 pproj R2

R1 R1 p2

R2 R1

zp points zp zˆ pproj p3

Fig. 3.4: Illustration of projection to path composed of two circular arcs. The arrows indicate the pose yaw.

zˆ p1 eψ C2,3 ey

pproj R3

R1 Rproj p2,prev p2

R4 R2 C1,2

zp points zp approximation zˆ pproj p3 p2,prev

Fig. 3.5: Illustration of the projection to path with the circular arc approximation. The pink p2,prev point is the previous point 2 of Fig. 3.4. The dashed-orange lines show the perfect circular arc built using the centers and the radius for each path pose. 92

zˆ

p1 eψ ey

pproj

zp points zp approximation R1 Rproj zˆ pproj θ 2 p2 Ratio angles

θ1 R2

C1,2

Fig. 3.6: Zoomed in illustration of projection to path with the circular arc approximation. In pink the angles that deﬁned the ratio are presented.

C2,3. The center of rotation C1,2 for poses p1 and p2 is deﬁned by the intersection of the orthogonal lines to the poses. These lines, contain R1 and R2, which are the instantaneous radius of the path at the pose. Note that a pose, is deﬁned with x, y, ψ, and a line orthogonal to a pose is a line that is orthogonal to the yaw of the pose and passes through it. Furthermore, p2,prev presented in pink is the previous p2 pose from Fig. 3.4, and in dashed-orange the circular arcs with the radius for each pose are presented.

As it can be seen in Fig. 3.5, the path approximation, zp, in black is not composed of circular arcs, instead it is an arc with varying radius. The idea for the path sampling is to assume that the radius of the arc in between two poses varies linearly with the angle. This way, both the interpolation of the position and yaw are continuous and will match the path poses.

Consider Fig. 3.6, which presents part of Fig. 3.5 zoomed in. With the assumption that the center of rotation is constant between points, the orthogonal projection to path is contained on the line that passes both on the center of rotation and on the pose ofz ˆ. Then, using the assumption that the radius varies linearly with the angle on the arc, it is possible to deﬁne the ratio of the projected 3.2. PATH SAMPLING 93

point. Consider the line angles deﬁned from the center C1,2, to p1, p2, andz ˆ,

y y x x α1 = arctan2 p1 − C1,2, p1 − C1,2 , y y x x α2 = arctan2 p2 − C1,2, p2 − C1,2 , y y x x αproj = arctan2 zˆ − C1,2, zˆ − C1,2 , where, arctan2(y, x) ∈ [−π; π] is the four-quadrant inverse tangent, and where the upper-script represent the coordinate, i.e., px is the x value of p. The angles, θ1 and θ2, that deﬁne the linear ratio for the projection can be seen in Fig. 3.6 and computed as,

θ1 = α2 − α1, |θ1| ≤ π

θ2 = αproj − α1, |θ2| ≤ π and the ratio, θ2 λ = . θ1 Having the ratio that deﬁnes where the projected point is in the arc, the radius of the projected point is deﬁned as,

Rproj = R1 + λ(R2 − R1) and the projected point is, x x pproj = C1,2 + Rproj cos(αproj), y y pproj = C1,2 + Rproj sin(αproj). The path point yaw (ψ), velocity (v), acceleration (a), distance traveled along the path (s), etc., are all defined with the ratio and the two received path points. For s example, pproj is defined as, s s s s pproj = p1 + λ(p2 − p1). It is not always possible to use the circular arcs approximation between path points. If the yaw of two poses is the same, instead of the circular arc approximation, a straight line between them is used. Furthermore, if the path only has one point a line defined by the position and its yaw is used. Using these lines, the orthogonal that passes throughz ˆ is computed, from which the intersection, i.e., the projected pose, is obtained. Having defined the projected pose, the ratio on the line is computed and all other states are defined.

3.2.2 MPC Horizon The MPC controllers, need more than just one point as reference to run. Having computed the ﬁrst reference point for the MPC horizon, pproj, it is now possible to compute the other points. 94

zp points pproj = p1,hor zp approximation pproj ∆s,t phor points

p2,hor

p3,hor ∆s,t

Fig. 3.7: Illustration of the path sampling for the MPC horizon. The distance between points sampled is ∆s,t.

The MPC horizon points are considered isochronal and the sampling distance between points is computed using the current velocity of the vehicle, i.e., ∆s,t = Tsvt. Having the desired ∆s,t and the s of all received path points. Consider Fig. 3.7, where the sampling of the ﬁrst 3 horizon points is illustrated. Consider the distance traveled along the path for the second horizon point, s p2,hor,

s s p2,hor = pproj + ∆s,t.

Then, the ratio can be computed as,

s s p2,hor − p2 λ2,hor = s s . p3 − p2

Note that the ratio, λ2,hor, is always computed with the correctly picked path points, s s s i.e., p2 ≤ p2,hor ≤ p3. Having the ratio defined, all other states are computed as before. To compute the (x, y) position, first the center of rotation and the radius for the path points are defined. Then, the θ1 angle for the path points is computed and the ratio is used to discover θ2. Having θ2 and the radius for the horizon point, the (x, y) position is computed using the center of rotation. Finally, the procedure for the sampling of horizon points is repeated until all points are computed. 3.3. CONSTRAINTS SETUP 95

3.3 Constraints Setup

The great advantage of MPC is its ability to explicitly handle constraints both on the system states and control inputs. Chapter 4, uses both state and input constraints, which are directly introduced in the chapter. In this Section, the physical actuation, safety, and comfort input constraints used in Chapter 5 are discussed. The control input constraints are set up not only to respect the limits of the steering-wheel and its actuator, but also to increase passengers and goods safety and comfort. These constraints are shown in Fig. 3.8. The control input constraints are set on the steering-wheel angle, which is the lateral controller output of the control module. This is done in order to satisfy the vehicle actuation limits easily. The steering is further constrained for comfort and safety, by limiting the lateral acceleration of the vehicle. This idea has been presented in literature (e.g., Kim et al. [311] and Sahlholm et al. [354]), where by considering the wheel angle (δ) given by the kinematic lateral acceleration (ay) of the vehicle, l ay,max δ = arctan , v2 it is possible to limit the maximum allowed vehicle wheel angle and/or velocity, taking into consideration the length (l) between the front and rear axles of the truck. The idea is presented for wheel angle for simplicity, but this is done for the SWA directly as explained before. This limitation and the physical limits lead to a constraint in the form presented in Fig. 3.8a. The steering-wheel angle rate and acceleration constraints follow the same setup idea as the SWA limitations. Which leads to constraints in the form presented in Fig. 3.8b and 3.8c. From a practical point of view these limitations are quite simple. With increasing speed the actuation is more limited, i.e., when the vehicle is moving faster it is not allowed to turn as much. The same idea is used by human drivers. When driving on a highway less actuation is used to change lane than when driving on a city/rural road at slower speeds.

To obtain the current curvature constraint sets Ut, U˙t, and U¨t for the MPC, the steering constraints are evaluated at every iteration with the vehicle speed. To obtain the constraints in curvature, the inverse conversion presented in Section 3.6 is made. To compute the curvature rate and acceleration constraints, the inverse conversion is made considering the steering rate and acceleration limits and steering-wheel angles requested. As a design choice, the curvature rate is further −1 −1 constrained to U˙t ∩[−0.05; 0.05]m s , which might or might not be the actuation limit depending on the velocity being considered. 96 SWA

0 10 20 30 40 50 60 70 80 90 100 1 v [km h− ]

(a) Steering-wheel angle (SWA) constraints. SWAR

0 10 20 30 40 50 60 70 80 90 100 1 v [km h− ]

(b) Steering-wheel angle rate (SWAR) constraints. SWAA

0 10 20 30 40 50 60 70 80 90 100 1 v [km h− ]

Fig. 3.8: Steering-wheel angle constraints. 3.4. LONGITUDINAL CONTROL 97

3.4 Longitudinal Control

The longitudinal control objective is to track the trajectory’s longitudinal position in time. Solutions to this problem have been discussed in Chapter 2. Although, longitudinal control is not studied in this thesis, it is necessary to have at least a simple version of it in order to make the vehicle move. For the results presented in Chapter 5, the velocity and transmission control of the vehicle is used, i.e. the same type of controller that is used for the vehicle’s cruise controller functionality. The vehicle controllers receive a request of the desired velocity, which is constantly being updated by the control module. As expected, the velocity proﬁle is not tracked perfectly, but it is deemed good enough for the analysis of the lateral controller performance. Better and more robust solutions are discussed in Chapter 2, which should be considered for level 5 autonomous vehicles.

3.5 Lateral Control

The lateral control objective is to track a path/trajectory by minimizing the lateral and yaw errors to it. Solutions to this problem have also been discussed in Chapter 2. The lateral controller for autonomous vehicles is the main focus of this thesis and is discussed in more detail in the upcoming Chapters. Chapter 4 of this thesis discusses a robust control approach to this problem. Chapter 5 presents a nominal control approach that is applied and experimentally tested with an HDV. In order for the controller to be easily applied to multiple vehicles, the control input is vehicle curvature. This curvature is then converted into a vehicle speciﬁc steering-wheel angle (SWA) that is directly actuated by the vehicle controllers. To compute the control input, the delay predicted future states (ˆz) and the sampled path (zp) are used, while the constraints (zp) are respected.

3.6 Output Conversion

Velocity and steering-wheel angle requests are made periodically to the vehicle. As explained before, this thesis is not focused on longitudinal control. So, the longitudinal controller outputs directly the desired velocity which is fed-through to the cruise controller of the vehicle. The lateral controller outputs curvature (κ). However, the steering controllers of the vehicle receive a steering-wheel angle. Furthermore, to achieve the requested curvature on different vehicles, different wheel angles need to be used. So, first, an auxiliary vehicle specific wheel angle, approximating all the steered wheel angles, is computed, 2 δ = arctan κ l + v KUS where l is the length between the front and rear axles of the truck and KUS is the under-steering coefficient. This approximation follows the discussion in Nyberg and 98

T¨ornroth[355] and considers both a kinematic and a dynamic bicycle model. It is a compromise between the necessary wheel angle for high-speed low-curvature turns, where the dynamic model gives a better result, and low-speed high-curvature turns, where the kinematic model is better. For details, the reader is referred to [355]. The actuation on the vehicle happens with the steering-wheel angle (SWA), and not directly with the wheel angles. To compute the SWA request (SWAreq) a vehicle speciﬁc nonlinear mapping from the desired wheel angle (δ) is used. The steering dynamics mapping takes into consideration the steering column and other mechanical connections, that connect the steering-wheel to the wheels. The control chain is complex and not perfect, meaning that the requested curvature (κreq) will never be perfectly followed by the vehicle. These internal disturbances of the vehicle also need to be considered when developing the lateral controller. Since, not only the vehicle can be disturbed by a hole on the road, but also the tracking of the high-level control request, κreq, will never be perfect, even for perfect external conditions.

3.7 Conclusion

This Chapter presented the control module architecture considered in this thesis. This architecture is composed of six blocks: delay prediction, path sampling, constraints setup, longitudinal control, lateral control, and output conversion. The delay prediction accounts for system delays by predicting the future states of the vehicle. The path sampling, samples the path guaranteeing continuity between received planner points, which is important for having a well posed and feasible problem. The constraints setup deﬁnes the current actuation limits for the MPC to maintain. The longitudinal and lateral controllers compute the desired high-level references to be actuated on the vehicle. The output conversion converts these references to vehicle speciﬁc reference values to be tracked by the vehicle controllers. Chapter 4

Linear Time-Varying Robust Model Predictive Control

odel predictive control (MPC) has been widely used in industry due to its M ability to handle both input and state constraints explicitly. MPC for linear time-varying (LTV) systems has been widely studied as noted in Chapter 2. However, the design of linear robust MPC to LTV systems has not seen so many advances. As noted before, [320–322] Mayne et al. have proposed an interesting linear robust MPC for disturbed linear time-invariant (LTI) systems. The approach distinguishes between the nominal and disturbed system. It optimizes the nominal system trajectory with a QP MPC, which is easily solved in real-time, and uses a feedback gain to handle disturbances and make the disturbed system follow the nominal trajectory. This chapter extends the linear robust MPC formulation for LTI systems to LTV systems. It extends the work done by Mayne et al. in [320], by bounding the LTV system on a LPV framework and considering rate of parameter variation for LPV systems [297]. Furthermore, it uses a disturbance invariant set proposed in [323], and considers the recent research within LTV-MPC stability in [337,338]. Robust exponential stability is obtained, by guaranteeing that all model mismatch and disturbances to the nonlinear system are captured in the disturbance set of the LTV system. Then, it applies the controller to a simple disturbed autonomous vehicle example. Finally, the limitations of the proposed approach are discussed with the help of a second, more constrained, autonomous vehicle example. Robust control of autonomous vehicles is of particular interest, because the motion controller needs to handle heavily disturbed environments. For example, consider a HDV driving in a mining environment, which consists of holes, rocks, gravel, sand, etc., the controller needs to account for disturbances in order to guarantee safety of the autonomous vehicle operation. The work presented in this chapter is mostly based on the paper:

99 100

• G. Collares Pereira, P. F. Lima, B. Wahlberg, H. Pettersson, and J. M˚artensson. Linear Time-Varying Robust Model Predictive Control for Discrete-Time Nonlinear Systems. In 2018 IEEE Conference on Decision and Control (CDC), pages 2659-2666, Dec 2018. The rest of this chapter is organized as follows. Section 4.1 formulates the problem. Section 4.2 introduces the proposed controller and presents the proof of robust exponential stability of a disturbance invariant set for the nonlinear disturbed system. In Section 4.3, the proposed controller is used in an autonomous driving application and simulation results are presented. In Section 4.4, the limitations of the approach are discussed with a more constrained autonomous vehicle example. Finally, in Section 4.5, some concluding remarks are done.

4.1 Problem Statement

The aim is to control a discrete-time nonlinear system subject to input disturbances,

zt+1 = f(zt, ut, wt), (4.1) n where t is a time instant and t+1 the next time instant, zt ∈ R is the state vector, m n+m ut ∈ R is the input vector, and wt ∈ R is the disturbance vector, which are constrained and bounded by

zt ∈ Z, ut ∈ U, wt ∈ W, ∀t ∈ N0,+, where the sets Z ⊆ Rn, U ⊆ Rm and W ⊆ Rn+m. The disturbance w lies within the bounded set W but it is, otherwise, unknown. The controller is required to keep the nonlinear system in the allowed sets Z and U regardless of the disturbances. Consider the linearization of (4.1) around zr,t and ur,t, state and control input references, respectively,

z˘t+1 = Ar,tz˘t + Br,tu˘t +w ˘t, (4.2) wherez ˘t = zt − zr,t,u ˘t = ut − ur,t,

∂f(z, u) ∂f(z, u) Ar,t = ,Br,t = . ∂z z=zr,t ∂u z=zr,t u=ur,t u=ur,t z˘t ∈ Z˘t = Z zr,t for zr,t ∈ Zr ⊆ Z andu ˘t ∈ U˘t = U ur,t for ur,t ∈ Ur ⊆ U. Let Γ be an uncertainty set that contains all possible pairs (Ar,t,Br,t) = γt. Then, it is possible to bindw ˘ ∈ W˘ , where W˘ is the disturbance set for the LTV system deﬁned as, W˘ = Conv f(zt, ut, wt) − zr,t+1 − Ar,tz˘t − Br,tu˘t : (4.3) ∀z˘t ∈ Z˘t, ∀u˘t ∈ U˘t, ∀γt ∈ Γ, ∀wt ∈ W, ∀zr,t+1 ∈ Zr, ∀t ∈ N0,+ . This set contains not only the possible disturbances for the nonlinear system (4.1), but also the linearization error. 4.2. CONTROLLER FORMULATION 101

4.2 Controller Formulation

In this section, the robust MPC controller formulation is presented and robust exponential stability of the disturbance invariant set is proven. Let (4.2) be written as an LPV system

z˘t+1 = A (λt)z ˘t + B (λt)u ˘t +w ˘t, (4.4)

(1) (nλ) T where λt = [λt , ..., λt ] is a time-varying scheduling parameter vector. A common assumption in the LPV framework is that these systems can be described to belong to a convex uncertainty polytope or in the parameter space (see, e.g., [294, 297]), n (θ) Xnλ (θ) o Λ = λt λt 0, λt = 1 , > θ=1 where the variation rate is

nλ λ˙ t = λt+1 − λt ∈ Λ˙ ⊂ R . The parameter-varying matrices are known to lie in the polytopes

Xnλ (θ) Xnλ (θ) A (λt) = Aθλt ,B (λt) = Bθλt , θ=1 θ=1 where Aθ and Bθ denote the θ-th vertice of the polytope. Then, for system (4.2) the original set Γ ⊆ Conv {(A(λt),B(λt))} , ∀λt ∈ Λ. Meaning that Λ needs to represent, at least, an outer approximation of Γ, which guarantees that a disturbance invariant set for all systems deﬁned by Λ is also a disturbance invariant for Γ. Furthermore, in [297], the knowledge of the parameter variation rate is used to more eﬃciently compute the control law. The parameter space Λ is partitioned into ns smaller parameter spaces {Λ1, ..., Λj, ..., Λns }. These have to be full-dimensional, collectively exhaustive, and mutually exclusive, i.e.,

[ns \ Λj = Λ, Λj Λl = ∅, ∀ j =6 l. j=1

Assume that the dynamics of system (4.1) and its references constrain the change rate of (A (λt) ,B (λt)) in such way that, if λt ∈ Λj, then λt+1 ∈ Λl, for l ∈ {j − 1, j, j + 1}. In other words, if the scheduling parameter of the system pair (A, B) is contained in the parameter space j at time t, it can at most be contained in parameter space j + 1 or j − 1 at t + 1. Consider the nominal system of (4.4) to be deﬁned as,

z¯t+1 = A (λt)z ¯t + B (λt)u ¯t, (4.5) wherez ¯t ∈ Z¯t ⊂ Z˘t andu ¯t ∈ U¯t ⊂ U˘t. Moreover, consider the diﬀerence between the system subject to disturbances (4.4) and its nominal system (4.5),

z˘t+1 − z¯t+1 = A (λt) (˘zt − z¯t) + B (λt) (˘ut − u¯t) +w ˘t, 102 the idea is to bindz ˘ − z¯ by a robust positively invariant set S. m×n Let F ∈ R be a feedback gain, such that AF (λt) = A (λt) + B (λt) F is stable, i.e., ρ (AF (λt)) < 1, ∀λt ∈ Λ. Let S be a disturbance invariant set for the controlled uncertain systemz ˘t+1 = AF (λt)z ˘t +w ˘t, then AF (λt) S ⊕ W˘ ⊆ S, ∀λt ∈ Λ, ∀ (˘z − z¯) ∈ S and ∀w˘ ∈ W˘ . According to [323], the set S can be written as, n o S = W˘ ⊕ Conv AF,θW˘ , ∀θ ∈ {1, ..., nλ} ⊕ n o (4.6) Conv AF,θAF,αW˘ , ∀θ, α ∈ {1, ..., nλ} ⊕ ... , where AF,θ = Aθ + BθF , where Aθ and Bθ denote the θ-th vertice of the polytope Λ and AF,α is equivalent to AF,θ. A polytopic, disturbance invariant, outer approximation of S can be computed [356]. This set will serve as the origin for the discrete-time nonlinear system subject to disturbances (4.1). A result obtained in [357] is extended by [323], namely: Proposition 4.1 Suppose S is disturbance invariant for

z˘t+1 = AF (λt)z ˘t +w ˘t, ∀λt ∈ Λ.

If z˘t ∈ z¯t ⊕ S and µ˘t = F (˘zt − z¯t) +u ¯t, then

z˘t+1 ∈ z¯t+1 ⊕ S, ∀w˘ ∈ W˘ , where z˘t+1 = A (λt)z ˘t + B (λt)µ ˘t +w ˘t and z¯t+1 = A (λt)z ¯t + B (λt)u ¯t, ∀λt ∈ Λ.

Having defined S, consider the definition of tighter control and state constraints for the nominal system, which is done to guarantee satisfaction of the original constraints by the final controller,

U¯t = U˘t F S, (4.7)

Z¯t = Z˘t S. (4.8)

Recent work for LTV-MPC, [337], ensures stability using a terminal state set and cost. In that work, the following recursion is presented, \ Ωk+1 = Preλ(Ωk) ∩ Ωk, Ω0 = Z¯t, (4.9) λ∈Λ where Preλ(Ω) represents the one-step controllable set to Ω using the nominal model with the pair (A(λ),B(λ)). 4.2. CONTROLLER FORMULATION 103

Remark 4.1 If multiple state reference points are being considered, then Z¯t might not be a good choice for the initial condition set. Any set can be chosen iﬀ Ω0 ⊆ Z¯t, ∀t ∈ N0,+. It is assumed, without loss of generality, that only one state reference point is being considered.

Recursion (4.9) is computed using the vertices of all parameter spaces Λj, computing the one-step controllable sets with multiple F ∗(λ) ∈ Rm×n, such that ∗ AF ∗ (λ) = A (λ) + B (λ) F (λ) is stable, i.e., ρ (AF ∗ (λ)) < 1. This recursion eventually converges to a polytopic maximal positively invariant set [358], which can be used as a terminal state set, i.e,

Z¯f = lim Ωk. (4.10) k→∞

Note that this deﬁnition of Z¯f is conservative, because it does not take into consideration the variation rate Λ,˙ i.e., it computes only one invariant set ∀λ ∈ Λ. Remark 4.2 The diﬀerence between F ∗(λ) and F , is that F is one gain that stabilizes all systems and F ∗(λ) can change and be an optimal gain for each system. Let the vector of time-varying scheduling parameters be

λt = {λ0|t, λ1|t, ..., λN−1|t}, the vector of control sequences for the nominal system be

u¯t = {u¯0|t, u¯1|t, ..., u¯N−1|t}, and let UN (¯zt, λt) denote the set of control sequences u¯t satisfying the following constraints, z¯i+1|t = A λi|t z¯i|t + B λi|t u¯i|t,

z¯0|t =z ¯t, z¯i|t ∈ Z¯i|t, u¯i|t ∈ U¯i|t, z¯N|t ∈ Z¯f ,

λi|t ∈ Λ, λN−1|t ∈ Λj, λq|t − λh|t ∈ Λ˙ ,

N−1 ns N−1 N−2 ∀i ∈ N0 , j ∈ N1 , ∀q ∈ N1 , ∀h ∈ N0 .

Consider the MPC problem PN (˘z, λt) for system (4.4), which is a modiﬁcation of the one proposed in [320], VN (˘zt, λt) = min V¯N (¯zt, u¯t)| z˘t ∈ z¯t ⊕ S, u¯t ∈ UN (¯zt, λt) , z¯t,u¯

∗ ∗ (¯zt , u¯t ) = arg min V¯N (¯zt, u¯t)| z˘t ∈ z¯t ⊕ S, u¯t ∈ UN (¯zt, λt) , z¯t,u¯ where N is the number of points in the horizon and XN−1 V¯N (¯zt, u¯t) = Vf z¯N|t, P¯j + ` z¯i|t, u¯i|t , i=0 104 where

∗ ∗ µ˘t = F (˘zt − z¯t ) +u ¯0|t. (4.13)

The domain of the value function, VN (˘zt, λt), is Z˘N deﬁned by,

Z˘N = {z˘t, λt | ∃z¯t :z ˘t ∈ z¯t ⊕ S, UN (¯zt, λt) =6 ∅} . (4.14)

The differences between this formulation and the one in [320], are that the nominal system is time-varying but contained in Λ and the terminal weight depends on which parameter space the system is at the predicted time t+N −1. As in [320], the nominal state of the system is an optimization variable and the state, control input, and terminal constraint sets are tighter than on a standard MPC. In [319] Mayne et al., defined four axioms that if satisfied, together with some other minor assumptions, ensure closed-loop asymptotic (exponential) stability of a system. Consider the alteration of these axioms, where the different LTV systems are taken into consideration:

Axiom 4.1 Z¯f ⊂ Z¯t, Z¯f is closed and contains the origin in its interior; ∗ ¯ ¯ Axiom 4.2 µf (¯zt, λt) = Fj z¯t ∈ Ut, ∀z¯t ∈ Zf , λt ∈ Λj;

Axiom 4.3 A ∗ λ z ∈ Z¯ , ∀z ∈ Z¯ , ∀λ ∈ ; Fj ( t)¯t f ¯t f t Λj

∗ ∗ ∗ Axiom 4.4 V A ∗ λ z , P¯ V z , P¯ − ` z ,F z , f ( Fj ( t)¯t l) 6 f (¯t j) ¯t j ¯t ∀z¯t ∈ Z¯f , ∀λt ∈ Λj, ∀λt+1 ∈ Λl, l ∈ {j − 1, j, j + 1}.

Axioms 4.1, 4.2 and 4.3 are easily satisﬁed by deﬁnition and correct choice of ∗ Fj for each parameter space, e.g., the linear-quadratic regulator (LQR) gain for one of the systems, that makes all other systems stable. ¯ ∗ ∗ ¯ ¯ Remark 4.3 If for a given Zf there is no Fj that respects Fj Zf ⊂ Ut, then another Z¯f can be considered, e.g., one where recursion (4.9) initial condition is tighter.

Consider Axiom 4.4, which by algebraic manipulation can be written in the form,

T ∗T ∗ A ∗ λ P¯ A ∗ λ Q F RF − P¯ , Fj ( t) l Fj ( t) + + j j j 4 0

∀λt ∈ Λj, ∀λt+1 ∈ Λl, l ∈ {j − 1, j, j + 1}, 4.2. CONTROLLER FORMULATION 105

A ∗ λ˙ since the expected Fj can be disturbed, assume that is bigger than any disturbance the system could suﬀer. To guarantee disturbance rejection of system (4.1) consider the more constrained case, which still holds true for Axiom 4.4 under undisturbed conditions,

T ∗T ∗ ∗ ¯ ∗ ¯ AF (λi) PlAF (λi) + Q + Fj RFj − Pj 4 0, j j (4.15) ∀λt ∈ Λj, ∀λt+1, λi ∈ Λl, l ∈ {j − 1, j, j + 1}.

Hence, by defining P¯j to respect (4.15), Axiom 4.4 is automatically satisfied. To do this, consider P¯j = βjPj, where Pj is the solution of the algebraic Riccati equation for (4.5) for some λt ∈ Λj and βj > 1 is chosen for each parameter space such that (4.15) is satisfied for Λ. A possible solution is then given as, ¯ ¯ ¯ ¯ ¯ P = {P1, ..., Pj, ..., Pns }. The algorithm used to compute P is presented in the next subsection.

∗ Remark 4.4 In , F is maintained from A ∗ λ for A ∗ λ , note that (4.15) j Fj ( t) Fj ( i) ∗ ρ A ∗ λ < even with the feedback gain F , λ ∈ . This guarantees that the ( Fj ( i)) 1 j t Λj terminal cost is computed without any information of the disturbances other than they are smaller than λ˙ .

4.2.1 Terminal Weights Computation The idea behind the algorithm for the terminal weights computation is to ﬁrst compute candidate feedback gains and terminal weights for all systems that represent each parameter space. And then, pick one of these gains and terminal weights for each space. Finally, guarantee that a constant βj times the chosen terminal weight Pj for each parameter space veriﬁes equation (4.15). The algorithm to compute P¯ is presented in Algorithm 1. In line 3, the LQR function returns the terminal weight matrix and feedback gain associated with an LQR controller. The function eig computes the eigenvalues of the given matrix. In line 9 and 22, when l∈ / {1, ..., ns} that loop is skipped. In line 10 and 23, ρ(A˜) < 1, otherwise a feedback gain from another system is chosen. 106

Algorithm 1: P¯ Algorithm input : Q, R, Λj output: P¯ 1 foreach Λj, j = 1, ..., ns do 2 foreach (Aθ,Bθ) ∈ Λj do 3 P˜j,θ, F˜j,θ ← LQR(Q, R, Aθ, Bθ)

∗ 4 P ← ∅,F ← ∅ 5 foreach Λj ∈ Λ, j = 1, ..., ns do 6 ξ ← ∅, Pˆ ← ∅, Fˆ ← ∅, m1 ← 1 7 foreach (Aθ,Bθ) ∈ Λj do 8 α ← ∅, m2 ← 1 9 foreach (Aδ,Bδ) ∈ Λl, l = j − 1, j, j + 1 do 10 A˜ ← Aδ + BδF˜j,θ ˜T ˜ ˜ ˜T ˜ ˜ 11 X ← A Pl,δA + Q + Fj,θRFj,θ − Pj,θ

12 αm2 ← max (eig(X)) 13 m2 ← m2 + 1 ˆ ˜ ˆ ˜ 14 ξm1 ← max(α), Pm1 ← Pj,θ, Fm1 ← Fj,θ 15 m1 ← m1 + 1

16 m3 ← arg min(ξ) ˆ ∗ ˆ 17 Pj ← Pm3 ,Fj ← Fm3 n 18 P¯ ← P, β ← 1 ∈ R s , ← 0.01 19 repeat 20 β¯ ← β ¯ ∗ 21 foreach (Pj,Fj ), j = 1, ..., ns do 22 foreach (Aδ,Bδ) ∈ Λl, l = j − 1, j, j + 1 do ˜ ∗ 23 A ← Aδ + BδFj ˜T ¯ ˜ ∗T ∗ ¯ 24 X ← A PlA + Q + Fj RFj − Pj 25 α ← max (eig(X)) 26 if α > 0 then 27 βj ← βj + 28 break

29 P¯j ← βjPj

30 until β equal to β¯ 31 return P¯ 4.2. CONTROLLER FORMULATION 107

4.2.2 Assumptions Summary A summary of the assumptions made throughout the setup is as follows. Assumption 4.1 Sets Z, U and W are closed and bounded.

Assumption 4.2 The LTV model (4.2) can be written as an LPV system, or, at least, an outer approximation of it can be made.

Assumption 4.3 The state and control references are feasible and ﬁxed over the prediction horizon. Therefore, A (λ) and B (λ) are known a priori.

Assumption 4.4 The dynamics of the nonlinear discrete-time system (4.1), allow the partition of Λ into smaller parameter spaces and the evolution of each smaller space to another is well deﬁned.

Assumption 4.5 Set W˘ and consequently S can be computed using the vertex systems of Λ and a gain F that stabilizes all those systems.

Assumption 4.6 Set S is small enough to ensure that S ⊂ interior(Z) and F S ⊂ interior(U).

Assumption 4.7 Axioms 4.1, 4.2, 4.3, and 4.4 are satisﬁed.

z˘t = zt − zr,t, z˘t ∈ z¯t ⊕ S,

z¯0|t =z ¯t, z¯i|t ∈ Z¯i|t, (4.16) u¯i|t ∈ U¯i|t, z¯N|t ∈ Z¯f ,

λi|t ∈ Λ, λN−1|t ∈ Λj, ˙ N−1 λq|t − λh|t ∈ Λ, ∀i ∈ N0 , ns N−1 j ∈ N1 , ∀q ∈ N1 , N−2 ∀h ∈ N0 and the control law for time t is,

∗ ∗ µt = F (zt − zr,t − z¯t ) +u ¯0|t + ur,t, (4.17) 108

which is the same asµ ˘t (4.13), since by deﬁnitionz ˘ = z − zr andu ˘ = u − ur. The domain of the value function is deﬁned as, ZN = zt, zr,t, λt | ∃z¯t : zt − zr,t ∈ z¯t ⊕ S, UN (¯zt, λt) =6 ∅ . (4.18)

4.2.4 Stability Analysis

The solution of PN (˘zt, λt) (presented in (4.16)) at time t yields the optimal control sequence ∗ n ∗ ∗ ∗ o u¯t = u¯0|t, u¯1|t, ..., u¯N−1|t and ∗ n ∗ ∗ ∗ o z¯t = z¯0|t, z¯1|t, ..., z¯N|t

∗ ∗ the associated optimal state trajectory forz ¯0|t =z ¯t . The system satisﬁes

z˘t+1 = A (λt)z ˘t + B (λt)µ ˘t +w ˘t,

∗ ∗ ˘ with the control lawµ ˘t = F (˘zt − z¯t ) +u ˘0|t and ∀w˘t ∈ W.

Three important results for exponential stability of S follow.

∗ ∗ Proposition 4.2 Suppose (˘zt, λt) ∈ Z˘N as deﬁned in (4.14), so that u¯t and z¯t exist and are feasible for PN (˘zt, λt) deﬁned in (4.16). Then,

∀z˘t+1 ∈ A (λt)z ˘t + B (λt)µ ˘t ⊕ W˘ with n ∗ ∗ ∗ ∗ o u˜t+1 = u¯1|t, ..., u¯N−1|t,Fj z¯N|t and the associated state sequence

∗ ∗ ∗ z {z , ..., z ,A ∗ λ z } ˜t+1 = ¯1|t ¯N|t Fj N|t ¯N|t is feasible for PN (˘zt+1, λt+1), for λt+1 = {λ1|t, λ2|t, ..., λN|t} a shifted λt with one new added element λN|t.

∗ ∗ Proof: Since PN (˘zt, λt) is feasible all the constraints are satisfied by u¯t and z¯t . ∗ ∗ Hence,z ˘t ∈ z¯0|t ⊕S so it follows from Proposition 4.1 and (4.13) thatz ˘t+1 ∈ z¯1|t ⊕S. This means that, all the constraints are satisfied by the first N −1 elements of u˜t+1 and the first N of z˜t+1. Furthermore, sincez ¯N|t ∈ Z¯f , it follows from Axiom 4.2 ∗ F z ∈ U¯ A ∗ λ z ∈ Z¯ that j ¯N|t N|t and from Axiom 4.3 that Fj N|t ¯N|t f , and so the last elements of u˜t+1 and z˜t+1 satisfy the necessary constraints and the problem PN (˘zt+1, λt+1) is feasible. 4.2. CONTROLLER FORMULATION 109

Proposition 4.3 Since the problem is feasible ∀z˘t+1 ∈ A (λt)z ˘t + B (λt)µ ˘t ⊕ W˘ with the control sequence u˜t+1 and the associated state sequence z˜t+1 as deﬁned in Proposition 4.2, the value function satisﬁes,

VN (˘zt+1, λt+1) 6 VN (˘zt, λt) − ` (¯zt, u¯t) . (4.19)

Proof: Consider problem PN at time t + 1, PN (˘zt+1, λt+1). Consider an upper bound for VN (˘zt+1, λt+1), VˆN (˘zt+1, λt+1) the cost of applying the sequence u˜t+1 and the corresponding state trajectory z˜t+1. It is possible to derive, that ˆ ∗ ∗ VN (˘zt+1, λt+1) = VN (˘zt, λt) − ` z¯0|t, u¯0|t + ∗ T T ∗T ∗ ∗ z A ∗ λ P¯ A ∗ λ Q F RF − P¯ z , ¯N|t Fj N|t l Fj N|t + + j j j ¯N|t

∀λN|t ∈ Λj, ∀λN+1|t ∈ Λl, l ∈ {j − 1, j, j + 1}, since P¯j and P¯l respect (4.15) by deﬁnition, then, ˆ ∗ ∗ VN (˘zt+1, λt+1) 6 VN (˘zt, λt) − ` z¯0|t, u¯0|t and since, by construction,

VN (˘zt+1, λt+1) 6 VˆN (˘zt+1, λt+1),

∗ ∗ VN (˘zt+1, λt+1) 6 VN (˘zt, λt) − ` z¯0|t, u¯0|t , which is the same as (4.19).

Proposition 4.4 There exist constants c2 > c1 > 0, such that, with

z˘t+1 ∈ A (λt)z ˘t + B (λt)µ ˘t ⊕ W˘ , then

2 c1 |z¯t| 6 VN (˘zt, λt), ∀z˘t ∈ Z˘N , (4.20) 2 VN (˘zt+1, λt+1) 6 VN (˘zt, λt) − c1 |z¯t| , ∀z˘t ∈ Z˘N , (4.21) 2 VN (˘zt, λt) 6 c2 |z¯t| , ∀z˘t ∈ Z¯f ⊕ S. (4.22)

2 2 Proof: The assumption ` (¯z, u¯) > c1 |(¯z, u¯)| > c1 |z¯| in (4.11) and constraint z˘ ∈ z¯ ⊕ S guarantee (4.20). From the same assumption and Proposition 4.3, (4.21) ∗ ¯ ¯ is guaranteed. The assumption Vf z,¯ P 6 c2 |z¯| in (4.12) for allz ¯ ∈ Zf and Axiom 4.4, as shown in [359] and [319], guarantees (4.22). The proof is as follows. Letz ¯t be an arbitrary point in Z¯f and let u¯f,t = {u¯f,0|t, ..., u¯f,N−1|t} and 110

z¯f,t = {z¯f,0|t, ..., z¯f,N|t} forz ¯f,0|t =z ¯t be the control input and state sequences resulting from applying the controller µf (¯z, λt) deﬁned in Axiom 4.2, then by Axiom 4.4,

N−1 ∗ ∗ X Vf z¯t, P¯j Vf z¯f,N|t, P¯l + ` z¯f,i|t, u¯f,i|t , > i=0 ∀λt ∈ Λj, ∀λt+N ∈ Λl, l ∈ {j − N, .., j, ..., j + N}.

By optimality, sincez ¯f,N|t ∈ Z¯f (Axiom 4.3) andz ˘t ∈ z¯t ⊕ S,

N−1 ∗ X VN (˘zt, λt) Vf z¯f,N|t, P¯l + ` z¯f,i|t, u¯f,i|t , 6 i=0

∗ ¯ ¯ hence, VN (˘zt, λt) 6 Vf z¯t, Pj 6 c2 |z¯t| for allz ˘ ∈ Zf ⊕ S and for all N > 0.

The ﬁrst main result follows.

Theorem 4.1 The set S is robustly exponentially stable for the controlled uncertain system z˘t+1 = A (λt)z ˘t + B (λt)µ ˘t +w ˘t, z˘t ∈ z¯t ⊕ S, w˘t ∈ W˘ . The region of attraction is Z˘N .

Proof: For the sake of simplicity the following notation is simpliﬁed without loss of information or detail, VN (˘zt, λt) = VN (˘zt). This proof follows Mayne et al. in [320]. Letz ˘t+i denote the i-th solution of the system given its initial statez ˘t and disturbancesw ˘ ∈ W˘ . From Proposition 4.2, if z˘t ∈ Z˘N thenz ˘t+i ∈ Z˘N , ∀i ∈ N+. Consider V˜α = {z˘ | VN (˘zt) 6 α}, where V˜0 = S and there exists α > 0, such that V˜α ⊂ Z¯f ⊕ S. From Proposition 4.4, i c1 VN (˘zt+i) 6 τ VN (˘zt) , τ = 1 − ∈ (0, 1), c2

2 i 2 so, c1 |z¯t+i| 6 VN (˘zt+i) 6 τ c2 |z¯t| and,

i/2 |z¯t+i| 6 τ c3 |z¯t| , ∀z˘t ∈ V˜α, for c3 < ∞. Finally, since Z˘N is bounded, it follows from Proposition 4.3 that there exists a ﬁnite integer I, such thatz ˘t+i ∈ V˜α, ∀i > I, ∀z˘t ∈ Z˘N . Hence, there exists a constant c4 > c3 such that,

i/2 |z¯t+i| 6 τ c4 |z¯t| , ∀z˘t ∈ Z˘N .

Since, by deﬁnition,z ˘t+i ∈ z¯t+i ⊕ S the set S is robustly exponentially stable for the controlled uncertain system. 4.3. APPLICATION: AUTONOMOUS DRIVING 111

Remark 4.5 As noted by Rawlings et al. in [342] (Section 3.4.4) robust exponential stability of S for z˘t+1 = A (λt)z ˘t + B (λt)µ ˘t +w ˘t, z˘t ∈ z¯t ⊕ S, w˘t ∈ W˘ is not as strong as robust exponential stability of S for z˘t+1 = A (λt)z ˘t + B (λt)µ ˘t +w ˘t, w˘t ∈ W˘ . To prove the latter, it would have to be shown that for some c4 > 0 and i/2 ∀i > I, d(˘zt+i, S) 6 τ c4 d(˘zt, S). However, the proof above only shows that i/2 d(˘zt+i, S) 6 τ c4 |z¯t|, since d(˘zt+i, S) ≤ |z¯t+i|.

The main result of this Chapter follows.

Theorem 4.2 The set S is robustly exponentially stable for the controlled discrete- time disturbed nonlinear system zt+1 = f(zt, µt, wt), ∀wt ∈ W. The region of attraction is ZN , deﬁned in (4.18). Proof: By deﬁnition set W˘ contains both all possible w ∈ W and the linearization error for all possible zr ∈ Zr ⊆ Z and ur ∈ Ur ⊆ U. So, since,

zt+1 = zr,t+1 + A (λt)(zt − zr,t) + B (λt)(ut − ur,t) +w ˘t, is the same as (4.4) and set S is robustly exponentially stable for this system, as shown in Theorem 4.1, then, set S is also robustly exponentially stable for the controlled discrete-time nonlinear system subject to disturbances with a region of attraction ZN , deﬁned in (4.18).

4.3 Application: Autonomous Driving

In this section, an autonomous driving application of the linear time-varying robust MPC (LTVR-MPC) is presented. First, a road-aligned spatial bicycle model with curvature control input is presented. After, the LTVR-MPC is used and simulation results are presented.

4.3.1 Vehicle Model In this subsection, the vehicle model used in with LTVR-MPC for the simulation example is presented. Fig. 4.1 presents the relation between the road-aligned and the global coordinates frames for the vehicle model. The movement of a car-like nonholonomic vehicle at low speeds, with negligible lateral dynamics, can be approximately described by its time-domain equations according to [360] by, dx x˙ = = v cos (ψ), dt dy y˙ = = v sin (ψ), (4.23) dt dψ v ψ˙ = = tan (δ), dt l 112

ψ˙s

rs δ

eψ v ψs road centerline ey tangent s

x ψ

Fig. 4.1: Nonlinear bicycle model with both the global and road-aligned coordinate frames depicted.

where x and y are the coordinates of the vehicle in the global coordinate sytem, ψ is the yaw angle, v is the longitudinal velocity measured in the vehicle local coordinate system, l is the distance between the front and rear axle, and δ is the steering angle tan δ of the front wheel. The vehicle curvature κ is described by κ = l . Considering that for vehicle lateral control the objective is not to track a trajectory, but a path, the vehicle is modeled in the space domain and in a road-aligned coordinate frame along the reference path to exclude the time and velocity from the model equations. The derivation of the spatial-based vehicle model follows [287], where the distance along the reference path s, the lateral error to the path ey, and the yaw error eψ are introduced. The following geometric relations can be derived from Fig. 4.1,

dey e˙y = = v sin (eψ), dt deψ e˙ψ = = ψ˙ − ψ˙s, dt ds v cos (eψ) s˙ = = , dt 1 − κsey 4.3. APPLICATION: AUTONOMOUS DRIVING 113

where ψs is the yaw angle of the path and κs is the curvature of the path. The lateral and yaw errors, ey and eψ, and curvature of the path, κs, are computed using orthogonal projection of the vehicle onto the path. Assuming the vehicle is moving, i.e., v =6 0, and that the velocity is a continuous function. Using the chain rule of calculus to compute the derivative of the compo- d(·) d(·) dt dt 1 sition of two functions ds = dt ds and noticing that ds = s˙ it is possible to obtain the spatial-based representation of (4.23),

0 e˙y e = = (1 − κsey) tan (eψ), y s˙ (4.24) 0 e˙ψ (1 − κsey)κ 0 eψ = = − ψs , s˙ cos (eψ)

0 where ψs = κs is the spatial derivative of the path yaw. T Let the linearization of (4.24) around reference values for the states [ey, eψ] , T T [ey,r, eψ,r] = [0, 0] , and the control input κ, κr = κs, and then do zero-order hold (ZOH) using a spatial interval ∆s,t = Tsvs,t, which can be written in matrix form z¯t+1 = Atz¯t + Btu¯t, where cos(∆s,tκs,t) sin(∆s,tκs,t)/κs,t At = , −κs,t sin(∆s,tκs,t) cos(∆s,tκs,t) 2 (4.25) −(cos(∆s,tκs,t) − 1)/κs,t Bt = , sin(∆s,tκs,t)/κs,t are derived symbolically1 using MATLAB. Assuming κs and ∆s are constrained by κs,min, κs,max, ∆s,min and ∆s,max and γt = (At,Bt), it is clear that γt ∈ Γ, where Γ is a compact uncertainty set. Further- more, considering ∆s,t depends on the vehicle velocity, it is clear this parameter does not go from ∆s,min to ∆s,max without going through all possible values in between and also that its rate is limited due to vehicle acceleration limitations. Finally, consider a curvature/velocity limitation of the form described in [354],

ay,max κs = , v2 where curvature/velocity are limited so that the vehicle has at most a maximum lateral acceleration ay,max. This limitation is important, because it constrains the linearization error to smaller values. Considering all these limitations, it is possible to partition set Γ in Γj, where, if γi ∈ Γj then γi+1 ∈ Γl, for l ∈ {j − 1, j, j + 1}, as assumed in the problem formulation in Section 4.2. Hence, consider Λj contains all λt, such that,

(A(λt),B(λt)) ∈ Conv {Γj} , (4.26)

1 When κs = 0 the matrices (4.25) are undefined. However, before doing the ZOH it is possible to replace κs by zero and compute the matrices for this case. 114 and Λ = Conv {∪∀jΛj} , so Λj represents an outer approximation of Γj, and Λ represents an outer approximation of Γ, as required. To compute the disturbance set, W˘ , using (4.3), a ZOH version of the road- aligned vehicle model subject to disturbances is used as the discrete-time nonlinear model, which is equivalent to the simulation model. To compute the disturbance invariant set, S, using (4.6), the vertices of the polytope Λ are used. All the computations of the sets and terminal weights are done offline. Problem PN (zt −zr,t, λt) (4.16) runs online using (4.25) and, finally, the control law is (4.17).

4.3.2 Simulation The vehicle being controlled is simulated using an exact discretization version of (4.23) subject to disturbances. For the sake of simplicity, consider disturbances applied to v and κ. The states x, y, and ψ could also be subject to direct disturbances, but in this example they are not. The results presented are obtained using MATLAB/Simulink and qpOASES [348] to solve the optimization problem that can be written as a quadratic problem. The offline computations are done using MPT toolbox [361]. The controller runs at 50 Hz. It is assumed that a perfect measurement of the system is available at every instant and that velocity and/or curvature, which is −1 the control input, are disturbed, where wv ∈ Wv = {w | |w| 6 1/3.6} m s and −1 wκ ∈ Wκ = {w | |w| 6 0.01} m . The problem is set up for speeds ranging from −1 −1 −1 15 m s to 25 m s and road curvatures κs ∈ Ur = {κ | |κ| 6 0.1} m . The set −1 partitions are done at 0.5 m s , i.e., each Γj contains all possible systems in a 0.5 m s−1 interval; note that for the vehicle to ”jump” a set it would have an acceleration of 0.5 × 50 = 25 m s−2 , which is assumed to be impossible. The state constraints are z ∈ Z = {(ey, eψ) | |ey| 6 0.5 m, |eψ| 6 π/9 rad} and −1 the control constraints u = κ ∈ U = {κ | |κ| 6 0.2} m . Note that Z˘ = Z because the state reference is the origin. The problem solved is PN (zt − zr,t, λt) defined in (4.16) and the control law at each time instant is µt defined in (4.17). The horizon length is N = 10, where each point of the horizon is sampled at 0.2 s and the predicted velocity at each point is used to compute ∆s. In the cost function, Q = [1, 0; 0, 10] and R = 1 are used and P¯ is chosen to respect (4.15) and computed using Algorithm 1. The feedback gain F in µt (4.17) is computed to be the optimal unconstrained controller for one of the systems sampled at 50 Hz with Q and R weights. Using F , the set S is computed as polytopic, disturbance invariant, outer approximation for all possible systems sampled at 50 Hz for W˘ as in (4.3). Both W˘ and S are presented in Fig. 4.2. Note that, for all the possible systems in this example, the optimal unconstrained LQR controller chosen, F , makes all possible pairs of matrices A and B stable. 4.4. LIMITATIONS OF THE APPROACH 115

0.4 0.1 ˘ W Z¯ S 0.2 Z¯ Zf [rad] [rad] 0 0 ψ ψ e e

0.2 − 0.1 − 0.4 0.2 0 0.2 − 0.5 0 0.5 − − ey [m] ey [m]

Fig. 4.2: Disturbance set W˘ and distur- Fig. 4.3: State constraint sets: Z for the bance invariant set S. disturbed system; Z¯ for the nominal system; terminal constraint set Z¯f for the nominal system.

The sets U¯t, Z¯t and Z¯f are chosen to respect (4.7), (4.8) and (4.10), respectively, using F and S. The sets Z, Z¯ and Z¯f are presented in Fig. 4.3. Finally, it is important to mention that Axioms 4.1, 4.2, 4.3 and 4.4 are satisﬁed. Fig. 4.4 presents the result of a simulation considering the vehicle starting T at z0 = (0.4999, 0.0102) (one of the vertices of the polytopic reachable set) and subject to a random sequence of max-min disturbances. The light blue dashed line is the actual trajectory of the system, and the dark blue solid line is the sequence of optimal states for the nominal system. The setsz ¯t ⊕ S are shown in orange. Fig. 4.4a presents the lateral error to path vs time, it also presents the lane limits and center line by black solid and dashed lines. Fig. 4.4b presents the lateral vs yaw errors to path. To give an idea, the worst computation time for one step of the controller during multiple simulations was 12.8 ms, which is less than its sampling time. The average computation time for a step of the run with this maximum value was 0.393 ms.

4.4 Limitations of the Approach

The previous example considered the vehicle directly controlled with curvature. This allowed for the curvature to be bang-bang, i.e., to go from maximum to minimum value in one iteration. For a real-system this is normally not possible, since the system has physical constraints such as maximum and minimum curvature rate. To account for a continuous curvature model consider the addition of the error in curvature, eκ = κ − κs, as a state and the control is sharpness, c =κ ˙ , where cs 116

0.6

Z¯ 0.4 Z zS 0.2 z¯ lane

[m] 0 y e

0.2 −

0.4 −

0.6 − 0 0.2 0.4 0.6 0.8 1 1.2 1.4 t [s]

(a) Lateral error to path vs time.

0.4 Z¯ Zt 0.2 zS z¯

[rad] 0 ψ e

0.2 −

0.4 − 0.1 0 0.1 0.2 0.3 0.4 0.5 − ey [m]

(b) Lateral vs yaw errors to path.

Fig. 4.4: Simulation results for one of the vertices of the polytopic reachable set as T initial condition, z0 = (0.4999, 0.0102) . 4.4. LIMITATIONS OF THE APPROACH 117 is the reference sharpness. Then (4.24) becomes,

0 e˙y e = = (1 − κsey) tan (eψ), y s˙ 0 e˙ψ (1 − κsey)(κs + eκ) 0 eψ = = − ψs , (4.27) s˙ cos (eψ)

0 e˙κ (1 − κsey)(c − cs) eκ = = , s˙ v cos (eψ) which, as before for (4.25), can be linearized around the origin for the states T [ey, eψ, eκ] and for the control input c linearized around cr = cs, the sharpness of the path. Doing the same discretization as before it can be written in the form z¯t+1 = Atz¯t + Btu¯t, where

∆ κ 2  2 sin s,t s,t  sin(∆s,tκs,t) 2 cos (∆s,tκs,t) κ κ2  s,t s,t    At =  sin(∆s,tκs,t)  , −κs,t sin (∆s,tκs,t) cos (∆s,tκs,t)   κs,t  0 0 1

 sin(∆s,tκs,t)−∆s,tκs,t  (4.28) − 3 κ vs,t  s,t  ∆ κ 2  2 sin s,t s,t  Bt =  2  ,  κ2 v   s,t s,t    ∆s,t vs,t are derived symbolically2 using MATLAB. The same disturbances as in the example in Section 4.3.2 are considered,

−1 −1 wv ∈ Wv = {w | |w| 6 1/3.6} m s and wκ ∈ Wκ = {w | |w| 6 0.01} m .

The state constraints are −1 z ∈ Z(κs) = (ey, eψ, eκ) | |ey| 6 0.5 m, |eψ| 6 π/9 rad, |eκ + κs| 6 0.2 m , −1 κs ∈ {κ | |κ| 6 0.1} m .

Note that Z˘(κs) = Z(κs), because the linearization happens around the origin of the states. But Z(κs) depends on κs because the state constraint does not happen directly on the error. Rather it is a physical actuation constraint on κ, so the set Z(κs) is constantly shifted on the third dimension by the current path curvature. The values of κs are taken into consideration when computing

2 When κs = 0 the matrices (4.28) are undeﬁned. However, before doing the ZOH it is possible to replace κs by zero and compute the matrices for this case. The case where vs,t = 0 is not considered, because the vehicle would not be moving. 118

˘ 0.2 Z ] 1 −

[m 0 κ e

0.2 − 0.4 0.2 0.5 e 0 0 ψ [rad] 0.2 [m] − 0.4 0.5 ey − − (a) State set Z˘.

˘ W 0.01 ] 1 −

[m 0 κ e 0.01 − 0.01 0 0.01 e 0 ψ [rad] 0.01 0.01 [m] − − ey (b) Disturbance set W˘ computed as in (4.3) using (4.27) and (4.28). Fig. 4.5: 3D state set Z˘ and disturbance set W˘ .

W˘ (4.3), but, for simplicity, ignored when plotting the following Figures, where Z˘ = Z(0). This is done without loss of generality, since all sets would shift with κs accordingly. Furthermore, once again for simplicity and also because it leads to a smaller S set, only a velocity of 17 m s−1 is considered. Finally, the −1 −1 control constraints u = c ∈ U = {c | |c| 6 0.05} m s and the control reference −1 −1 cs ∈ {cs | |cs| 6 0.04} m s are considered. Fig. 4.5 presents both the state set Z˘ and the disturbance set W˘ computed as in (4.3) using (4.27) and (4.28). Fig. 4.6 presents vertices of set S. The set is a convex polytope with more than 338 thousand vertices making it computational expensive to plot all vertices and color all faces of the polyhedron, so only a fraction of the vertices are presented. Furthermore, interpreting and discussing the results in 3D is signiﬁcantly harder than in 2D. So, for the sake of simplicity, all vertices of the set are projected onto 4.4. LIMITATIONS OF THE APPROACH 119

vertices S

0.1 ] 1 −

[m 0 κ e

0.1 −

0.4 0.2 1 e 0 0 ψ [rad] [m] 0.2 1 ey − 0.4 − − Fig. 4.6: 3D Vertices of S (the polytope has more than 338 thousand vertices so only a fraction is presented). the plane eκ = 0, and from those points the set S2D is created and is presented in Fig. 4.7. Fig. 4.7 also presents the state set Z2D, where the notation ·2D has the same meaning as for set S, i.e., its the 3D set projected onto the plane eκ = 0.

Remark 4.6 On the previous example in Section 4.3.2 the disturbance invariant set S is computed until (4.6) converges. For this example, the computational complexity is signiﬁcantly larger and this equation does not converge easily. So, the minimal robust positively invariant (mRPI) set is invariantly approximated using the algorithm originally presented in [356] and extended in [362] which computes an outer -RPI approximation of the mRPI. For these examples = 1 × 10−3, smaller means a better approximation of the mRPI, but it also takes longer to compute.

From Fig. 4.7 is is clear that S2D 6⊂ interior(Z2D), meaning that Assumption 4.6 is not fulﬁlled. So, when using (4.8) to compute the nominal state set Z¯, it leads to an empty set. Then, problem PN (˘zt, λt) (4.16) and its MPC are infeasible, because there is no nominal statez ¯ that can be inside an empty set. Note that looking at the results in 2D or 3D does not change the argument, since if S2D 6⊂ Z2D in 2D, S is also not a subset of Z in 3D. For this setup and system, the controller, does not work. The controller is too conservative since it tries to handle all the linearization error and disturbance error for all possible systems. This means that for a more complex system, which does not allow bang-bang control on curvature (as presented in the previous example) the set S grows larger than the state set Z which renders the controller useless. Clearly, the correction of disturbances and linearization errors does not happen fast 120

0.4 ˘ Z2D S2D 0.2

[rad] 0 ψ e

0.2 −

0.4 − 1.5 1 0.5 0 0.5 1 1.5 − − − ey [m] Fig. 4.7: 2D representation of sets Z˘ and S. enough given the feedback gain chosen to correct the system and maintain it inside set S. On the previous example, the allowed error in |ey| 6 0.5 m is derived by considering the width of a truck (≈ 2.5 m) and a highway lane (≈ 3.5 m) and dividing the difference by 2 to have the allowed deviation from the center of the lane. A similar analysis was not considered for the allowed error in yaw |eψ| 6 π/9 rad, this value was chosen because it seemed reasonable. However, let us consider the truck centered in a straight lane with a eψ = π/9 and that the distance between the rear and front wheels of the truck is 5 m. This means that the center of the truck front wheels has an error ey,front = 5 tan(π/9) ≈ 1.8 m, i.e., the front of the truck is outside the lane. Consider the inverse approach, which eψ can the truck have, so that it only de- viates 0.5 m on the front point, arctan(0.5/5) ≈ 0.0997 rad < π/30. Then, consider the following state set, −1 z ∈ Zβ(κs) = (ey, eψ, eκ) | |ey| 6 0.5 m, |eψ| 6 π/30 rad, |eκ + κs| 6 0.2 m , −1 κs ∈ {κ | |κ| 6 0.1} m , where β represents this new setup and is used to differentiate from the first setup presented in this Section. Finally, the previous example also considers a curvature disturbance of 5 % of the actuation limit 0.2 m−1. Let us consider a smaller disturbance of around 1 %, i.e.,

−1 wκ ∈ Wβ,κ = {w | |w| 6 0.0025} m . 4.4. LIMITATIONS OF THE APPROACH 121

˘ W ˘ 0.01 β

] W 1 −

[m 0 κ e 0.01 − 0.01 0 0.01 e 0 ψ [rad] 0.01 0.01 [m] − − ey

Fig. 4.8: Comparison of disturbance set W˘ and W˘ β.

The rest of the setup β is left unchanged to the first setup. Fig. 4.8 presents both the W˘ from the initial setup and W˘ β for the modified β setup. Clearly, the modifications in allowed eψ and wk greatly reduce the linearization and disturbance error. In turn, this leads to a much smaller Sβ making the problem feasible. Fig. 4.9 presents the 2D representation of both Sβ and Zβ, where clearly Sβ,2D ⊂ interior(Zβ,2D), making the problem Pβ,N (˘zt, λt) (4.16) possibly feasible. Note that all Assumptions 4.1-4.7 need to be verified, but that is out of scope in this discussion.

Remark 4.7 It is not enough that Sβ,2D ⊂ interior(Zβ,2D) to make the problem Pβ,N (˘zt, λt) feasible, since this does not mean Sβ ⊂ interior(Zβ). The latter was veriﬁed by certifying that all vertices of Sβ are inside Zβ.

Fig. 4.10 presents both the 3D and 2D representations of the nominal state set Z¯β computed as in (4.8). This set has a major inﬂuence on the feasibility of the MPC since all nominal statesz ¯ need to be inside of it. Considering that in this case the setup is only done with one velocity, this set can be considered too small for the robust MPC controller to be useful. Note that after the nominal statez ¯ reaches the origin the control law becomes the LQR feedback gain, as can be seen in the example presented in Section 4.3.2. And, with a set Sβ that covers most of the state-space Z˘β, the nominal state set Z¯β becomes small, and the nominal system would reach the origin signiﬁcantly fast. Even when only considering one velocity, in the worst case scenario, with the LQR control law, disturbances, and linearization error considered, the system is only guaranteed to have a lateral error to path smaller than ≈ 0.3 m, since this is the maximum ey value of the set Sβ. Furthermore, note that this is with perfect knowledge of the system being controlled. A real vehicle has more unknowns, i.e., the prediction model is less accurate, and there are other road and environment 122

0.1 ˘β,2D Z Sβ,2D 0.05

[rad] 0 ψ e

0.05 −

0.1 − 0.4 0.2 0 0.2 0.4 − − ey [m]

Fig. 4.9: 2D representation of sets Z˘β and Sβ.

¯β,2D ¯ Z 0.2 Zβ 0.02 ] 1 −

[rad] 0

[m 0 ψ κ e e 0.02 0.2 − − 0.02 0.2 0 0 eψ 0.2 0 0.2 [rad] 0.02 0.2 [m] − − ey − ey [m]

(a) 3D representation. (b) 2D representation.

Fig. 4.10: Nominal state set Z¯β. 4.5. CONCLUSION 123 disturbances. This leads to the conclusion that the performance achieved is not good enough and the approach is considered too conservative. All in all, it is important to study and understand how to decrease the linearization error and if possible the disturbance error. First, the state-space of the controller needs to be better studied. It is clear from Fig. 4.8, that the linearization error caused by eψ and wκ have a big effect on the disturbance set W˘ (4.3). So, the state-space limits for eψ need to be studied from a practical point of view to understand which values need to be considered. Second, a feedback controller can be considered to generate a new linearization trajectory starting from the current point of the system. The linearization around this trajectory would automatically decrease set S, because the linearization error from the current point would disappear. Third, the disturbances that the real system is subject to also need to be better understood to have the minimal necessary disturbance sets. Note that the disturbance sets for a real system also need to consider the difference between the prediction model and the real system. Furthermore, the sharpness example presented in this section only considers a fixed velocity, since the linearization error on the states grows with increased velocity. When the linearization error of the system at higher velocities has to be corrected by slower velocity systems, set S grows even larger. To reduce the influence of all systems on each other, set S should also consider the parameter variation rate such that there exists one Sj for each set Λj. Each Sj takes into consideration a disturbance set W˘ j defined as in (4.3) but for the systems in the convex hull of Γj (see equation (4.26)) and its neighboring sets, i.e., Conv{Γl} with l ∈ {j − 1, j, j + 1}. Note that this might not be trivial when applied to this approach, because the controller still needs to respect all assumptions and propositions presented. In particular, the MPC cost function still has to decrease over time, which might not be easy or even doable with different S sets on different iterations.

4.5 Conclusion

This chapter presented a robust model predictive controller for discrete-time nonlinear systems subject to disturbances. The controller is relatively simple, although its setup, which is done offline, is fairly complex. Online, the controller optimizes the initial statez ¯0, of the nominal model, which is constrained to be in the vicinity of the real system, and a sequence of control actions subject to tighter constraints. The optimal control problem is posed as a quadratic program, for which there are multiple efficient solvers available. Since, the value function is zero in the disturbance invariant set S, robust exponential stability of S is achieved. The approach was verified by means of simulation with a simple autonomous driving example. Finally, the example was modified and made more complex and realistic, which allowed for the discussion of some of the shortcomings and future possible directions of the approach.

Chapter 5

Reference Aware Model Predictive Control

ontrolling an autonomous vehicle is not only about tracking a path, but also C doing it in a comfortable and smooth manner, while respecting vehicle actuation constraints. For industrial applications having good tracking and smooth behavior is of extreme importance, because vehicles are supposed to run uninter- ruptedly for long periods of time. Aggressive actuations result in high wear and tear, which shortens the components and trucks lifetime. Moreover, aggressive maneuvers can lead to other undesired behaviors, such as, vehicle rollovers and/or cargo damage/loss. On the other hand, the control method still needs to track the path with significantly small errors. For example, consider a truck inside a mine, where often the vehicle barely fits passages to go through. This chapter presents a lateral controller for autonomous vehicles called reference aware MPC (RA-MPC). The controller is designed to achieve the desired objectives, as well as the designed actuation limits presented in Chapter 3. For HDVs and the control problem addressed in this chapter, the SA-MPC [94, Ch.7] can be considered the state-of-the-art controller. However, this controller is out- dated with respect to the current vehicle configuration. The SA-MPC was designed taking into consideration a less constrained steering actuator, i.e., the steering actuator was able to produce high torque values resulting in large steering rates and its limitations were constant throughout operation. Today, due to the necessity of complying with safety regulations, more constrained actuators are used, which makes the system dynamics more important to understand and deal with. In particular, the overall vehicle presents slower dynamics than before, which is a greater challenge for a motion controller. The RA-MPC is able to systematically handle motion planner references not generated for the specific discretization and prediction model used in the controller. This allows for the lateral controller to be aware of imperfections on the reference control signals, which are then corrected by the optimization. This is achieved

125 126 by computing an extra error within the MPC setup and accounting for it in the controller’s cost-function. The performance of MPC is highly dependent on the accuracy of the prediction model used. Vehicle models can be very complex, where, e.g., tire forces, road friction, slip angles, and suspension forces can be considered, or else they can be simpler, e.g., consider the kinematic bicycle model with no slip [363]. The complexity of the model influences the complexity of the MPC making it harder to solve in real-time. Thus, a trade-off between the fidelity of the vehicle model and computational resources needs to be made when considering real-time systems. The RA-MPC is originally developed with the spatial-based road-aligned kinematic bicycle prediction model. However, vehicle experiments show a significant mismatch between requested and measured vehicle curvature. As a consequence, this Chapter also presents a prediction model modification to account for the nonlinear curvature response of the vehicle. The curvature response is approximated by two Gaussian functions, where the parameters are identified using experimental data. Finally, this chapter verifies both the RA-MPC and the model modification through realistic simulations and experiments with a Scania construction truck. Moreover, the SA-MPC is used as a performance baseline for comparison. The work presented in this chapter is based on the papers:

The remaining part of this chapter is organized as follows. Section 5.1 presents the problem statement. Section 5.2 introduces the proposed controller, including the initial LTV-MPC formulation, and the method to handle non-perfect path references. Section 5.3 presents the vehicle modeling, including the standard kinematic bicycle model, and the model modiﬁcation to account for the vehicle’s nonlinear curvature response. In Section 5.4 both simulation and experimental results with a heavy-duty vehicle are presented, where the proposed RA-MPC is benchmarked to the SA-MPC. This section also introduces the scenario, control parameters, and performance metrics for the evaluation and comparison. Finally, in Section 5.5, some concluding remarks are made.

5.1 Problem Statement

Consider a heavy-duty vehicle like the one presented in Fig. 5.1. This type of vehicles have rather slow time-varying dynamics, which makes the task of path 5.1. PROBLEM STATEMENT 127

Fig. 5.1: A modified R580 Scania truck used for the experiments. following challenging. Furthermore, the steering actuation limitations discussed in Chapter 3 make it even harder. The work presented in this Chapter makes use of the control architecture presented in Chapter 3 and focuses on the lateral controller block. The controller receives the delay predicted vehicle states at time t (ˆzt), the sampled planned path (zp), and the constraints for each path point (zp) in the form of control input constraint sets Ut, U˙t, and U¨t, for curvature, sharpness (curvature rate), and curvature acceleration, respectively. The controller sends a high-level curvature request for the system to achieve the desired behavior. The system, in this case, also includes low-level controllers implemented to track the received request, which for the pur- pose of this work cannot be modified and are considered as a black-box. The objective of this work is to improve the performance of the path following controller, while maintaining certain error bounds, respecting actuation constraints, and providing a safe and comfortable ride. This Chapter, reformulates the optimal control problem and introduces, for the LTV-MPC framework, a method to systematically handle references generated by planner modules with different algorithms and models from the controller. Finally, as discussed before, when a curvature (κ) is requested, the system does not follow it exactly, because of both external and internal disturbances. Consider 1 ψ˙ Fig. 5.2, where a snippet of the measured curvature (in red, κψ˙ = v , the quotient between measured yaw-rate and velocity) is compared to the curvature request (in green, κ) for an experiment with the vehicle in Fig. 5.1. Clearly, the predicted motion by the MPC is not followed exactly. To further improve the controller’s performance, the nonlinear curvature response of the system is modeled and used in the controller.

1The signal presented is shifted to account for system delay. 128

1 10− · 3 κ ˙ κ 10− ψ 0.5 3 · ]

1 0 −

[m 3

κ − 2,000 2,500

1,500 2,000 2,500 3,000 s [m]

Fig. 5.2: Curvature measured (κψ˙ ) and curvature request (κ) for a experiment with the vehicle presented in Fig. 5.1.

5.2 Controller Formulation

In this section, the proposed path following controller is formulated and the method to handle references generated by diﬀerent motion planners is presented.

5.2.1 LTV-MPC Consider the disturbed nonlinear system,

dz z˙ = = f(z, u, w), dt where z ∈ Rn is the state vector, u ∈ Rm is the input vector, and w ∈ Rm+n accounts for disturbances. These are constrained by closed and bounded sets,

z ∈ Z, u ∈ U, w ∈ W.

To control the system with a linear MPC, the disturbances are assumed negligible and handled by the control feedback loop. Consider the system states, zt, and the control input, ut, at time t. The system can be linearized using a ﬁrst-order Taylor approximation with respect to state references, zr,t, and control input references, ur,t, and then discretized using, e.g., zero-order hold (ZOH), obtaining the following dynamics, z˘t+1 = Ar,tz˘t + Br,tu˘t, (5.1) where t is a time instant and t + 1 the next time instant, the variablez ˘t = zt − zr,t, and the variableu ˘t = ut − ur,t. The LTV-MPC with N prediction horizon steps can be formulated as a quadratic program, with quadratic cost on state and control input error, subject to control 5.2. CONTROLLER FORMULATION 129 input constraints,

T PN−1 T T min z˘N|tQz˘N|t + i=0 z˘i|tQz˘i|t +u ˘i|tR0u˘i|t u˘t s.t. N−1 z˘i+1|t = Ai|tz˘i|t + Bi|tu˘i|t, ∀i ∈ N0 , z˘0|t =z ˘t = zt, N−1 ui|t ∈ U, ∀i ∈ N0 , where, u˘t = [˘u0|t, u˘1|t, ..., u˘N−1|t]. The matrices (Ai|t,Bi|t) are deﬁned in (5.1) for the references at time t + i, and zt is the system state vector received by the controller. Finally, Q and R0 are positive deﬁnite weights and give the state and control input costs, respectively.

5.2.2 Controller Design To achieve smooth and accurate driving behavior, it is necessary to consider how the system is actuated. The objective is to control the center of the truck’s rear- axle and have it follow a reference path, but the control actuation is a couple of meters ahead at the front-axle. Thus, to have better performance of the overall system, not only the lateral error (ey) and yaw error (eψ) to path are penalized but also the front-axle error to path, such that the length of the vehicle is taken into consideration. To achieve this without increasing the number of system states, a possible approach is to use a weighting matrix Q with nonzero oﬀ-diagonal elements, instead of the common diagonal matrix [339]. For a straight reference path the front-axle error of the vehicle can be written as, ey,f = ey + l sin(eψ), where l is the distance between the front and rear axles of the vehicle. T Using ey,f , the state vector is deﬁned as g(˘z) = [ey, eψ, ey,f ] , which can be linearized around the origin to obtain a linear relationship withz ˘, i.e., g(˘z) ≈ dg dz˘ 0z˘ = Mz˘. Consider now the quadratic cost on g(˘z),

g(˘z)TQg˜ (˘z) =z ˘TM TQM˜ z.˘

T ˜ ˜ Matrix Q in (5.3) is deﬁned as Q = M QM, where Q = diag(Qey ,Qeψ ,Qey,f ) is positive deﬁnite with the desired penalization for each state, i.e., Qey + Qey,f l Qey,f Q = 2 . l Qey,f l Qey,f + Qeψ

The objective of the controller is to follow a path by requesting a vehicle curvature at every time instant. The vehicle models used by the planner and controller are never perfect with respect to the real vehicle. This implies that the reference control input does not make the vehicle track the path with null error. Then, the 130 controller constantly has to request a diﬀerent value from the reference to achieve the desired tracking, this means that tracking the control input reference is not important, and its cost is set to 0, i.e., R0 = 0. Note, that the models are assumed to be accurate enough, such that, the feedback loop is able to correct the errors and make the vehicle track the path. Furthermore, the controller generates references to a servo motor that turns the steering-wheel angle to the desired position to achieve the requested curvature. Not only does the servo motor have physical actuation limitations, but also, due to safety and comfort concerns, the servo motor actuation angle, rate, and acceleration capabilities are limited. These limitations can be accounted for by constraining the allowed control input. Consider the curvature rate2 κ˙ , and the curvature accelerationκ ¨, to be computed as,

κt − κt−1 κ˙ t = , (5.2a) ∆T t κ˙ t − κ˙ t−1 κ¨t = , (5.2b) ∆T t where, ∆Tt is the time diﬀerence between instants t and t − 1. The curvature and rates are then bounded to be inside time-varying, velocity dependent, sets, i.e.,

κt ∈ Ut, κ˙ t ∈ U˙t, κ¨t ∈ U¨t, ∀t.

These sets are computed oﬀ-line, based on previously recorded data, and evaluated on-line, according to the current velocity, to obtain the current maximum and minimum actuation limits. More details can be found in Chapter 3. Remark 5.1 The use of discrete diﬀerence approximation for the rates does not compromise the continuous time validity of the constraints, because the constraint sets are built using the same approximations.

To guarantee problem feasibility at all times, the curvature rate and acceleration constraints are softened, with the help of two slack variables, λ1 and λ2. The variable λ1 is used for all curvature rate constraints and λ2 for all acceleration constraints. Moreover, both slack variables are quadratically and linearly penalized by a positive cost Λ, and are also constrained to be non-negative. The quadratic penalty allows for a well posed QP [334], since the Hessian matrix is positive deﬁ- nite, and the linear cost guarantees a non-smooth cost function ensuring the slack variables penalty to be exact [335]. Finally, the control requests should be smooth, to reduce wear and tear of the vehicle components, improving the up-time and lifetime of the vehicle. Furthermore, smooth requests also lead to a safer and more comfortable ride and, although a heavy-duty autonomous truck will not have passengers, a bus will, so comfort is important. Both the curvature rate (κ ˙ ) and acceleration (¨κ) are penalized, and

2Also known as sharpness, c =κ ˙ . 5.2. CONTROLLER FORMULATION 131 not the difference to the reference. This design choice is made in order to handle non-smooth curvature references being sent from the planner. For each prediction T T point two extra terms are added to the cost function:u ˙ i|tR1u˙ i|t andu ï|tR2uï|t, where R1 and R2 are positive tuning parameters. In contrast to the SA-MPC [94, Ch.7], this work computes both the control input rate and acceleration in time and not in space. The controller runs periodically with a constant frequency, i.e., the algorithm runs with a constant time-interval between iterations. Designing the rates in time makes it easier to guarantee constraint satisfaction rather than designing them in space, since the second approach would depend on the vehicle speed, and would lead to a space-varying interval between iterations. Furthermore, the desired closed-loop behavior is to have an equally smooth control input request for all speeds, so the rate and acceleration of the control input is to be penalized always in the same manner. If the rates are computed and penalized with a space-varying interval between iterations (as in the SA-MPC), it would mean that for increased sample distance the rates would be less penalized, since the spatial rate is smaller at higher velocities for the same curvature change between two consecutive points. This is not the desired behavior, since it would allow for faster and more aggressive changes in the control input request. The desired behavior can be easily obtained by computing and penalizing the rates in time, because the controller is running periodically, which leads to the same rate change for the same curvature change between two consecutive requests, independently of the current speed. Consider the following MPC formulation,

min JS u˘t,λ1,λ2 s.t. z˘i+1|t = Ai|tz˘i|t + Bi|tu˘i|t, z˘0|t =z ˘t =z ˆt, (5.3) u˘i|t = ui|t − ur,i|t, ui|t ∈ Ui|t, ˙ ¨ d(u ˙ i|t, Ui|t) 6 λ1, d(¨ui|t, Ui|t) 6 λ2, N−1 2 λp > 0, ∀i ∈ N0 , ∀p ∈ N1

2 X T T JS = λp Λλp + Λλp +z ˘N|tQz˘N|t+ p=1 , N−1 X T T T z˘i|tQz˘i|t +u ˙ i|tR1u˙ i|t +u ¨i|tR2u¨i|t i=0 which, in this Chapter, is called standard MPC, or S-MPC for short. This formulation takes into account the designed stage cost, through matrix Q, the designed control cost through R1 and R2, and the steering actuation limitations, through the sets Ut, U˙t, and U¨t. It also accounts for the system delay by using the predicted vehicle states,z ˆt, as the initial states of the vehicle for the optimization. 132

5.2.3 Controller References As explained in Chapters 2-3, the planner module sends a path for the controller to follow, which can be described in multiple ways. A common approach is sending a set of points containing information about the desired vehicle poses, curvatures, velocities, and accelerations. This general interface allows more flexibility for testing different planners and algorithms, since the communication interface is not planner or controller specific. Furthermore, Chapter 3 presents how the vehicle is projected to the path and how the MPC references are sampled. These computations have discretization and other approximation errors, specially when it comes to the computation of the necessary control input signal. If the controller’s prediction model is different than the planner’s model, the control input references have to be recomputed for the controller’s prediction model to follow the state references. A common approach to compute the reference control signals is to use the inverse prediction model to find out what control input takes the system between two (consecutive) states. Depend- ing on the model and its discretization this is not trivial to do. Furthermore, this can lead to noisy signals, since it is done with finite differences over approximated signals, and noisier references inevitably lead to noisier control signals. On the other hand, the MPC is solving an optimization problem to compute the best control input to take the vehicle back to the path. This makes it possible to use the planned references sampled at the desired points, and penalize eventual differences between prediction model-generated references and the original path references. Using this approach, there is no need to recompute the reference control signals for the controller prediction model, avoiding the noisy signals obtained from finite differences and solving issues with infeasible paths. The idea is to use the sampled planned control input references to predict a path with the controller’s prediction model. Then, the error between the predicted path and received planned path is computed and included in the cost function of the MPC. LTV-MPC usually assumes that given no error to correct, following the control references means following the state references. As explained before, this is highly dependent on having the correct references for the prediction model. To better understand why this is important consider Fig. 5.3, where an example is presented. In the example, the planned path at time t (zp,t, presented in black) is created using an Euler discretized kinematic bicycle model with 1ms between points, a constant −1 −3 −1 −1 speed, v = 10m s , a constant sharpness, c = 1×10 m s (which creates up,t, the planned curvature, presented in black), and with all initial states equal to zero. Moreover, consider that the vehicle is currently traveling with the same velocity 3 and direction as the path , but it has 1 m of lateral error (ˆzt, red dot in Fig. 5.3a). 4 Now consider the sampling of the planned path using ∆Tt = 0.44 s (zp,t and ur,t) presented in long-dash-red in Fig. 5.3. Note that the sampled control signal (ur,t) presents a stair-like behavior, since the control input is assumed constant

3 For simplicity, in this example, it is assumed there is no system delay, i.e.,z ˆt = zt. 4 The same value of ∆Tt is used in Section 5.4 for the results. 5.2. CONTROLLER FORMULATION 133

1.5 zp,t zˆt zp,t zg,t ze,t 1 [m]

Y 0.5 0 0.5 − 0 10 20 30 40 X [m]

(a) Paths.

2 10− 0.6 · ] 1

− 0.3

[m up,t κ ur,t 0 0 1 2 3 4 5 Time [s]

(b) Curvature signals.

Fig. 5.3: Reference aware importance example. between points (ZOH). Furthermore, consider the vehicle poses zg,t (presented in dash-dot-blue in Fig. 5.3a) predicted by forward simulating the linearized and discretized controller prediction model using the sampled control references from the planned path, starting on the current projected position of the vehicle on the path. The reference awareness of the controller aims at correcting the error ze,t = zg,t−zp,t (presented for a point in Fig. 5.3a), i.e., the error between the predicted vehicle poses and the planned ones. Consider the diﬀerence between the vehicle states and the planned path for a single point,z ¯t = zt − zp,t. Furthermore, note that the linearization of the prediction model is being done around zg,t, i.e., zr,t = zg,t, because of the control input references. Thenz ¯t can be written as,

z¯t = zt − zr,t + zg,t − zp,t =z ˘t + ze,t, by subtracting zr,t and adding zg,t. To achieve the desired correction, the optimization problem (5.3) is then modiﬁed to penalizez ¯ in the cost function, instead ofz ˘. 134

To conclude the example, the optimization problem is run with and without knowledge of the error ze,t, i.e., by penalizingz ¯ orz ˘, respectively. This leads to the ? optimized control input sequences uz,t¯ , presented in dash-green in Fig. 5.4b, and ? uz,t˘ , presented in short-dash-orange in Fig. 5.4b, respectively. This optimized control input sequences are then forward simulated using the linearized and discretized MPC prediction model, starting on the current position of the vehicle, leading to ? ? zz,t¯ and zz,t˘ , respectively, presented in Fig. 5.4a. Clearly, the path generated by the optimized control input with knowledge of the error ze,t tracks the planned path ? much better. Furthermore, the last optimized control inputs uz,t˘ are following the ? sampled planned control inputs, while uz,t¯ are not. This is expected, since, in this case, more control input is required than the planned one to track the path with the controller’s prediction model. Although this procedure removes the assumption that an LTV-MPC requires good references to work well, it adds a diﬀerent assumption. The new assumption is that the controller’s prediction model describes more accurately the movement of the system than the model used to plan the path.

5.2.4 Reference Aware MPC

The formulation for the reference aware MPC, or RA-MPC for short, is,

min JRA u˘t,λ1,λ2 s.t. z˘i+1|t = Ai|tz˘i|t + Bi|tu˘i|t, z¯i|t =z ˘i|t + ze,i|t, z¯0|t =z ¯t =z ˘0|t =z ˘t =z ˆt, (5.4) u˘i|t = ui|t − ur,i|t, ui|t ∈ Ui|t, ˙ ¨ d(u ˙ i|t, Ui|t) 6 λ1, d(¨ui|t, Ui|t) 6 λ2, N−1 2 λp > 0, ∀i ∈ N0 , ∀p ∈ N1

2 X T T JRA = λp Λλp + Λλp +z ¯N|tQz¯N|t+ p=1 N−1 X T T T z¯i|tQz¯i|t +u ˙ i|tR1u˙ i|t +u ¨i|tR2u¨i|t i=0 where the control input is curvature, u = κ, and the states are the lateral and T yaw errors to path, z = [ey, eψ] . Compared to (5.3),z ¯ is penalized in the cost function instead ofz ˘. To compute the curvature rates and accelerations, the previous requested control inputs κt−1 and κt−2 are saved and used at time t. The optimization problem is posed as a QP and solved online at every iteration. 5.2. CONTROLLER FORMULATION 135

zp,t zˆt 1.5 zp,t zg,t ? ? zz,t¯ zz,t˘ 1 [m]

Y 0.5

0.5 − 0 10 20 30 40 X [m]

(a) Paths.

2 10− · 0.9

0.6 ] 1

− 0.3 [m

κ 0

up,t ur,t 0.3 ? ? − uz,t¯ uz,t˘ 0.6 − 0 1 2 3 4 5 Time [s]

(b) Curvature signals.

Fig. 5.4: Reference aware importance example - optimized paths. 136

5.3 Vehicle Model

For completeness, this section starts by re-introducing the standard road-aligned spatial-based kinematic bicycle model presented in Chapter 4. Then, it presents the vehicle curvature response modeling, including how to include it in the kinematic model.

5.3.1 Road-Aligned Spatial-Based Kinematic Bicycle Model

The model is a coordinate transformation of the conventional kinematic bicycle model. Fig. 5.5 presents the relation between the road-aligned and the global coordinate frames for the vehicle model.

ψ˙s

rs δ

eψ v ψs road centerline ey tangent s

x ψ

Fig. 5.5: Nonlinear bicycle model with both the global and road-aligned coordinate frames depicted.

The movement of a car-like nonholonomic vehicle at low speeds, with negligible lateral dynamics, can be approximately described by its time-domain equations 5.3. VEHICLE MODEL 137 according to [360] by, dx x˙ = = v cos (ψ), dt dy y˙ = = v sin (ψ), (5.5) dt dψ v ψ˙ = = tan (δ), dt l where x and y are the vehicle’s coordinates in the global coordinate system, ψ is the yaw angle, v is the longitudinal velocity measured in the vehicle local coordinate system, l is the distance between the front and rear axles, and δ is the steering tan δ angle of the front wheel. The vehicle curvature κ is described by κ = l . This work only considers the improvement of the lateral controller of the system. The lateral controller objective is to track a path and not a trajectory. So, the vehicle is modeled in the space domain and in the road-aligned coordinate frame along the reference path to exclude time and velocity from the model equations. The derivation of the spatial-based vehicle model follows [287], where the distance along the reference path s, the lateral error to the path ey and the yaw error eψ are introduced. The following geometric relations can be derived from Fig. 5.5,

dey e˙y = = v sin (eψ), dt deψ e˙ψ = = ψ˙ − ψ˙s, (5.6) dt ds v cos (eψ) s˙ = = , dt 1 − κsey where ψs is the yaw angle of the path and κs is the curvature of the path. The lateral and yaw errors, ey and eψ, and curvature of the path, κs, are computed using orthogonal projection of the vehicle onto the path. Assuming the vehicle is moving, i.e., v =6 0, and that the velocity is a continuous function, the chain rule of calculus can be used to compute the derivative of the d(·) d(·) dt dt 1 composition of two functions ds = dt ds . Noting that ds = s˙ , it is possible to obtain the spatial-based representation of (5.6),

0 e˙y e = = (1 − κsey) tan (eψ), y s˙ (5.7) 0 e˙ψ (1 − κsey)κ 0 eψ = = − ψs , s˙ cos (eψ)

0 where ψs = κs is the spatial derivative of the path’s yaw. Finally, consider the system states, zt, and the control input, ut, at time t, and T do the linearization of (5.7) around reference values. For the states z = [ey, eψ] , T T [ey,r, eψ,r] = [0, 0] , and for the control input u = κ, κr = κs. Next, do zero-order hold (ZOH) using a spatial interval ∆st = ∆Ttvt to obtain the system written in 138

z zp

Path Sampling

Lateral Control zp - Planned Path z - Vehicle States κ

zp - Planned Path Sampled Nonlinear Mapping κ - Curvature Request SWA req SWA - Steering-Wheel Angle

SWA Control SWAreq - SWA Request

SWA

Steering Column

Fig. 5.6: Simpliﬁed lateral controller actuation architecture.

matrix form asz ˘t+1 = Atz˘t + Btu˘t, where the notationz ˘t = zt − zr,t and the matrices cos(∆st κst ) sin(∆st κst )/(κst ) At = , −κst sin(∆st κst ) cos(∆st κst ) (5.8) − κ − /κ2 (cos(∆st st ) 1) st Bt = , sin(∆st κst )/κst are derived symbolically5 using MATLAB.

5.3.2 Vehicle Curvature Response Modeling In (5.5), the vehicle’s yaw-rate is assumed to be v κ, which may be inaccurate when referring to a real vehicle. Consider Fig. 5.6, where a simpliﬁed lateral controller actuation architecture is presented. When the curvature request (κ) is made to the vehicle this value is transformed into a steering-wheel angle (SWA) using a nonlinear mapping. The SWA is then physically actuated by a servo motor on the vehicle’s steering-wheel, which turns the steering column resulting in the front-axle wheels turning. Besides the nonlinearities in the curvature to steering conversion, and the nonlinearities and diﬀerent responses of the actuators, there are also tire

5 When κs = 0 the matrices (5.8) are undeﬁned. However, before doing the ZOH it is possible to replace κs by zero and compute the matrices for this case. 5.3. VEHICLE MODEL 139

1 10− ·

1 κψ˙ (κ) κ κ α(κ) 0.5 ] 1 −

[m 0 ˙ ψ κ 0.5 −

1 − 1 10− 1 0.5 0 0.5 1 · − − 1 κ [m− ]

Fig. 5.7: Curvature response for the vehicle presented in Fig. 5.1 in yellow. Kine- matic bicycle model curvature in blue. Proposed curvature model in green. forces and external disturbances to consider. In the end, the requested curvature will most likely not lead to the MPC predicted motion of the vehicle. This problem is highly related to understeer and oversteer of the vehicle. However, instead of looking at steering angle to yaw-rate, this work considers modeling the system from curvature request to measured vehicle curvature (according to the kinematic model) and everything else is treated as a black-box. Fig. 5.2 shows that there is a mismatch between the measured curvature and the MPC requested curvature. The idea is to change the MPC model to account for this curvature response. Note that this mismatch could also be accounted for outside the MPC by doing an extra nonlinear mapping or controller. However, doing that has the big disadvantage that the control input constraints would most likely not be respected for some requests. For example, if the MPC requests maximum curvature rate and then the actual curvature request would be increased even more, the maximum curvature rate request limit would not be respected.

Fig. 5.7 presents curvature request against curvature measured (κψ˙ (κ) in yel- low6) for multiple experiments with the vehicle presented in Fig. 5.1. As expected, the same request has a large range of responses due to noise, nonlinearities, and dis-

6This is presented in the form of a set, being easier to understand. The number of measured points is very large so in the end it would look very similar. 140

1 α

0.5 αψ˙ (κ) αpoly αw α(κ) 1 0 10− 0.6 0.3 0 0.3 0.6 · − − 1 κ [m− ]

Fig. 5.8: Variable α measured for the vehicle presented in Fig. 5.1 in yellow. Poly- nomial fit (using symmetrical κψ˙ ) in red, and in blue the sliding window average. Proposed α(κ) function in green. turbances. Furthermore, Fig. 5.7 illustrates the kinematic bicycle curvature model presented in Section 5.3.1, κ, in blue. ψ˙ κψ˙ Consider the variable α = v κ = κ , i.e., the ratio between measured and 6 requested curvature. Fig. 5.8 presents the measured α points (αψ˙ (κ) in yellow ), which have some symmetry around the y-axis. This property is explored for easier mathematical modeling, so α(κ) is considered an even function. If α(κ) is even, the curvature response is odd, i.e., symmetrical with respect to the origin. To account for this symmetry the original data is doubled and the origin symmetrical points are included. Moreover, for the symmetrical data, consider a first degree polynomial fit (αpoly, presented in red in Fig. 5.8) and the sliding window average (αw, presented in blue in Fig. 5.8), made with 1 × 10−3m−1 window centered on the desired point7. The sliding window average, αw, presented in Fig. 5.8 resembles two negative Gaussian or exponentials of the form,

2 2 κ κ − a − b α(κ) = a1e 2 + b1e 2 + c1, (5.9)

7 Both, αpoly and αw, are actually obtained using the symmetrical κψ˙ (κ) and then divided by κ, such that, all points are included and there is no loss of information. 5.4. RESULTS 141

where a1, a2, b1, and b2 are parameters that define the size and shape of the exponentials, and c1 defines the position on the y-axis. Then the curve fitting toolbox function fit from MATLAB, is used to fit the α(κ) parameters for (5.9) to both the linear polynomial and moving average points, using tuned weights such that close to zero the points are well fitted. This leads to the function α(κ) presented in green in Fig. 5.8, and to the κ α(κ) proposed curvature model presented in green in Fig. 5.7. For the vehicle presented in Fig. 5.1 the function parameters are listed in Table 5.1.

a1 a2 b1 b2 c1 −0.1669 0.0174 −0.4513 0.0032 1.0136

Table 5.1: α(κ) function parameters.

This function is now used in the prediction model by replacing the yaw-rate in (5.5) by

ψ˙α = v κ α(κ). (5.10)

The function α(κ) changes the model behavior for low-curvature values, reducing the yaw-rate of the standard model down to ≈ 40% around zero for the same curvature request. In practice, to follow low curvature paths, the controller will request more curvature to compensate for the response of the vehicle. This model leads to a better prediction of the vehicle states, due to the improved prediction of the vehicle’s yaw-rate using (5.10), which includes the curvature response (5.9). Finally, to obtain the A and B matrices for the MPC, the road-aligned model is derived and then linearized and discretized in the same manner as in (5.8).

5.4 Results

This section presents simulation and experimental results. It starts by introducing the performance measurements and the scenario used to evaluate the controller. Afterwards, the control parameters and setup are speciﬁed. Then, an analysis of the contributions made in this Chapter is done by means of simulations. Finally, the performance of the proposed RA-MPC is benchmarked against the SA-MPC [94, Ch.7] in simulations as well as in real-world experiments using the HDV presented in Fig. 5.1. It is important to note that the SA-MPC controller was designed without knowledge of the more constrained and tightened safety and comfort limitations implemented on the steering motor actuator. Not only is the controller re-tuned, but it is also expected that the behavior and results presented are worse than the results previously reported, since the system is now more constrained. 142

1,000 Track Start End

500 [m] Y

1,200 800 400 0 − − − X [m]

Fig. 5.9: X and Y path coordinates of the scenario.

5.4.1 Performance It is important to deﬁne the controller key performance indicators (KPIs). The KPIs are task, scenario, and vehicle speciﬁc. In some situations a better performance might be being very accurate in following the path and, for that, use more aggressive control inputs. In other situations a better performance might be being less accurate in following the path, but smoother when using the control inputs. These two cases can be found, e.g., on a truck driving in a mining environment and a bus driving on public roads with passengers, respectively. For safety and accuracy, the most important variable is the lateral error to path (ey). In this work it is assumed that the path is obstacle free close to the planned path, |ey| 6 0.5 m. For smoothness, two metrics are considered: the rate and acceleration of the curvature request. Both these metrics provide insight on how fast and aggressively the steering-wheel is rotating, which in turn indicate how the vehicle’s yaw angle is changing. This means that larger changes in curvature are associated with less smooth behavior. Furthermore, constant changes in curvature, with less peaks, are also better since the stress and forces applied to all vehicle components are smaller. Both these metrics are computed as in (5.2). For each performance metric both the maximum of the absolute value, | · |max, and the average of the absolute value, | · |, are presented.

5.4.2 Scenario The scenario used to evaluate the controller is presented in Fig. 5.9. It starts with an almost straight part where the vehicle speeds up until 50 km h−1, then slows down and does a long turn with a maximum curvature of 0.052 m−1. After, it comes into 5.4. RESULTS 143 the second almost straight part and speeds up again to 50 km h−1. Finally, it makes a second large turn with a maximum curvature of 0.073 m−1 and stops afterwards. The maximum velocity of 50 km h−1 is used for safety reasons. Furthermore, the scenario is run on an asphalt road, that has varying road grade and bank (not accounted for in simulation), and other disturbances, e.g., manholes.

5.4.3 Controller Setup The controller runs at 50 Hz and uses the architecture presented in Chapter 3. It is assumed that a measurement of the system is available at every sampling instant. Furthermore, at each iteration the vehicle speed, used in the controller’s prediction horizon, is assumed constant and the same as the current measurement. This is a reasonable assumption since a heavy-duty vehicle has slow dynamics. The velocity control is assumed to be done by a separate controller. All controllers use 10 prediction steps and the MPC problems are solved online as QPs using qpOASES [348]. Results are obtained for ﬁve diﬀerent controllers: 1. SA: SA-MPC [94, Ch.7];

2. Ss: the controller presented in (5.3) without reference awareness and with the standard kinematic model, i.e., withz ˘ on the cost function and the model presented in Section 5.3.1;

3. Sα: the controller presented in (5.3) without reference awareness and with the modiﬁed kinematic model with α(κ), i.e., withz ˘ on the cost function and the model presented in Section 5.3.2;

4. RAs: the controller presented in (5.4) with reference awareness and with the standard kinematic model, i.e., withz ¯ on the cost function and the model presented in Section 5.3.1;

5. RAα: the controller presented in (5.4) with reference awareness and with the modified kinematic model with α(κ), i.e., withz ¯ on the cost function and the model presented in Section 5.3.2. 8 The parameters for the SA-MPC [94, Ch.7] are λ = 0.15, α = 1000, ey,max = 0, and Ts = 0.2 s. The references for this controller are computed for the forward Euler discretized model, doing finite differences of the desired planned positions. All S and RA controllers use the same control parameters to make it possible to compare the results between them, their parameters are presented in Table 5.2:

Qey Qeψ Qey,f R1 R2 Λ ∆Tt 1 25 1 1.5 0.001 106 0.44

Table 5.2: Control parameters. 8These variables are not introduced in this Chapter, see [94, Ch.7]. 144

It is important to mention that the results from [94, Ch.7] for the SA-MPC cannot be used nor compared to the ones presented in this Chapter, since not only a diﬀerent vehicle is used, but also the steering actuation system used is more restricted on the current platform. Moreover, the SA-MPC had to be re-tuned to perform the scenario well. The tuning for both SA and RA is performed for the real vehicle. This means that the results in simulation could most likely be improved, by tuning for the simulation. On the other hand, this way of tuning has the advantage of allowing for a direct comparison between results, and verify how diﬀerent the vehicle is from the simulator. To obtain the current constraint sets Ut, U˙t, and U¨t, the steering constraints presented in Fig. 3.8 are evaluated at every iteration with the current vehicle speed. Then, the obtained values are transformed into curvature constraints using the inverse nonlinear mapping of curvature to steering-wheel angle. As a design choice, −1 −1 the curvature rate is further constrained to U˙t ∩ [−0.05; 0.05]m s , which might or might not be the actuation limit depending on the vehicle’s current speed.

Remark 5.2 Note that the α(κ) function parameters presented in Table 5.1 are only used for the real vehicle. For simulation the α(κ) function parameters are designed using the procedure presented in Section 5.3.2, but for data collected with the simulator. This corresponds to one disadvantage of using a vehicle speciﬁc model, i.e., the model needs to be ﬁt to each individual vehicle.

5.4.4 Simulations

Simulation allows for the showcase of the importance of different controller aspects. The simulation environment is fully developed using MATLAB/Simulink. The vehicle simulator is succinctly described in Lima’s PhD thesis [94, Ch.3.1]. Ad- ditionally, the current version of the simulator includes the safety limitations and constraints on the steering-wheel actuation. The simulator also generates virtual sensor data, that is fed to an estimation module, which outputs the current vehicle states estimate to the controller. These modules are courtesy of Scania CV AB. Fig. 5.10 presents bar charts with five simulation results, one for each controller. All controllers operate within the safety limits for the lane, since |ey|max 6 0.5 m. Consider Fig. 5.11, where the reference awareness contribution is studied. The top plot compares the results for the two controllers using the standard kinematic curvature model, and the bottom plot compares the results for the two controllers using the proposed model including the α(κ) function (5.9). For the controllers with the standard model, Ss and RAs, the results are very similar on the higher speed parts of the track, but the RAs performs significantly better on the high curvature segments of the track (around 1500 m and 3200 m). This result can also be verified from Fig. 5.10, which shows the RAs controller (red bars) performance metrics are always significantly smaller than the Ss controller ones. The best and most significant performance improvement is on maximum 5.4. RESULTS 145

SA Ss Sα RAs RAα

3.23

m] 1.78 1 max |

− 1.82 y e [10 | 1.05 1.18

m] 2.43 | 2 y − e 2.36 |

[10 2.18 1.32 ] 1 1.53 − s

1 4.17 − max

| 4.17 m ˙ κ 2 | 3.3 − 3.3 [10 ] 1 1.05 − s

1 1.52 | − ˙ κ 1.57 | m 3 1.44 − 1.5 [10 ] 2 1.17 − s

1 3.11 − max

| 3.11 m ¨ κ 1 | 2.15 − 3.09 [10 ] 2 0.87 − s

1 2.94 | − ¨ κ 3.02 | m 3 2.11 − 2.25 [10

Fig. 5.10: Performance metrics comparison for simulation results. 146

0.1

0 [m] y e 0.1 − Ss RAs 0.2 − 0 1,000 2,000 3,000

0.1

0 [m] y e 0.1 − Sα RAα 0.2 − 0 1,000 2,000 3,000 s [m]

Fig. 5.11: Reference aware comparison - lateral error to path simulation results.

lateral error (|ey|max), where the RAs is better by more than 7 cm, an improvement of about 40 %. For the controllers with the curvature response modified model, Sα and RAα, the results are clearly better with the reference awareness controller, RAα. The RAα is able to follow the reference path much more accurately on the higher speed segments of the track and has less maximum lateral error to path (|ey|max) than the Sα controller. The performance metrics in Fig. 5.10 confirm the improvement of the reference awareness, where the RAα has all metrics smaller than the Sα controller. The most significant performance enhancement is for the lateral error to path, where the maximum (|ey|max) is 35 % smaller for the RAα compared to the Sα, and the average (|ey|) is 44 % smaller. Clearly, the reference awareness, i.e., penalizingz ¯ in the MPC cost function, instead ofz ˘, improves the performance of the controller. The impact of the nonlinear curvature response model modification is presented in Fig. 5.12. The top plot compares the results for the S-MPC presented in (5.3) using both the standard kinematic curvature model and the modified model, and the bottom plot does the same comparison but using the RA-MPC (5.4). For the standard MPC, Ss and Sα, there is no clear improvement or deterioration of performance by changing the model. Note that in this controller the reference correction is not active. This means that the feed-forward part of the control law (i.e., the control input reference), is never corrected with respect to the model. 5.4. RESULTS 147

0.1

0 [m] y e 0.1 − Ss Sα 0.2 − 0 1,000 2,000 3,000

0.1

0 [m] y e 0.1 − RAs RAα 0.2 − 0 1,000 2,000 3,000 s [m]

Fig. 5.12: Model comparison - lateral error to path simulation results.

For the reference aware MPC, RAs and RAα, the inclusion of α(κ) in RA- MPC yields significant improvements. Fig. 5.12 shows that during the higher speed parts of the track the RAα tracks the path significantly better, and on the high curvature parts of the track the maximum error to path (|ey|max) is very similar between both controllers. These results are confirmed by Fig. 5.10, where the average lateral error (|ey|) to path is shown to be about 40 % smaller for the RAα compared to the RAs. Note that the small 1 cm increase in maximum lateral error (|ey|max) is not a concern, since both controllers maximum lateral error (|ey|max) is very small. Most of the other metrics are very similar between the controllers, with the exception of maximum curvature acceleration (|κ¨|max). The bigger maximum curvature acceleration (|κ¨|max) is considered to be a good trade-off, because of the major improvement on average lateral error (|ey|) and the fact that the average curvature acceleration (|κ¨|) is very similar for both controllers. Finally, consider the results comparison presented in Fig. 5.13 of the SA controller with both the RAs and RAα. The simulation results of the RA-MPC with either model have significant less lateral error to path, so the RA-MPC is considered better. This is confirmed by the KPIs presented in Fig. 5.10, which shows that the RA-MPC improves the maximum lateral error to path (|ey|max) by more than 20 cm, an improvement of more than 60 %. When using the standard kinematic model, the RA controller improves the average lateral error to path (|ey|) by 27 % comparing to the SA controller, and when using the curvature response modified model the average lateral error to path (|ey|) for the RA controller is 56 % better 148

0.3 SA RAs RAα 0.2

[m] 0.1 y e 0

0.1 − 0 1,000 2,000 3,000

Fig. 5.13: Simulation results of previous work smooth and accurate MPC (SA- MPC) compared to the reference aware MPC (RA-MPC), presented for both models considered. than the SA. On the other hand, the average curvature rate (κ ˙ ) and acceleration (¨κ) metrics are better for the SA controller. However, since the diﬀerence in lateral error to path is very big, the RA-MPC is considered better. Furthermore, if the SA-MPC is tuned to have equal maximum lateral error9 to path as the RA- MPC, the average curvature rate and acceleration metrics are bigger than for the RA controller. For example, for the acceleration metrics they are more than ten times larger for the SA-MPC than for the RA-MPC, when tuned to have similar maximum lateral error to path. For the controllers to be run on a vehicle they need to be real-time implementable. The computation times for an iteration of the RA-MPC with the proposed model are measured, and the worst time in multiple runs is less than 2.9 ms. Since the controller runs at 50 Hz it is real-time implementable. These measurements were done with a laptop running an Intel(R) Core(TM) i7-8850H CPU at 2.6 GHz, Windows 10, and MATLAB 2016b. Note that, all controllers were veriﬁed, and, as expected, the RA-MPC with the curvature response model is the one that takes longer to solve.

5.4.5 Experiments

This section presents results from real-world experiments on a Scania HDV (see Fig. 5.1) using the diﬀerent MPC controllers. Fig. 5.14 presents the KPIs for four experimental results. Experiments are only run for the SA and RA controllers. Furthermore, these results were originally presented in the papers on which this

9In fact this was not possible to achieve. The SA (in simulation) is always a few cm worse than the RA. 5.4. RESULTS 149

SAdry RAs,dry RAs,wet RAα,wet

3.77 m]

1 2.66 max | −

y 3.22 e [10 | 2.01

9.14 m] |

2 7.44 y − e | 7.5 [10 4.94 ] 1 −

s 1.86 1 3.33 − max | m

˙ 3.68 κ 2 |

− 3.34 [10 ] 1 −

s 2.12 1

| 2.01 − ˙ κ | m 2.1 3

− 2.59 [10 ] 2 −

s 5.83 1 2.42 − max | m

¨ 2.59 κ 1 |

− 2.63 [10 ] 2 −

s 7.17 1

| 1.92 − ¨ κ | m 2.64 2

− 1.66 [10

Fig. 5.14: Performance metrics comparison for experimental results obtained with the vehicle presented in Fig. 5.1. 150

0.4 0.3 0.2 0.1

[m] 0 y e 0.1 − 0.2 − 0.3 − SAdry RAs,dry 0.4 − 0 1,000 2,000 3,000 s [m]

(a) Lateral error to path. 1 10− · SAdry RAs,dry 0.5 ] 1 − 0 [m κ 0.5 − 0 1,000 2,000 3,000 s [m]

(b) Curvature request.

Fig. 5.15: Experimental results on dry asphalt road for the SA and RAs controllers.

Chapter is based. For the two papers, the experiments were run on diﬀerent days, which led to diﬀerent road conditions. The comparison between the SA and RAs is performed based on results collected with a dry asphalt road, while the comparison between the RAs and RAα is performed on the same road but with wet asphalt. The is explicitly mentioned on the controller legend by adding dry or wet on the name, e.g., RAα,wet.

Consider the lateral error to path, ey, and the curvature request, κ, along the path, for the dry experiments for both SA and RAs controllers presented in Fig. 5.15. First, note that the controllers obey the specified safety limits for the lane, since |ey|max 6 0.5 m. Both the KPIs presented in Fig. 5.14 and the results presented in Fig. 5.15, show that the RAs controller performs better than the SA controller. The RAs is able to follow the path with better accuracy in almost 5.4. RESULTS 151 all metrics comparing to the SA. In particular, the curvature acceleration metrics (|κ¨|max and |κ¨|) are significantly smaller for the RAs, because the controller is aware of the limitations associated with this state and conforms with them. The only metric that is worse for the RAs controller is the maximum curvature rate (|κ˙ |max) used. However, since this is only one point, the average curvature rate (|κ˙ |) is actually smaller, and the tracking error (|ey|max and |ey| metrics) is also smaller for the RAs, this is considered to be a good trade-off. Moreover, note the very similar maximum requested curvature rate in the simulation (Fig. 5.10) and experiment (Fig. 5.14) for the RAs controller. Thus, this value is expected to be able to follow the path at the achieved lateral error accuracy. Furthermore, Fig. 5.15 shows that the RAs,dry controller performs better on the high curvature parts of the track (around 1500 m and 3200 m) since it has significantly less lateral error to path, and on the higher speed and straighter segments of the path it has smaller amplitude and frequency of oscillations. In fact, a frequency analysis of both oscillatory parts of the ey signal, shows that the peak frequencies for the SAdry controller are 0.149 Hz and 0.157 Hz, with 0.114 m and 0.067 m amplitude, respectively, and for the RAs,dry 0.122 Hz and 0.138 Hz, with 0.080 m and 0.055 m, respectively, which confirms the more comfortable experience obtained in the vehicle with the RA-MPC. This behavior is also clear from the curvature plot in Fig. 5.15b, since the blue line oscillates with a slower frequency and has less amplitude than the orange line. Comparing the results obtained for the RAs controller in dry and wet road conditions presented in Fig. 5.14, it is clear that a wet road leads to a deterioration of the controller performance in all KPIs. Not much more can be commented, besides that this result is expected, since the driving on a wet road is harder than on a dry one. Note, that the controller is not adapting to the road conditions, i.e., there is no change in model parameters or controller tuning being done. Consider the lateral error to path, ey, and the curvature request, κ, along the path, for the wet experiments for both RAs and RAα controllers presented in Fig. 5.16. Once again, the controllers perform within the safety limits for the lane, since |ey|max 6 0.5 m. The results presented in Fig. 5.14 show that the RAα is better than the RAs, since it is able to follow the path with better accuracy in almost all metrics, specially in the lateral error to path. There are two metrics where the standard model behaves better than the modified model, |κ¨|max and |κ˙ |. The maximum curvature acceleration (|κ¨|max) being higher for RAα than for RAs, is not a concern since this is just one point, and because the average curvature acceleration(|κ¨|) is significantly smaller for RAα, so over the whole run RAα uses less curvature acceleration. The average curvature rate (|κ˙ |) being higher for RAα than for RAs is of more concern, but when compared to the major improvement on lateral error to path (|ey|max and |ey| metrics) this is considered to be a good trade-off. Fig. 5.16 shows that both models have very similar behavior on the high curvature parts of the track (around 1500 m and 3200 m). This behavior is expected since α(κ) is very close to 1 for high curvature values (Fig. 5.8), i.e., the models are very 152

0.3 0.2 0.1

[m] 0 y e 0.1 − 0.2 − 0.3 RAs,wet RAα,wet − 0 1,000 2,000 3,000 s [m]

(a) Lateral error to path. 1 10− · RAs,wet RAα,wet 0.5 ] 1 − 0 [m κ 0.5 − 0 1,000 2,000 3,000 s [m]

(b) Curvature request.

Fig. 5.16: Experimental results on wet asphalt road for the RAs and RAα controllers. similar for high curvature values. On the higher speed and straighter parts of the track the tracking error is signiﬁcantly smaller for RAα and the oscillations have signiﬁcantly smaller amplitude. However, the oscillations have a higher frequency for RAα than for RAs. A frequency analysis of both oscillatory parts of the ey signal shows that the peak frequencies for RAs,wet are 0.140 Hz and 0.118 Hz, with 0.077 m and 0.053 m amplitude, respectively, and for RAα,wet are 0.210 Hz and 0.1779 Hz, with 0.025 m and 0.019 m, respectively. From these values it is hard to say which one provides a more comfortable experience since the RAs,wet oscillates slower but with more than double the amplitude. Subjectively, the in-vehicle experience is more or less the same, being comfortable for both, and maybe feeling slightly safer for the RAα,wet since there is less lateral movement on the lane. 5.5. CONCLUSION 153

Finally, it is important to note that the simulation results do not present the type of oscillations seen in the experiments with the real vehicle, meaning that their cause comes from the more complex and difficult to control real system. However, as mentioned before, the oscillations are sufficiently slow and have sufficiently small amplitude to have a comfortable and safe experience in the vehicle. The improvements were also observed by the test drivers who felt comfortable during the entire motion execution. In conclusion, the RA-MPC and the inclusion of the nonlinear curvature response model have shown to bring significant improvements to the lateral controller for autonomous HDV vehicles. This conclusion is mostly linked to the major improvement in path following lateral error accuracy, which improved10 by more than 45 % in both metrics (|ey|max and |ey|) when compared to the SA-MPC.

5.5 Conclusion

This Chapter presented the reference aware MPC (RA-MPC) and a vehicle specific model modification for lateral control of autonomous heavy-duty vehicles. The previous state-of-the-art for the lateral control problem considered, the SA-MPC, has problems with respect to the current configuration and limitations imposed on the automated test vehicles used, which leads to reduced performance. The RA-MPC is able to handle these constraints and limitations, while following the path more accurately due to the reference awareness. Moreover, the yaw-rate state modification with two Gaussian functions to account for the nonlinearities of the vehicle’s curvature response, further improved the controllers performance when compared to the standard kinematic bicycle model. The proposed model and controller achieve a lateral error to path accuracy more than 45 % better than the SA-MPC in both metrics considered.

10 Note that this is comparing a dry run (SAdry) with a wet (RAα,dry) run. The improvement should be even bigger for the same conditions.

Chapter 6

Conclusions and Future Work

utonomous vehicle technology promises greener, more energy efficient, more A convenient, and safer means of transportation. However, only partial levels of automation are currently available and there are still open problems to address in all modules of the system. This thesis addressed the lateral motion control problem for heavy-duty vehicles using model predictive control. First, a control architecture was presented using general interfaces to be easily applicable to different vehicles and to different planner modules. Then, a linear time-varying robust MPC framework was presented and applied to a simple autonomous vehicle example. Furthermore, some shortcomings of the approach were discussed with a second more complex example. Afterwards, the reference aware MPC (RA-MPC) was introduced to explicitly handle references not necessarily generated for the prediction model being used in the controller. This controller also accounts for time-varying steering constraints and optimizes smoothness of the control request. Experiments were run to verify the controller and compare to previous work. Finally, the prediction model used in the RA-MPC was modified to account for the curvature response of the vehicle. Which was, once again, experimentally tested and verified with a Scania construction truck. The following sections discuss in more detail the main conclusions and present future possible research directions.

6.1 Conclusions

First, the control module architecture considered was presented. The objective of the control module is to follow the references received by the planner, while maintaining certain error bounds and providing a safe and comfortable ride. Moreover, it needs to handle system delays and disturbances. The delays can be, e.g., from computation, communication, actuation, and feedback. The disturbances can be internal, such as, model uncertainty, unmodeled actuator dynamics, or diﬀerent loads during operation, or external disturbances, such as, potholes, rain, snow, or gravel.

155 156

Furthermore, it is important that the controller is easily applicable to multiple vehicles and compatible with other modules. A generic planner-controller interface was used, as well as a generic high-level control input request was considered in the lateral and longitudinal controllers. The control module architecture considered is composed of six blocks: delay prediction, path sampling, constraints setup, longitudinal control, lateral control, and output conversion. Delay prediction accounts for the system delays by predicting the future states of the vehicle, which are used to control the vehicle at each iteration. Path sampling, samples the path points received by the planner guaranteeing continuity between them, allowing for a well posed and feasible problem. The constraints setup defines the current actuation limitations, based on the vehicle’s states and the desired behavior. The longitudinal and lateral controllers compute the desired high-level reference request to be actuated on the vehicle and make the vehicle follow the trajectory/path. The output conversion converts the high-level reference into a vehicle specific steering wheel-angle and velocity reference requests to be tracked by the vehicle controllers. Second, a robust model predictive controller framework for discrete-time disturbed nonlinear systems was presented. The controller bounds the LTV systems on an LPV framework, and makes use of the parameter variation rate to more efficiently compute the control law. The control law is composed of a convex QP optimization and a feedback gain. The MPC optimizes the nominal model states, which are constrained to be in the vicinity of the disturbed system, and a sequence of control actions subject to tighter constraints. The constraint sets are computed in the offline setup, which is fairly complex. These sets are computed based on the feedback gain, a disturbance set, and the disturbance invariant set. Finally, the disturbance invariant set was proved to be robust exponentially stable for the disturbed nonlinear system. The proposed robust controller was initially verified with a simple autonomous vehicle lateral control example. The example considers disturbances in both curvature and velocity of the vehicle, and a curvature controlled kinematic bicycle model. As expected, the controller successfully stabilized the vehicle to path, while correcting the disturbances. Then, with the help of a more complex vehicle model that considers sharpness (derivative of curvature) as control input, the limitations of the proposed controller were discussed. It is argued that for the framework to be applicable in practice, further work needs to be done. Having concluded that the real system needs to be better studied before the proposed robust controller is applicable, it was decided to take a step back and consider nominal control of the system, assuming that the system disturbances, model mismatch, and linearization errors are small enough to be handled by the feedback loop. This lead to the reference aware MPC (RA-MPC). The RA-MPC is able to systematically handle motion planner references not generated for the specific discretization and prediction model used in the controller. Furthermore, the RA-MPC was designed accounting for the time-varying actuation constraints and control objectives of being smooth and accurate while tracking the path. The 6.2. FUTURE WORK 157

RA-MPC was compared with the previous state-of-the-art SA-MPC and is considered better, achieving a significantly improvement in path tracking accuracy and smoothness. The initially proposed RA-MPC controller was designed using a standard kinematic bicycle model. However, as expected, this model showed a significant mismatch between the curvature request and the curvature measured on vehicle experiments. To improve the prediction model of the controller, a two Gaussian function approximation was proposed to model the nonlinear curvature response of the vehicle. This approximation was then taken into account by modifying the yaw-rate state of the kinematic model with the proposed function map. The RA- MPC with the proposed model achieved a maximum lateral error to path of 20 cm, which corresponds to a performance improvement of around 35 % when compared to the results obtained with the RA-MPC and the original, unmodified, prediction model. Finally, when compared1 to the SA-MPC the proposed model and controller achieved a lateral error to path accuracy more than 45 % better in both metrics considered.

6.2 Future Work

In this section possible future research directions are discussed. First, future work for each chapter is discussed. After, other interesting and related directions are mentioned

Chapter 3: Control Architecture

The lateral controller is correcting the vehicle to path using the vehicle’s predicted states to account for the system delay. This prediction is being done with an open- loop simulation of the vehicle model, which has mismatch to the real vehicle. This leads to a mismatch in the actual error the vehicle has at the predicted point. To account for this and have better predicted states a ﬁlter can be implemented on the prediction error. Then the corrected predicted states can be used in the controller. The value of time delay used at each iteration for the vehicle delay prediction is currently considered to be known. This is not trivial to assume since it has been seen in experiments that the delay is not static. In fact, the delay depends on current conditions and vehicle states. Therefore, estimating this delay online may lead to great improvements in controller performance. Moreover, it helps in generalizing the control architecture to multiple vehicles, reducing the engineering work necessary for each vehicle. Path sampling is done considering the current velocity of the vehicle. However, this can aﬀect the performance of the lateral control. Another approach would be to

1Note that this comparison gives advantage to the SA-MPC since it was run on dry road conditions, while the results with the RA-MPC were obtained in wet road conditions. The improvement should be even bigger for the same conditions. 158 consider the expected future velocity, for example, assuming that the longitudinal proﬁle of the trajectory is followed perfectly. These approaches should be studied and compared to conclude which one is best.

Chapter 4: LTVR-MPC

As discussed in Section 4.4, before applying the robust controller, it is important to study and understand how to decrease the linearization and disturbance errors. The state-space of the controller influences the linearization errors, so the necessary space for the real system should be studied. Furthermore, the disturbances that the real system is under, also need to be studied. In Chapter 5, by modifying the vehicle prediction model to better account for the nonlinear curvature response of the vehicle, the ”disturbance” to the control request was heavily reduced and the control performance improved significantly. So, by a relatively simple model modification the prediction model improved significantly in respect to the real vehicle. Note that this is very important in this robust framework since the disturbance sets for a real system, need also to consider the difference between the prediction model and the real system. In relation to real systems, it would also be interesting to consider that the current system state estimation is not fully known and can be performed online. To improve the robust controller there are some obvious extensions that follow the discussion in Section 4.4. A feedback controller can be considered to generate a new linearization trajectory starting from the current pose of the system. The linearization around this trajectory would automatically decrease the disturbance set, because the linearization error from the current point would disappear. Fur- thermore, the parameter variation rate could also be considered for the disturbance set and for the disturbance invariant set. This would minimize the influence of the disturbance effects of the LTV systems on each other. That, in turn, will most likely lead to a more useful controller, since it would be able to handle bigger disturbances and maintain safety. It would also be interesting to understand what is the overall complexity of the offline setup of the problem, which would, hopefully, help determine the maximum dimension for which this approach is applicable. It would, as well, be interesting to study the optimality and convergence properties of Algorithm 1 used to compute the terminal cost for the MPC. Lastly, the approach should be compared to other similar approaches and the benefits and/or downsides should be highlighted.

Chapter 5: RA-MPC

The testing scenario considered and results presented were limited by time and resources. Future work, includes testing the controller in different environments and vehicles. Implementing the controller in different vehicles should be easy, since the control architecture considered is very general, however the proposed model is vehicle specific. Although this means the proposed model needs to be designed for 6.2. FUTURE WORK 159 each vehicle, it can be done from readily available data (without needing specific designed scenarios) making it easier to implement in different vehicles than more complex dynamic vehicle models. Furthermore, HDVs also run in heavily disturbed scenarios, such as mining environments, where the road is made of dirt, rocks, holes, etc.. The controller and proposed model need to be evaluated under these environment conditions, to understand if it is good enough, or if further modifications should be considered. Finally, a better and more conclusive analysis of the contributions should be done using statistical analysis, considering and comparing results from multiple experiments. In this work, the RA-MPC is assumed stable because it has a long enough prediction horizon, and because it is tuned carefully. This should be addressed and controller stability should be guaranteed by, e.g., including terminal cost and terminal set constraint, making use of the theory presented in Chapter 4. The proposed controller and model do not account for disturbances and noises. These are assumed small enough to be handled by the feedback loop. However, this might not be always the case, so it is important to study how to account for disturbances and what are the limitations of the system. Furthermore, the current controller does not provide offset-free path tracking, this should also be taken into consideration when developing a new model. Finally, although the new proposed curvature response model has improved the controller performance significantly, this model has the disadvantage of being vehicle specific. Other models should be studied in order to achieve a more general controller that is easily applied to different vehicles. On the other hand, if a vehicle specific model is to be considered, then more complex dynamic vehicle models should also be studied.

Other Directions

Currently the MPCs are tuned manually, which requires a significant amount of time and might not generalize very well for all vehicles. With this in mind, the development of a systematic procedure to tune the controller would be extremely useful. The tuning of the controller could be done offline by considering well designed experiments and their results. Or it could be done online, by considering machine learning and system identification approaches, which would learn and adapt the controller gains and parameters depending on the current conditions and vehicle. After implementing the nominal MPC and studying the real system better, a much clearer understanding of the model errors, disturbances, and other nonlinearities was obtained. It would be interesting to, once again, study how to ”robustify” the system. Not only new robust controllers, but also stochastic controllers, have seen many advances in recent years. These methods would handle changes in operating conditions better, as well as other disturbances. And that is important to do to provide more guarantees of safety for the system. In the controllers proposed it is assumed that if the vehicle tracks the path well (maintains certain error bounds) the vehicle will not collide with anything. This assumption is reasonable, assuming the planning module is accounting for the 160 obstacles and is providing an obstacle-free tube to the controller. However, for some scenarios with fast changing dynamic obstacles, it can be interesting to explicitly account for the obstacles. This would allow the controller to override the planned trajectory, altering its speed and positions, providing an extra layer of security and safety. This thesis is focused on lateral control and the assumption made throughout is that the longitudinal controller is good enough for the applications. However, in certain scenarios, the current longitudinal controller that tracks the desired velocity profile of the path might not be enough. Consider a vehicle operating on sand, if the vehicle slows down too much it will sink, rendering the system useless until the vehicle is pulled out of the sand. So, the longitudinal controller also needs to be robustified against disturbances and environment conditions. Furthermore, trajectory tracking requires to be at a pose at a specific time, this is highly important when accounting for moving obstacles. So, the development of a longitudinal controller to operate together with the proposed lateral controllers would be an interesting direction. Moreover, the development of a coupled longitudinal and lateral controller could also be considered. The controller architecture considered is general in order to operate well with multiple planning algorithms and methods. However, the necessity to account better for the references in the controller due to model mismatch was clear. It would be interesting to study how much there is to gain from using a planner that considers the same model and discretization as the controller vs other approaches. And also to understand how much the control performance could be improved by having a planner and controller that consider the same information and predictions. Finally, throughout the thesis an assumption was made that the vehicle controllers are fixed and cannot be modified. However, the performance of the high- level controllers are always capped by the performance of the vehicle controllers. Meaning that if the vehicle controllers do not track the steering and velocity requests well, neither will the high-level controllers perform well. The vehicle system should be studied to understand what are the current limitations and maximum performance achievable. Then, if necessary, new vehicle controllers should be considered. Appendices

161

Appendix A

MPC Feasibility and Stability

This appendix restates Theorems 2.4-2.5 and derives the proofs for them. Theorem 2.4 (MPC feasibility) Problem (2.20) is feasible ∀t > 0 if Qf = LQR LQR P∞, Zf = O∞ , and if z0 ∈ KN O∞ . LQR Proof: If z0 ∈ KN O∞ , then the system is feasible at time t = 0, i.e., there LQR exists a sequence of N control inputs that brings the system to the set O∞ , while LQR respecting the system constraints. By definition of O∞ , the system constraints LQR LQR are satisfied for all points in O∞ , and O∞ is invariant under the control law ut = F∞zt. Thus, the system is feasible for all t > 0. Theorem 2.5 (MPC stability) Consider system (2.18), constrained by (2.19), under closed-loop RHC with the linear MPC (2.20) as defined above with all the as- LQR sumptions for A, B, Q, R, Qf = P∞, and Zf = O∞ . Then, the origin is asymp- LQR totically stable for the closed-loop system with region of attraction KN O∞ . LQR Proof: Consider problem (2.20) at time t. Let zt ∈ KN O∞ and let ? ? ut be the optimizer of problem (2.20) and zt be the corresponding optimal state ? trajectory. After the implementation of ut = u0|t system (2.18) evolves to zt+1 = ? ? 1 ? ? Azt + Bu0|t = z1|t . Finally, let J (zt, ut ) be the optimal total predicted cost for ? cost function J when applying ut to system with state zt. Consider now problem (2.20) at time t+1. Consider the upper bound J(zt+1, ut+1) ? ? on the optimal predicted cost J (zt+1, ut+1), by considering the sequence

? ? ? ut+1 = {u1|t, ..., uN−1|t,F∞zN|t}, and the corresponding state trajectory from the initial state zt+1,

? ? ? zt+1 = {z1|t, ..., zN|t, (A + BF∞) zN|t},

1Assuming there is no model mismatch between the prediction model and the plant model.

163 164

where F∞ is the LQR feedback gain (2.17). Thus, the constructed cost at time t+1 is,

? ? T ? T ? J(zt+1,ut+1) = J (zt, ut ) − z0|tQz0|t + u0|t Ru0|t (A.1) ? T T T ? + zN|t (A + BF∞) P∞(A + BF∞) + Q + F∞RF∞ − P∞ zN|t.

T Consider the simpliﬁcations, using the distributive property and P∞ = P∞,

T T (A + BF∞) P∞(A + BF∞) + Q + F∞RF∞ T T T T T = A P∞A + 2A P∞BF∞ + F∞B P∞BF∞ + Q + F∞RF∞ T T T T = A P∞A + Q + 2A P∞BF∞ + F∞ B P∞B + R F∞

T −1 T and, substituting the ﬁrst F∞ with (2.17), i.e., F∞ = −(B P∞B + R) B P∞A,

T T F∞ B P∞B + R F∞ T T −1 T = −A P∞B(B P∞B + R) B P∞B + R F∞ T = −A P∞BF∞ so,

T T (A + BF∞) P∞(A + BF∞) + Q + F∞RF∞ T T = A P∞A + Q + A P∞BF∞ = P∞ according to DARE (2.16).

Then the cost J(zt+1, ut+1) (A.1) can be simpliﬁed to

? ? T ? T ? J(zt+1, ut+1) = J (zt, ut ) − z0|tQz0|t + u0|t Ru0|t .

? ? By deﬁnition J (zt+1, ut+1) 6 J(zt+1, ut+1), so

? ? ? ? T ? T ? J (zt+1, ut+1) − J (zt, ut ) 6 − z0|tQz0|t + u0|t Ru0|t , where the right-hand side is clearly negative definite due to the positive definiteness of Q and R. Furthermore, the cost function J is zero at the origin and greater than LQR zero for all other points in KN O∞ . All the conditions for Theorem 2.2 are fulfilled and the origin is asymptotically stable for the closed-loop system. Appendix B

QP Formulation

This appendix clariﬁes how to formulate problem (2.20) as a quadratic program that matches the formulation (2.12). This is of interest since it allows to solve the optimization problem online eﬃciently with readily available solvers. In problem (2.20) the optimization variable is denoted as ut. Thus, start by writing system (2.18) equations for an arbitrary time t + 2,

zt+2 = Azt+1 + But+1

= A (Azt + But) + But+1

= AAzt + ABut + But+1, this recursion can be written for any time, and leads to the compact notation,

zt = Azt + But (B.1)

T N.n T N.m where zt = [zt+1, ..., zt+N ] ∈ R , ut = [ut, ..., ut+N−1] ∈ R ,

 A   B 0 ... 0   A2   AB B . . . 0    N.n×n   N.n×N.m A =  .  ∈ R and B =  . . .  ∈ R .  .   ......  AN AN−1BAN−2B...B N.n×N.n Moreover, consider Q = diag([Q, Q, ..., Q, Qf ]) ∈ R and R = diag([R, R, ..., R]) ∈ RN.m×N.m, squared matrices where the diagonal is composed of the vector entries, i.e.,

Q 0 ... 0 0  R 0 ... 0   0 Q... 0 0     0 R... 0   . . . .    Q =  ......  and R =  . . .  .    ......   0 Q...Q 0  0 0 ...R 0 0 ... 0 Qf

165 166

Then the cost function J(zt, ut) of problem (2.20), can be written as, T T T J(zt, ut) = zt Qzt + zt Qzt + ut Rut T T T = (Azt + But) Q (Azt + But) + zt Qzt + ut Rut T T T T T T = ut B QB + R ut + 2zt A QBut + zt A QA + Q zt. So the cost function in (2.12) is written as T H = 2 B QB + R ,

T T T h = 2zt A QB, T T and the constant term zt A QA + Q zt is disregarded, since it does not inﬂuence the optimal solution. Note that for the optimal MPC cost this term cannot be disregarded. To setup the constraints in QP form ﬁrst remember that the constraint sets n are convex polytopes so, U = {u ∈ R : GU u 6 gU } with mU inequalities, Z = n n {z ∈ R : GZ u 6 gZ } with mZ inequalities, and Zf = z ∈ R : GZf z 6 gZf with mZf inequalities. mU .N×m.N Consider GU = diag ([GU , ..., GU ]) ∈ R , i.e., matrix GU repeated N T mU .N times on a diagonal matrix, and consider the vector gU = [gU , ..., gU ] ∈ R . Then the control input constraints can be written as GU ut 6 gU . 2 2 3 T Consider the notation A0 = [A, A ,A ] which represents lines 1 to 3 of matrix A for the compact system (B.1), and the same is valid for B, and zt. mZ .(N−1)×n.(N−1) Consider GZ = diag ([GZ , ..., GZ ]) ∈ R and vector T mZ .N−1 gZ = [gZ , ..., gZ ] ∈ R . Then the state constraints can be written as N−2 GZ zt0 6 gZ , this is equivalent to writing N−2 N−2 GZ A0 zt + B0 ut 6 gZ , or, N−2 N−2 GZ B0 ut 6 gZ − GZ A0 zt.

Finally, the terminal set constraint can be written as, GZf zN|t 6 gZf , which is the same as, N−1 N−1 GZf AN−1zt + BN−1ut 6 gZf , or, N−1 N−1 GZf BN−1ut 6 gZf − GZf AN−1zt. Then the constraints Gut 6 g in (2.12) are written as,     GU gU N−2 N−2  GZ B0  ut 6  gZ − GZ A0 zt  , N−1 N−1 GZf BN−1 gZf − GZf AN−1zt and the equality constraints are not used and set to zero. Note that the system model is fully incorporated in the state and input constraints, as well as in the cost function. Bibliography

[1] KTH - Royal Institute of Techonlogy, 2019. Site: https://www.kth.se/.

[2] Scania CV AB, 2019. Site: https://www.scania.com/.

[3] WASP - Wallenberg Artiﬁcial Intelligence, Autonomous Systems, and Soft- ware Program, 2019. Site: https://wasp-sweden.org/.

[4] European Comission. Vehicle categories, 2016. Available at https://ec.europa.eu/growth/sectors/automotive/vehicle-categories en.

[5] European Comission. Questions and answers on the commission strategy for reducing heavy-duty vehicles’ (HDVs) fuel consumption and CO2 emissions, 2014. Available at https://europa.eu/rapid/press-release MEMO-14-366 en.htm.

[6] Sarah Petit. World vehicle population rose 4.6% in 2016. Technical report, Wards Intelligence, 2018. Available at https://wardsintelligence.informa .com/WI058630/World-Vehicle-Population-Rose-46-in-2016.

[7] Statista Research Department. How many registered motor vehicles are there in the U.S.?, 2019. Available at https://www.statista.com/statistics/ 183505/number-of-vehicles-in-the-united-states-since-1990/.

[8] Sarah Zheng. China now has over 300 million vehicles... that’s almost america’s total population, 2019. Available at https://www.scmp.com/news/china/economy/article/2088876/chinas-more- 300-million-vehicles-drive-pollution-congestion.

[9] European Comission. EU transport in ﬁgures - statistical pocketbook 2018. Technical report, 2018. Available at https://ec.europa.eu/transport/sites/transport/ﬁles/pocketbook2018.pdf.

[10] European Comission. EU transport in ﬁgures - statistical pocketbook 2017. Technical report, 2017. Available at https://ec.europa.eu/transport/sites/transport/ﬁles/pocketbook2017.pdf.

167 168 BIBLIOGRAPHY

[11] European Comission. EU transport in figures - statistical pocketbook 2016. Technical report, 2016. Available at https://ec.europa.eu/transport/sites/transport/files/pocketbook2016.pdf. [12] Center for Solar Energy and Hydrogen Research Baden-Württemberg (ZSW). Global e-car count up from 3.4 to 5.6 million. Technical report, 2018. Available at https://www.zsw-bw.de/en/newsroom/news/news-detail/news/ detail/News/global-e-car-count-up-from-34-to-56-million.html. [13] European Environment Agency. Electric vehicles from life cycle and circular economy perspectives. Technical report, 2018. Available at https://www.eea.europa.eu/publications/electric-vehicles-from-life-cycle. [14] A. Brown, B. Repac, and J. Gonder. Autonomous vehicles have a wide range of possible energy impacts (poster). 2013. [15] Carolyn Beeler. Driverless cars could either be ’scary’ or great for the environment, 2017. Available at https://www.pri.org/stories/2017-04-18/driverless- cars-could-either-be-scary-or-great-environment. [16] Justin Worland. Self-driving cars could help save the environment—or ruin it. it depends on us, 2016. Available at https://time.com/4476614/self-driving-cars-environment/. [17] Billy Shea. Are self driving cars good for the environment?, 2019. Available at https://goodmenproject.com/featured-content/self-driving-cars- environment-jrrd/. [18] Brian S. Fisher and Sabine Schnittger. Autonomous and remote operation technologies in the mining industry: Benefits and costs. Technical report, BAEconomics, 2012. Available at http://www.baeconomics.com.au/wp- content/uploads/2010/01/Mining-innovation-5Feb12.pdf. [19] Sebastian van de Hoef. Coordination of Heavy-Duty Vehicle Platooning. PhD thesis, KTH, Division of Decision and Control Systems, 2018. [20] Jan Birdsey, William Sieber, Guang X Chen, Edward Hitchcock, Jennifer E Lincoln, Akinori Nakata, Cynthia Robinson, and Marie H Sweeney. Na- tional survey of us long-haul truck driver health and injury health behaviors. Journal of occupational and environmental medicine / American College of Occupational and Environmental Medicine, 57:210–6, 02 2015. [21] World Health Organization. Obesity and overweight, 2018. Available at https://www.who.int/news-room/fact-sheets/detail/obesity-and-overweight. [22] Centers for Disease Control and Prevention. Health effects of cigarette smoking, 2018. Available at https://www.cdc.gov/tobacco/data statistics/ fact sheets/health effects/effects cig smoking/. BIBLIOGRAPHY 169

[23] Ann Williamson, R Friswell, S Sadural, et al. Driver fatigue: a survey of long distance heavy vehicle drivers in australia. ACT: Australian Transport Safety Bureau (ATSB), 2001.

[24] Sergio Garbarino, Ottavia Guglielmi, Walter Sannita, Nicola Magnavita, and Paola Lanteri. Sleep and mental health in truck drivers: Descriptive review of the current evidence and proposal of strategies for primary prevention. International Journal of Environmental Research and Public Health, 15:1852, 08 2018.

[25] K S Cho and S H Lee. Occupational health hazards of mine workers. Bulletin of the World Health Organization, 56:205–18, 1978.

[26] The National Institute for Occupational Safety and Health (NIOSH). Mining topic: Respiratory diseases, 2018. Available at https://www.cdc.gov/niosh/mining/topics/RespiratoryDiseases.html.

[27] Ernst and Young. Who’s in the driving seat? how the rise of autonomous vehicles will transform the relationship between man and car. Technical report, 2015. Available at https://www.ey.com/Publication/vwLUAssets/EY-whos- in-the-driving-seat/$FILE/EY-whos-in-the-driving-seat.pdf.

[28] World Health Organization. Global status report on road safety 2018. Tech- nical report, 2018. Available at https://www.who.int/ violence injury prevention/road safety status/2018/en/.

[29] World Health Organization. Road traﬃc injuries, 2018. Available at https://www.who.int/news-room/fact-sheets/detail/road-traﬃc-injuries.

[30] National Highway Traﬃc Safety Administration. Critical reasons for crashes investigated in the national motor vehicle crash causation survey. Technical report, U.S. Department of Transportation, March 2018. Available at https://crashstats.nhtsa.dot.gov/Api/Public/ViewPublication/812506.

[31] Cadie Thompson. The 3 biggest ways self-driving cars will improve our lives, 2016. Available at https://www.businessinsider.com/advantages-of-driverless-cars-2016-6.

[32] Nidhi Kalra and David G. Groves. The enemy of good: Estimating the cost of waiting for nearly perfect automated vehicles. Technical report, RAND Corporation, 2017. Available at https://www.rand.org/pubs/research reports/RR2150.html.

[33] Ashley Nunes and Kristen Hernandez. The cost of self-driving cars will be the biggest barrier to their adoption. Harvard Business Review, 2019. Available at https://hbr.org/2019/01/the-cost-of-self-driving-cars-will- be-the-biggest-barrier-to-their-adoption. 170 BIBLIOGRAPHY

[34] National Highway Traﬃc Safety Administration. Automated driving systems 2.0: A vision for safety, 2017. Available at https://www.nhtsa.gov/sites/ nhtsa.dot.gov/ﬁles/documents/13069a-ads2.0 090617 v9a tag.pdf.

[35] Viktor Berggren, Aneta Vulgarakis, Athanasios Karapantelakis, Elena Fers- man, Keven Wang, Leonid Mokrushin, Nicolas Schrammar, and Raﬁa Inam. 5G teleoperated vehicles for future public transport, 2017. Avail- able at https://www.ericsson.com/en/blog/2017/6/5g-teleoperated-vehicles- for-future-public-transport.

[36] Jamie Carter. Hacked driverless cars could cause collisions and gridlock in cities, say researchers, 2019. Available at https://www.forbes.com/sites/ jamiecartereurope/2019/03/05/hacked-driverless-cars-could-cause-collisions- and-gridlock-in-cities-say-researchers/#1ef50d362a09.

[37] Department of Economic and Social Aﬀairs, Population Division (2019). World population prospects 2019: Data booklet. Technical report, United Nations, 2017. Available at https://population.un.org/wpp/ Publications/Files/WPP2019 DataBooklet.pdf.

[38] Federal Highway Administration. Traﬃc volume trends. Technical report, U. S. Department of Transportation, June 2019. Available at https://www.fhwa. dot.gov/policyinformation/travel monitoring/19juntvt/19juntvt.pdf.

[39] Thomas Black. U.S. truck driver shortage is on course to double in a decade, 2019. Available at https://www.bloomberg.com/news/articles/2019-07-24/u- s-truck-driver-shortage-is-on-course-to-double-in-a-decade.

[40] International Road Transport Union. Driver shortage problem, 2019. Avail- able at https://www.iru.org/what-we-do/network/driver-portal/problem.

[41] Alan Mckinnon, Christoph Fl¨othmann,Kai Hoberg, and Christina Busch. Logistics competencies, skills, and training: A global overview, 2017. Avail- able at https://openknowledge.worldbank.org/bitstream/handle/10986/ 27723/9781464811401.pdf?sequence=2&isAllowed=y.

[42] Bob Costello and Alan Karickhoﬀ. Truck driver shortage analysis 2019, 2019. Available at https://www.trucking.org/ATA%20Docs/News%20and %20Information/Reports%20Trends%20and%20Statistics/ATAs%20Driver %20Shortage%20Report%202019%20with%20cover.pdf.

[43] J´ozefDubi´nski.Sustainable development of mining mineral resources. Journal of Sustainable Mining, 12(1):1 – 6, 2013.

[44] James Manyika, Michael Chui, Jacques Bughin, Richard Dobbs, Peter Bis- son, and Alex Marrs. Disruptive technologies: Advances that will transform life, business, and the global economy. Technical report, McKinsey Global BIBLIOGRAPHY 171

Institute, McKinsey & Company, 2013. Available at https://www.mckinsey.com/˜/media/McKinsey/Business%20Functions/ McKinsey%20Digital/Our%20Insights/Disruptive%20technologies/ MGI Disruptive technologies Full report May2013.ashx. [45] Lewis M. Clements and Kara M. Kockelman. Economic effects of automated vehicles. Transportation Research Record, 2606(1):106–114, 2017. [46] Alan Hooper and Dan Murray. An analysis of the operational costs of trucking: 2018 update, 2018. Available at https://atri-online.org/wp- content/uploads/2018/10/ATRI-Operational-Costs-of-Trucking-2018.pdf. [47] SAE International. Taxonomy and Definitions for Terms Related to Driving Automation Systems for On-Road Motor Vehicles, jun 2018. [48] Tesla. Autopilot, 2019. Available at https://www.tesla.com/support/autopilot. [49] Aisha Chottani, Greg Hastings, John Murnane, and Florian Neuhaus. Distraction or disruption? autonomous trucks gain ground in us logistics, 2018. Available at https://www.mckinsey.com/industries/travel-transport- and-logistics/our-insights/distraction-or-disruption-autonomous-trucks-gain- ground-in-us-logistics. [50] A. Alam, B. Besselink, V. Turri, J. Martensson, and K. H. Johansson. Heavy- duty vehicle platooning for sustainable freight transportation: A cooperative method to enhance safety and efficiency. IEEE Control Systems Magazine, 35(6):34–56, Dec 2015. [51] Jeff Peters. The future of autonomous vehicles runs off roads and on to farms, construction sites and mines, 2019. Available at https://techcrunch.com/2019/07/10/autonomous-vehicle-startups-are-dead- long-live-autonomous-vehicle-startups/. [52] Kennecott Utah Copper. Youtube video: Rio tinto kennecott mine access road construction time lapse, 2013. Available at https://www.youtube.com/watch?v=wu5c5TCEoVM. [53] Scania CV AB. Scania and rio tinto trialling autonomous truck in western australian mine, 2018. Available at https://www.scania.com/group/en/scania-and-rio-tinto-trialling- autonomous-truck-in-western-australian-mine/. [54] Nic White. The world’s biggest automated port terminal opens in shanghai as it gears up to handle 6.3 million shipping containers weighing up to 136 million tonnes a year, 2018. Available at https://www.dailymail.co.uk/news/article-5604199/The-worlds-biggest- automated-port-terminal-opens-Shanghai.html. 172 BIBLIOGRAPHY

[55] Aditya Ambadipudi, Kersten Heineke, Philipp Kampshoff, and Emily Shao. Gauging the disruptive power of robo-taxis in autonomous driving. Technical report, McKinsey Center for Future Mobility, McKinsey & Company, 2017. Available at https://www.mckinsey.com/˜/media/McKinsey/Industries/ Automotive%20and%20Assembly/Our%20Insights/Gauging%20the%20 disruptive%20power%20of%20robo%20taxis%20in%20autonomous%20 driving/Gauging-the-disruptive-power-of-robo-taxis-in-autonomous- driving.ashx. [56] National League of Cities. How will autonomous vehicles revolutionize cities?, 2018. Available at https://www.nlc.org/article/how-will-autonomous- vehicles-revolutionize-cities. [57] National Express Transit. How autonomous vehicles are driving the adoption of mobility as a service (MaaS), 2019. Available at https://www.nationalexpresstransit.com/blog/autonomous-vehicles- mobility-as-a-service-maas/. [58] KPMG and CAR - Center for Automotive Research. Self-driving cars: The next revolution. Technical report, 2012. Available at https://institutes.kpmg.us/manufacturing-institute/articles/2017/self- driving-cars-the-next-revolution.html. [59] Aneesh Paul, Rohan Chauhan, Rituraj Srivastava, and Mriganka Baruah. Advanced driver assistance systems, 2016. [60] Kersten Heineke, Philipp Kampshoff, Armen Mkrtchyan, and Emily Shao. Self-driving car technology: When will the robots hit the road?, 2017. Available at https://www.mckinsey.com/industries/automotive-and-assembly/our- insights/self-driving-car-technology-when-will-the-robots-hit-the-road#. [61] J. Kocić,N. Joviˇcić,and V. Drndarević. Sensors and sensor fusion in autonomous vehicles. In 2018 26th Telecommunications Forum (TELFOR), pages 420–425, Nov 2018. [62] Nick Oliver, Kristina Potoˇcnik,and Thomas Calvard. To make self-driving cars safe, we also need better roads and infrastructure. Harvard Business Re- view, 2018. Available at https://hbr.org/2018/08/to-make-self-driving-cars- safe-we-also-need-better-roads-and-infrastructure. [63] Wilko Schwarting, Javier Alonso-Mora, and Daniela Rus. Planning and decision-making for autonomous vehicles. Annual Review of Control, Robotics, and Autonomous Systems, 1(1):187–210, 2018. [64] Philip Koopman and Michael Wagner. Challenges in autonomous vehicle testing and validation. SAE International Journal of Transportation Safety, 4:15–24, 04 2016. BIBLIOGRAPHY 173

[65] MIT Technology Review Insights. Self-driving cars take the wheel. MIT Technology Review, 2019. Available at https://www.technologyreview.com/s/612754/self-driving-cars-take-the- wheel/.

[66] Paul Lienert. Self-driving costs could drop 90 percent by 2025, Delphi CEO says, 2017. Available at https://www.reuters.com/article/us-autos-delphi/self-driving-costs-could- drop-90-percent-by-2025-delphi-ceo-says-idUSKBN1DY2AC.

[67] Steve LeVine. What it really costs to turn a car into a self-driving vehicle, 2017. Available at https://qz.com/924212/what-it-really-costs-to-turn-a-car- into-a-self-driving-vehicle/.

[68] Tom Randall. Waymo starts selling sensors to lower cost of self-driving cars, 2019. Available at https://www.bloomberg.com/news/articles/2019-03-06/waymo-starts- selling-sensors-to-lower-cost-of-self-driving-cars.

[69] Waymo. Laser bear honeycomb, 2019. Site: https://waymo.com/lidar.

[70] Lance Eliot. Traﬃc lights and ai autonomous cars, 2019. Available at https://www.aitrends.com/ai-insider/traﬃc-lights-and-ai-autonomous-cars/.

[71] Roberto Baldwin. Honda’s smart intersection tells drivers what’s around the corner, 2018. Available at https://www.engadget.com/2018/10/06/honda-smart-intersection-ohio/.

[72] Victor Tangermann. This is how intelligent smart roads could become, 2018. Available at https://www.engadget.com/2018/10/06/honda-smart-intersection-ohio/.

[73] Thorsten Lohmar, Ali Zaidi, Høakan Olofsson, and Christer Boberg. Trans- forming transportation with 5G. Ericsson Technology Review, 2019. Available at https://www.ericsson.com/4a61e3/assets/local/publications/ericsson- technology-review/docs/2019/etr-transforming-transportation-with-5g.pdf.

[74] M. Lauer. Grand cooperative driving challenge 2011 [its events]. IEEE Intel- ligent Transportation Systems Magazine, 3(3):38–40, Fall 2011.

[75] J. Ploeg, C. Englund, H. Nijmeijer, E. Semsar-Kazerooni, S. E. Shladover, A. Voronov, and N. van de Wouw. Guest editorial introduction to the special issue on the 2016 grand cooperative driving challenge. IEEE Transactions on Intelligent Transportation Systems, 19(4):1208–1212, April 2018.

[76] J. Ploeg, E. Semsar-Kazerooni, A. I. Morales Medina, J. F. C. M. de Jongh, J. van de Sluis, A. Voronov, C. Englund, R. J. Bril, H. Salunkhe, A.´ Arr´ue, 174 BIBLIOGRAPHY

A. Ruano, L. Garc´ıa-Sol,E. van Nunen, and N. van de Wouw. Cooperative automated maneuvering at the 2016 grand cooperative driving challenge. IEEE Transactions on Intelligent Transportation Systems, 19(4):1213–1226, April 2018.

[77] KPMG International. 2019 autonomous vehicles readiness index, 2019. Avail- able at https://home.kpmg/xx/en/home/insights/2019/02/2019- autonomous-vehicles-readiness-index.html.

[78] Aaron Smith and Monica Anderson. Automation in everyday life, 2017. Avail- able at https://www.pewinternet.org/2017/10/04/automation-in-everyday-life/.

[79] Charlie Hewitt, Theocharis Amanatidis, Advait Sarkar, and Ioannis Politis. Assessing public perception of self-driving cars: the autonomous vehicle acceptance model. 01 2019.

[80] Joanna Moody, Nathaniel Bailey, and Jinhua Zhao. Public perceptions of autonomous vehicle safety: An international comparison. Safety Science, 2019.

[81] Sam Levin and Julia Carrie Wong. Self-driving uber kills arizona woman in ﬁrst fatal crash involving pedestrian, 2018. Available at https://www.theguardian.com/technology/2018/mar/19/uber-self-driving- car-kills-woman-arizona-tempe.

[82] Jean-Fran¸coisBonnefon, Azim Shariﬀ, and Iyad Rahwan. The social dilemma of autonomous vehicles. Science, 352(6293):1573–1576, 2016.

[83] Tobias Holstein, Gordana Dodig-Crnkovic, and Patrizio Pelliccione. Ethical and social aspects of self-driving cars. ArXiv, abs/1802.04103, 2018.

[84] Finn Murphy. Truck drivers like me will soon be replaced by automation. you’re next., 2017. Available at https://www.theguardian.com/commentisfree/2017/nov/17/truck-drivers- automation-tesla-elon-musk.

[85] International Transport Forum. Managing the transition to driverless road freight transport. Technical report, 2017. Available at https://www.itf- oecd.org/managing-transition-driverless-road-freight-transport.

[86] Carmen Reinicke. Autonomous vehicles won’t only kill jobs. they will create them, too, 2018. Available at https://www.cnbc.com/2018/08/10/autonomous-vehicles-are-creating-jobs- heres-where.html. BIBLIOGRAPHY 175

[87] Timothy B. Lee. Self-driving cars will destroy a lot of jobs—they’ll also create a lot, 2018. Available at https://arstechnica.com/tech-policy/2018/08/self-driving-cars-will-destroy- a-lot-of-jobs-theyll-also-create-a-lot/.

[88] Aaron Cosbey, Howard Mann, Nicolas Maennling, Perrine Toledano, Jeﬀ Geipel, and Martin Dietrich Brauch. Mining a mirage? reassessing the shared- value paradigm in light of the technological advances in the mining sector. Technical report, International Institute for Sustainable Development, 2016. Available at https://www.iisd.org/sites/default/ﬁles/publications/mining-a-mirage.pdf.

[89] Futurism. Laws and ethics for autonomous cars: sharing the road with robots, 2017. Available at https://futurism.com/images/laws-and-ethics-for-autonomous-cars.

[90] Corinne Iozzio. Who’s responsible when a self-driving car crashes?, 2016. Available at https://www.scientiﬁcamerican.com/article/who-s-responsible- when-a-self-driving-car-crashes/.

[91] Autovista Group. The state of autonomous legislation in europe, 2019. Avail- able at https://autovistagroup.com/news-and-insights/state-autonomous- legislation-europe.

[92] European Transport Forum. Is Europe ready for self-driving vehicles?, 2019. Available at https://www.europeantransportforum.eu/mediaroom/is-europe- ready-for-self-driving-vehicles/.

[93] P. F. Lima, M. Nilsson, M. Trincavelli, J. M˚artensson, and B. Wahlberg. Spatial model predictive control for smooth and accurate steering of an autonomous truck. IEEE Transactions on Intelligent Vehicles, 2(4):238–250, Dec 2017.

[94] Pedro F. Lima. Optimization-Based Motion Planning and Model Predictive Control for Autonomous Driving: With Experimental Evaluation on a Heavy- Duty Construction Truck. PhD thesis, KTH, Division of Decision and Control Systems, 2018.

[95] Google. Maps, 2019. Site: https://www.google.com/maps/.

[96] Lisa Eadicicco. 14 incredibly useful google maps features everyone should know about, 2019. Available at https://www.businessinsider.com/google-maps-best-features-tips-tricks- 2019-5?r=US&IR=T#see-how-crowded-your-train-will-be-8.

[97] E. W. Dijkstra. A note on two problems in connexion with graphs. Nu- merische Mathematik, 1(1):269–271, Dec 1959. 176 BIBLIOGRAPHY

[98] Hannah Bast, Daniel Delling, Andrew Goldberg, Matthias Müller- Hannemann, Thomas Pajor, Peter Sanders, Dorothea Wagner, and Renato F. Werneck. Route planning in transportation networks. Lecture Notes in Com- puter Science, page 19–80, 2016. [99] Anirudh Kishore Bhoopalam, Niels Agatz, and Rob Zuidwijk. Planning of truck platoons: A literature review and directions for future research. Trans- portation Research Part B: Methodological, 107:212 – 228, 2018. [100] Rebecca Carroll. A ’brain’ for cars, tested for mars, 2018. Available at https://www.nasa.gov/directorates/spacetech/spinoff/feature/Brain for Cars Tested for Mars. [101] Adam Fisher. Google’s self-driving cars: A quest for acceptance, 2013. Avail- able at https://www.popsci.com/cars/article/2013-09/google-self-driving-car/. [102] Hans P. Moravec. Sensor fusion in certainty grids for mobile robots. AI Magazine, 9(2):61, Jun. 1988. [103] A. Elfes. Using occupancy grids for mobile robot perception and navigation. Computer, 22(6):46–57, June 1989. [104] A. Vatavu and S. Nedevschi. Real-time modeling of dynamic environments in traffic scenarios using a stereo-vision system. In 2012 15th International IEEE Conference on Intelligent Transportation Systems, pages 722–727, Sep. 2012. [105] G. Catalin and S. Nedevschi. Object tracking from stereo sequences using particle filter. In 2008 4th International Conference on Intelligent Computer Communication and Processing, pages 279–282, Aug 2008. [106] I. Miller and M. Campbell. Rao-blackwellized particle filtering for mapping dynamic environments. In Proceedings 2007 IEEE International Conference on Robotics and Automation, pages 3862–3869, April 2007. [107] Jessica Van Brummelen, Marie O’Brien, Dominique Gruyer, and Homayoun Najjaran. Autonomous vehicle perception: The technology of today and to- morrow. Transportation Research Part C: Emerging Technologies, 89:384 – 406, 2018. [108] Claudine Badue, Rânik Guidolini, Raphael Vivacqua Carneiro, Pedro Azevedo, Vinicius Brito Cardoso, Avelino Forechi, Luan Jesus, Rodrigo Berriel, Thiago Paixão,Filipe Mutz, Lucas Veronese, Thiago Oliveira-Santos, and Alberto Ferreira De Souza. Self-driving cars: A survey, 2019. [109] Galileo GNSS. The path to high gnss accuracy, 2018. Available at https://galileognss.eu/the-path-to-high-gnss-accuracy/. BIBLIOGRAPHY 177

[110] Oxford Technical Solutions. RT3000 v3 - Our highest performance INS for ADAS and autonomous vehicle testing, 2019. Available at https://www.oxts.com/products/rt3000/.

[111] S. Thrun, M. Montemerlo, H. Dahlkamp, D. Stavens, A. Aron, J. Diebel, P. Fong, J. Gale, M. Halpenny, G. Hoﬀmann, K. Lau, C. Oakley, M. Palatucci, V. Pratt, P. Stang, S. Strohband, C. Dupont, L. Jendrossek, C. Koelen, C. Markey, C. Rummel, J. van Niekerk, E. Jensen, P. Alessandrini, G. Brad- ski, B. Davies, S. Ettinger, A. Kaehler, A. Neﬁan, and P. Mahoney. Stanley: The robot that won the DARPA Grand Challenge. Journal of Field Robotics, 23(9):661–692, 2006.

[112] A. Broggi, P. Cerri, S. Debattisti, M. C. Laghi, P. Medici, D. Molinari, M. Panciroli, and A. Prioletti. Proud—public road urban driverless-car test. IEEE Transactions on Intelligent Transportation Systems, 16(6):3508–3519, Dec 2015.

[113] J. Ziegler, P. Bender, M. Schreiber, H. Lategahn, T. Strauss, C. Stiller, T. Dang, U. Franke, N. Appenrodt, C. G. Keller, E. Kaus, R. G. Herrtwich, C. Rabe, D. Pfeiﬀer, F. Lindner, F. Stein, F. Erbs, M. Enzweiler, C. Kn¨oppel, J. Hipp, M. Haueis, M. Trepte, C. Brenk, A. Tamke, M. Ghanaat, M. Braun, A. Joos, H. Fritz, H. Mock, M. Hein, and E. Zeeb. Making Bertha drive—an autonomous journey on a historic route. IEEE Intelligent Transportation Systems Magazine, 6(2):8–20, Summer 2014.

[114] E. Ward and J. Folkesson. Vehicle localization with low cost radar sensors. In 2016 IEEE Intelligent Vehicles Symposium (IV), pages 864–870, June 2016.

[115] N. Akai, L. Y. Morales, T. Yamaguchi, E. Takeuchi, Y. Yoshihara, H. Okuda, T. Suzuki, and Y. Ninomiya. Autonomous driving based on accurate localization using multilayer lidar and dead reckoning. In 2017 IEEE 20th Interna- tional Conference on Intelligent Transportation Systems (ITSC), pages 1–6, Oct 2017.

[116] S. Kuutti, S. Fallah, K. Katsaros, M. Dianati, F. Mccullough, and A. Mouza- kitis. A survey of the state-of-the-art localization techniques and their poten- tials for autonomous vehicle applications. IEEE Internet of Things Journal, 5(2):829–846, April 2018.

[117] E. Shelhamer, J. Long, and T. Darrell. Fully convolutional networks for semantic segmentation. IEEE Transactions on Pattern Analysis and Machine Intelligence, 39(4):640–651, April 2017.

[118] L. Deng, M. Yang, Y. Qian, C. Wang, and B. Wang. Cnn based semantic segmentation for urban traﬃc scenes using ﬁsheye camera. In 2017 IEEE Intelligent Vehicles Symposium (IV), pages 231–236, June 2017. 178 BIBLIOGRAPHY

[119] Joel Janai, Fatma G¨uney, Aseem Behl, and Andreas Geiger. Computer vision for autonomous vehicles: Problems, datasets and state-of-the-art. CoRR, abs/1704.05519, 2017.

[120] M. Darms, P. Rybski, and C. Urmson. Classiﬁcation and tracking of dynamic objects with multiple sensors for autonomous driving in urban environments. In 2008 IEEE Intelligent Vehicles Symposium, pages 1197–1202, June 2008.

[121] St´ephanieLef`evre,Dizan Vasquez, and Christian Laugier. A survey on motion prediction and risk assessment for intelligent vehicles. ROBOMECH Journal, 1(1):1–14, 2014.

[122] Dave Ferguson, Thomas M. Howard, and Maxim Likhachev. Motion planning in urban environments. Journal of Field Robotics, 25(11-12):939–960, 2008.

[123] S. Ammoun and F. Nashashibi. Real time trajectory prediction for collision risk estimation between vehicles. In 2009 IEEE 5th International Conference on Intelligent Computer Communication and Processing, pages 417–422, Aug 2009.

[124] S. Atev, G. Miller, and N. P. Papanikolopoulos. Clustering of vehicle trajectories. IEEE Transactions on Intelligent Transportation Systems, 11(3):647– 657, Sep. 2010.

[125] Weiming Hu, Xuejuan Xiao, Zhouyu Fu, D. Xie, Tieniu Tan, and S. Maybank. A system for learning statistical motion patterns. IEEE Transactions on Pattern Analysis and Machine Intelligence, 28(9):1450–1464, Sep. 2006.

[126] P. Kumar, M. Perrollaz, S. Lef`evre,and C. Laugier. Learning-based approach for online lane change intention prediction. In 2013 IEEE Intelligent Vehicles Symposium (IV), pages 797–802, June 2013.

[127] A. Lawitzky, D. Althoﬀ, C. F. Passenberg, G. Tanzmeister, D. Wollherr, and M. Buss. Interactive scene prediction for automotive applications. In 2013 IEEE Intelligent Vehicles Symposium (IV), pages 1028–1033, June 2013.

[128] G. Agamennoni, J. I. Nieto, and E. M. Nebot. A bayesian approach for driving behavior inference. In 2011 IEEE Intelligent Vehicles Symposium (IV), pages 595–600, June 2011.

[129] C. Urmson, J. Anhalt, D. Bagnell, C. Baker, R. Bittner, M. N. Clark, J. Dolan, D. Duggins, T. Galatali, C. Geyer, M. Gittleman, S. Harbaugh, M. Hebert, T. M. Howard, S. Kolski, A. Kelly, M. Likhachev, M. Mc- Naughton, N. Miller, K. Peterson, B. Pilnick, R. Rajkumar, P. Rybski, B. Salesky, Y. Seo, S. Singh, J. Snider, A. Stentz, W. Whittaker, Z. Wolkow- icki, J. Ziglar, H. Bae, T. Brown, D. Demitrish, B. Litkouhi, J. Nickolaou, V. Sadekar, W. Zhang, J. Struble, M. Taylor, M. Darms, and D. Ferguson. BIBLIOGRAPHY 179

Autonomous driving in urban environments: Boss and the Urban Challenge. Journal of Field Robotics, 25(8):425–466, 2008. [130] M. Montemerlo, J. Becker, S. Bhat, H. Dahlkamp, D. Dolgov, S. Ettinger, D. Haehnel, T. Hilden, G. Hoﬀmann, B. Huhnke, D. Johnston, S. Klumpp, D. Langer, A. Levandowski, J. Levinson, J. Marcil, D. Orenstein, J. Paefgen, I. Penny, A. Petrovskaya, M. Pﬂueger, G. Stanek, D. Stavens, A. Vogt, and S. Thrun. Junior: The Stanford entry in the Urban Challenge. Journal of Field Robotics, 25(9):569–597, 2008. [131] M. Aramrattana, J. Detournay, C. Englund, V. Frimodig, O. U. Jansson, T. Larsson, W. Mostowski, V. D. Rodr´ıguez, T. Rosenstatter, and G. Sha- hanoor. Team halmstad approach to cooperative driving in the grand cooperative driving challenge 2016. IEEE Transactions on Intelligent Transportation Systems, 19(4):1248–1261, April 2018. [132] S. Kokogias, L. Svensson, G. Collares Pereira, R. Oliveira, X. Zhang, X. Song, and J. M˚artensson.Development of platform-independent system for cooperative automated driving evaluated in gcdc 2016. IEEE Transactions on Intelligent Transportation Systems, 19(4):1277–1289, April 2018.

[133] Pete Trautman, Jeremy Ma, Richard M. Murray, and Andreas Krause. Robot navigation in dense human crowds: Statistical models and experimental stud- ies of human–robot cooperation. The International Journal of Robotics Re- search, 34(3):335–356, 2015.

[134] F. Havlak and M. Campbell. Discrete and continuous, probabilistic anticipa- tion for autonomous robots in urban environments. IEEE Transactions on Robotics, 30(2):461–474, April 2014. [135] S. Brechtel, T. Gindele, and R. Dillmann. Probabilistic decision-making under uncertainty for autonomous driving using continuous pomdps. In 17th In- ternational IEEE Conference on Intelligent Transportation Systems (ITSC), pages 392–399, Oct 2014. [136] W. Liu, S. Kim, S. Pendleton, and M. H. Ang. Situation-aware decision making for autonomous driving on urban road using online pomdp. In 2015 IEEE Intelligent Vehicles Symposium (IV), pages 1126–1133, June 2015.

[137] Enric Galceran, Alexander Cunningham, Ryan Eustice, and Edwin Ol- son. Multipolicy decision-making for autonomous driving via changepoint- based behavior prediction: Theory and experiment. Autonomous Robots, 41(6):1367–1382, 2017. [138] M. Althoff, D. Althoff, D. Wollherr, and M. Buss. Safety verification of autonomous vehicles for coordinated evasive maneuvers. In 2010 IEEE Intel- ligent Vehicles Symposium, pages 1078–1083, June 2010. 180 BIBLIOGRAPHY

[139] Shai Shalev-Shwartz, Shaked Shammah, and Amnon Shashua. On a formal model of safe and scalable self-driving cars. CoRR, abs/1708.06374, 2017.

[140] David Nistér,Hon-Leung Lee, Julia Ng, and Yizhou Wang. An introduction to the safety force field, 2019. Available at https://www.nvidia.com/content/dam/en-zz/Solutions/self-driving- cars/safety-force-field/the-safety-force-field.pdf.

[141] D. González,J. Pérez,V. Milanés,and F. Nashashibi. A review of motion planning techniques for automated vehicles. IEEE Transactions on Intelligent Transportation Systems, 17(4):1135–1145, April 2016.

[142] Mihail Pivtoraiko, Ross A. Knepper, and Alonzo Kelly. Diﬀerentially constrained mobile robot motion planning in state lattices. Journal of Field Robotics, 26(3):308–333, 2009.

[143] Jeong hwan Jeon, R. V. Cowlagi, S. C. Peters, S. Karaman, E. Frazzoli, P. Tsiotras, and K. Iagnemma. Optimal motion planning with the half-car dynamical model for autonomous high-speed driving. In 2013 American Con- trol Conference, pages 188–193, June 2013.

[144] D. González,J. Pérez,R. Lattarulo, V. Milanés,and F. Nashashibi. Con- tinuous curvature planning with obstacle avoidance capabilities in urban scenarios. In 17th International IEEE Conference on Intelligent Transportation Systems (ITSC), pages 1430–1435, Oct 2014.

[145] Alexander Heilmeier, Alexander Wischnewski, Leonhard Hermansdorfer, Jo- hannes Betz, Markus Lienkamp, and Boris Lohmann. Minimum curvature trajectory planning and control for an autonomous race car. Vehicle System Dynamics, 0(0):1–31, 2019.

[146] M. Cirillo. From videogames to autonomous trucks: A new algorithm for lattice-based motion planning. In 2017 IEEE Intelligent Vehicles Symposium (IV), pages 148–153, June 2017.

[147] J. Bohren, T. Foote, J. Keller, A. Kushleyev, D. Lee, A. Stewart, P. Vernaza, J. Derenick, J. Spletzer, and B. Satterﬁeld. Little Ben: The Ben Franklin Racing Team’s entry in the 2007 DARPA Urban Challenge. Journal of Field Robotics, 25(9):598–614, 2008.

[148] A. Bacha, C. Bauman, R. Faruque, M. Fleming, C. Terwelp, C. Reinholtz, D. Hong, A. Wicks, T. Alberi, D. Anderson, S. Cacciola, P. Currier, A. Dal- ton, J. Farmer, J. Hurdus, S. Kimmel, P. King, A. Taylor, D. Van Covern, and M. Webster. Odin: Team VictorTango’s entry in the DARPA Urban Challenge. Journal of Field Robotics, 25(8):467–492, 2008. BIBLIOGRAPHY 181

[149] P. E. Hart, N. J. Nilsson, and B. Raphael. A formal basis for the heuristic determination of minimum cost paths. IEEE Transactions on Systems Science and Cybernetics, 4(2):100–107, July 1968.

[150] J. Ziegler, M. Werling, and J. Schroder. Navigating car-like robots in unstructured environments using an obstacle sensitive cost function. In 2008 IEEE Intelligent Vehicles Symposium, pages 787–791, June 2008.

[151] O. Ljungqvist, N. Evestedt, M. Cirillo, D. Axehill, and O. Holmer. Lattice- based motion planning for a general 2-trailer system. In 2017 IEEE Intelligent Vehicles Symposium (IV), pages 819–824, June 2017.

[152] J. Ziegler and C. Stiller. Spatiotemporal state lattices for fast trajectory planning in dynamic on-road driving scenarios. In 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems, pages 1879–1884, Oct 2009.

[153] Christos Katrakazas, Mohammed Quddus, Wen-Hua Chen, and Lipika Deka. Real-time motion planning methods for autonomous on-road driving: State- of-the-art and future research directions. Transportation Research Part C: Emerging Technologies, 60:416 – 442, 2015.

[154] Steven M. LaValle. Rapidly-exploring random trees: A new tool for path planning. Technical report, Iowa State University.

[155] S. Karaman, M. R. Walter, A. Perez, E. Frazzoli, and S. Teller. Anytime motion planning using the RRT?. In 2011 IEEE International Conference on Robotics and Automation, pages 1478–1483, May 2011.

[156] Sertac Karaman and Emilio Frazzoli. Sampling-based algorithms for optimal motion planning. The International Journal of Robotics Research, 30(7):846– 894, 2011.

[157] Y. Kuwata, J. Teo, G. Fiore, S. Karaman, E. Frazzoli, and J. P. How. Real- time motion planning with applications to autonomous urban driving. IEEE Transactions on Control Systems Technology, 17(5):1105–1118, Sep. 2009.

[158] N. Evestedt, D. Axehill, M. Trincavelli, and F. Gustafsson. Sampling recovery for closed loop rapidly expanding random tree using brake proﬁle regenera- tion. In 2015 IEEE Intelligent Vehicles Symposium (IV), pages 101–106, June 2015.

[159] Larissa Labakhua, Urbano Nunes, Rui Rodrigues, and F´atima S. Leite. Smooth Trajectory Planning for Fully Automated Passengers Vehicles: Spline and Clothoid Based Methods and Its Simulation, pages 169–182. Springer Berlin Heidelberg, Berlin, Heidelberg, 2008. 182 BIBLIOGRAPHY

[160] L. E. Dubins. On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents. American Journal of Mathematics, 79(3):497–516, 1957.

[161] J. A. Reeds and L. A. Shepp. Optimal paths for a car that goes both forwards and backwards. Paciﬁc J. Math., 145(2):367–393, 1990.

[162] T. Fraichard and A. Scheuer. From reeds and shepp’s to continuous-curvature paths. IEEE Transactions on Robotics, 20(6):1025–1035, Dec 2004.

[163] R. Oliveira, P. F. Lima, M. Cirillo, J. M˚artensson, and B. Wahlberg. Tra- jectory generation using sharpness continuous dubins-like paths with applications in control of heavy-duty vehicles. In 2018 European Control Conference (ECC), pages 935–940, June 2018.

[164] Joseph Funke and J. Christian Gerdes. Simple Clothoid Lane Change Tra- jectories for Automated Vehicles Incorporating Friction Constraints. Journal of Dynamic Systems, Measurement, and Control, 138(2), 12 2015. 021002.

[165] M. Brezak and I. Petrovi´c. Real-time approximation of clothoids with bounded error for path planning applications. IEEE Transactions on Robotics, 30(2):507–515, April 2014.

[166] P. Petrov and F. Nashashibi. Modeling and nonlinear adaptive control for autonomous vehicle overtaking. IEEE Transactions on Intelligent Transporta- tion Systems, 15(4):1643–1656, Aug 2014.

[167] Yanfeng Cong, O. Sawodny, Hong Chen, J. Zimmermann, and A. Lutz. Mo- tion planning for an autonomous vehicle driving on motorways by using ﬂat- ness properties. In 2010 IEEE International Conference on Control Applica- tions, pages 908–913, Sep. 2010.

[168] S. Glaser, B. Vanholme, S. Mammar, D. Gruyer, and L. Nouveli`ere. Maneuver-based trajectory planning for highly autonomous vehicles on real road with traﬃc and driver interaction. IEEE Transactions on Intelligent Transportation Systems, 11(3):589–606, Sep. 2010.

[169] L. Han, H. Yashiro, H. Tehrani Nik Nejad, Q. H. Do, and S. Mita. B´ezier curve based path planning for autonomous vehicle in urban environment. In 2010 IEEE Intelligent Vehicles Symposium, pages 1036–1042, June 2010.

[170] T. Berglund, A. Brodnik, H. Jonsson, M. Staﬀanson, and I. Soderkvist. Plan- ning smooth and obstacle-avoiding b-spline paths for autonomous mining vehicles. IEEE Transactions on Automation Science and Engineering, 7(1):167– 172, Jan 2010. BIBLIOGRAPHY 183

[171] J. Ziegler, P. Bender, T. Dang, and C. Stiller. Trajectory planning for Bertha — a local, continuous method. In 2014 IEEE Intelligent Vehicles Symposium Proceedings, pages 450–457, June 2014.

[172] G P Bevan, H Gollee, and J O’Reilly. Trajectory generation for road vehicle obstacle avoidance using convex optimization. Proceedings of the Institu- tion of Mechanical Engineers, Part D: Journal of Automobile Engineering, 224(4):455–473, 2010.

[173] R. Oliveira, P. F. Lima, G. Collares Pereira, J. M˚artensson,and B. Wahlberg. Path planning for autonomous bus driving in highly constrained environments. In 2019 IEEE Intelligent Transportation Systems Conference (ITSC), pages 2743–2749, Oct 2019.

[174] H. Andersen, W. Schwarting, F. Naser, Y. H. Eng, M. H. Ang, D. Rus, and J. Alonso-Mora. Trajectory optimization for autonomous overtaking with visibility maximization. In 2017 IEEE 20th International Conference on In- telligent Transportation Systems (ITSC), pages 1–8, Oct 2017.

[175] I. Batkovic, M. Zanon, M. Ali, and P. Falcone. Real-time constrained trajectory planning and vehicle control for proactive autonomous driving with road users. In 2019 18th European Control Conference (ECC), pages 256–262, June 2019.

[176] W. Schwarting, J. Alonso-Mora, L. Paull, S. Karaman, and D. Rus. Safe nonlinear trajectory generation for parallel autonomy with a dynamic vehicle model. IEEE Transactions on Intelligent Transportation Systems, 19(9):2994–3008, Sep. 2018.

[177] Rui Oliveira. Motion planning for heavy-duty vehicles, 2019.

[178] R. Oliveira, M. Cirillo, J. M. artensson, and B. Wahlberg. Combining lattice- based planning and path optimization in autonomous heavy duty vehicle applications. In 2018 IEEE Intelligent Vehicles Symposium (IV), pages 2090– 2097, June 2018.

[179] Dmitri Dolgov, Sebastian Thrun, Michael Montemerlo, and James Diebel. Path planning for autonomous vehicles in unknown semi-structured environments. The International Journal of Robotics Research, 29(5):485–501, 2010.

[180] N. Evestedt, E. Ward, J. Folkesson, and D. Axehill. Interaction aware trajectory planning for merge scenarios in congested traﬃc situations. In 2016 IEEE 19th International Conference on Intelligent Transportation Systems (ITSC), pages 465–472, Nov 2016.

[181] S. Heinrich, A. Zoufahl, and R. Rojas. Real-time trajectory optimization under motion uncertainty using a gpu. In 2015 IEEE/RSJ International 184 BIBLIOGRAPHY

Conference on Intelligent Robots and Systems (IROS), pages 3572–3577, Sep. 2015. [182] B. Paden, M. Cáp,S.ˇ Z. Yong, D. Yershov, and E. Frazzoli. A survey of motion planning and control techniques for self-driving urban vehicles. IEEE Transactions on Intelligent Vehicles, 1(1):33–55, March 2016. [183] Stefan Björnander and Lars Grunske. Adaptive cruise controllers–a literature review. Technical report, Swinburne University of Technology, 2008. Available at http://www.ipr.mdh.se/pdf publications/1828.pdf. [184] Yinglong He, Biagio Ciuffo, Quan Zhou, Michail Makridis, Konstantinos Mattas, Ji Li, Ziyang Li, Fuwu Yan, and Hongming Xu. Adaptive cruise control strategies implemented on experimental vehicles: A review. IFAC- PapersOnLine, 52(5):21 – 27, 2019. 9th IFAC Symposium on Advances in Automotive Control AAC 2019. [185] Z. Wang, G. Wu, and M. J. Barth. A review on cooperative adaptive cruise control (cacc) systems: Architectures, controls, and applications. In 2018 21st International Conference on Intelligent Transportation Systems (ITSC), pages 2884–2891, Nov 2018. [186] Michele Vignati, Edoardo Sabbioni, and Federico Cheli. A torque vectoring control for enhancing vehicle performance in drifting. Electronics, 7(12):394, 2018. [187] Rivian. Tank turn | electric adventure vehicles | rivian. Available at https://www.youtube.com/watch?v=yzwM8KE2L3I&feature=emb title. [188] Audi. Audi A8 – dynamic all-wheel steering. Available at https://www.audi-technology-portal.de/en/chassis/wheel-suspension- steering/audi-a8-dynamic-all-wheel-steering. [189] G. Collares Pereira, L. Svensson, P. F. Lima, and J. M˚artensson. Lateral model predictive control for over-actuated autonomous vehicle. In 2017 IEEE Intelligent Vehicles Symposium (IV), pages 310–316, June 2017. [190] DLR Deutsches Zentrum fürLuft-und Raumfahrt. A car with space technology - driving with the robomobil. Available at https://www.youtube.com/watch?v=CfSGFrbRlzc. [191] U. Rosolia, S. De Bruyne, and A. G. Alleyne. Autonomous vehicle control: A nonconvex approach for obstacle avoidance. IEEE Transactions on Control Systems Technology, 25(2):469–484, March 2017. [192] Omead Amidi and Chuck E. Thorpe. Integrated mobile robot control. In Wendell H. Chun and William J. Wolfe, editors, Mobile Robots V, volume 1388, pages 504 – 523. International Society for Optics and Photonics, SPIE, 1991. BIBLIOGRAPHY 185

[193] R. Wallace, Anthony (Tony) Stentz, Chuck Thorpe, Hans Moravec, William (Red) L. Whittaker, and Takeo Kanade. First results in robot road-following. In Proceedings of the International Joint Conference on Artiﬁcial Intelligence, January 1985.

[194] Jarrod M. Snider. Automatic steering methods for autonomous automobile path tracking. Technical report, Robotics Institute, Carnegie Mellon Univer- sity, March 2009. Available at https://www.ri.cmu.edu/pub ﬁles/2009/2/Automatic Steering Methods for Autonomous Automobile Path Tracking.pdf.

[195] M. Buehler, K. Iagnemma, and S. Singh. The 2005 DARPA Grand Challenge: The Great Robot Race. Springer Tracts in Advanced Robotics. Springer Berlin Heidelberg, 2007.

[196] M. Buehler, K. Iagnemma, and S. Singh. The DARPA Urban Challenge: Autonomous Vehicles in City Traﬃc. Springer Tracts in Advanced Robotics. Springer Berlin Heidelberg, 2009.

[197] Mechanical Simulation Corporation. Carsim. Site: https://www.carsim.com/.

[198] Yunxiao Shan, Wei Yang, Cheng Chen, Jian Zhou, Ling Zheng, and Bijun Li. Cf-pursuit: A pursuit method with a clothoid ﬁtting and a fuzzy controller for autonomous vehicles. International Journal of Advanced Robotic Systems, 12(9):134, 2015.

[199] Yoshiaki Kuwata, Justin Teo, Sertac Karaman, Gaston Fiore, Emilio Frazzoli, and Jonathan How. Motion Planning in Complex Environments Using Closed- loop Prediction. 2008.

[200] G. M. Hoﬀmann, C. J. Tomlin, M. Montemerlo, and S. Thrun. Autonomous automobile trajectory tracking for oﬀ-road driving: Controller design, experimental validation and racing. In 2007 American Control Conference, pages 2296–2301, July 2007.

[201] Noor Hafizah Amer, Khisbullah Hudha, Hairi Zamzuri, Vimal Rau Aparow, Amar Faiz Zainal Abidin, Zulkiffli Abd Kadir, and Muhamad Murrad. Adap- tive modified stanley controller with fuzzy supervisory system for trajectory tracking of an autonomous armoured vehicle. Robotics and Autonomous Sys- tems, 105:94 – 111, 2018.

[202] Patrick Fleischmann, Tobias F¨ohst,and Karsten Berns. Trajectory planning and lateral control for agricultural guidance applications. In 8th International Conference on Information Technology and Applications, volume 128, page 133, 2013. 186 BIBLIOGRAPHY

[203] M. Cibooglu, U. Karapinar, and M. T. S¨oylemez. Hybrid controller approach for an autonomous ground vehicle path tracking problem. In 2017 25th Mediterranean Conference on Control and Automation (MED), pages 583–588, July 2017.

[204] Karl J. Astr¨om.˚ PID controllers theory, design, and tuning, 2nd edition. 2nd ed edition, 1995.

[205] National Instruments. Pid theory explained. Available at https://www.ni.com/sv-se/innovations/white-papers/06/pid-theory- explained.html.

[206] Salim Hima, Benoit Lusseti, Benoit Vanholme, Sebastien Glaser, and Said Mammar. Trajectory tracking for highly automated passenger vehicles. IFAC Proceedings Volumes, 44(1):12958 – 12963, 2011. 18th IFAC World Congress.

[207] K. Osman, M. F. Rahmat, and M. A. Ahmad. Modelling and controller design for a cruise control system. In 2009 5th International Colloquium on Signal Processing Its Applications, pages 254–258, March 2009.

[208] K. Lidström,K. Sjöberg, U. Holmberg, J. Andersson, F. Bergh, M. Bjäde,and S. Mak. A modular cacc system integration and design. IEEE Transactions on Intelligent Transportation Systems, 13(3):1050–1061, Sep. 2012.

[209] Julius K. Kolb, Gunter Nitzsche, and Sebastian Wagner. A simple yet eﬃ- cient path tracking controller for autonomous trucks. IFAC-PapersOnLine, 52(8):307 – 312, 2019. 10th IFAC Symposium on Intelligent Autonomous Vehicles IAV 2019.

[210] Riccardo Marino, Stefano Scalzi, and Mariana Netto. Nested pid steering control for lane keeping in autonomous vehicles. Control Engineering Practice, 19(12):1459 – 1467, 2011.

[211] Muhammad Aizzat Zakaria, Hairi Zamzuri, and Saiful Amri Mazlan. Dy- namic curvature steering control for autonomous vehicle: performance analysis. In IOP Conference Series: Materials Science and Engineering, volume 114, page 012149. IOP Publishing, 2016.

[212] Alberto Broggi, Massimo Bertozzi, Alessandra Fascioli, C Guarino Lo Bianco, and Aurelio Piazzi. The argo autonomous vehicle’s vision and control systems. International Journal of Intelligent Control and Systems, 3(4):409–441, 1999.

[213] Pan Zhao, Jiajia Chen, Yan Song, Xiang Tao, Tiejuan Xu, and Tao Mei. Design of a control system for an autonomous vehicle based on adaptive-pid. International Journal of Advanced Robotic Systems, 9(2):44, 2012. BIBLIOGRAPHY 187

[214] Lghani Menhour, Brigitte d’Andr´ea Novel, Michel Fliess, and Hugues Mounier. Coupled nonlinear vehicle control: Flatness-based setting with algebraic estimation techniques. Control Engineering Practice, 22:135 – 146, 2014.

[215] M. Soheil-Hamedani, M. Zandi, R. Gavagsaz-Ghoachani, B. Nahid- Mobarakeh, and S. Pierfederici. Flatness-based control method: A review of its applications to power systems. In 2016 7th Power Electronics and Drive Systems Technologies Conference (PEDSTC), pages 547–552, Feb 2016.

[216] Jonathan Y. Goh, Tushar Goel, and J. Christian Gerdes. Toward Automated Vehicle Control Beyond the Stability Limits: Drifting Along a General Path. Journal of Dynamic Systems, Measurement, and Control, 142(2), 11 2019. 021004.

[217] Stanford. Youtube video - beyond the limits: Martykhana, 2019. Available at https://www.youtube.com/watch?v=3x3SqeSdrAE.

[218] Bjorn Carey. Driving sideways to move forward: Stanford engineers show how an autonomous, drifting delorean can improve driver safety, 2019. Avail- able at https://news.stanford.edu/2019/12/20/autonomous-delorean-drives- sideways-move-forward/.

[219] L.A. Zadeh. Fuzzy sets. Information and Control, 8(3):338 – 353, 1965.

[220] VilémNovák,Irina Perfilieva, and Jiˇr´ıMoˇckoˇr. Mathematical Principles of Fuzzy Logic, volume 517 of The Springer International Series in Engineering and Computer Science,. Springer US, Boston, MA, 1999.

[221] Lotﬁ A. Zadeh. Fuzzy logic—a personal perspective. Fuzzy Sets and Systems, 281:4 – 20, 2015. Special Issue Celebrating the 50th Anniversary of Fuzzy Sets.

[222] Astrid Rupp and Michael Stolz. Survey on Control Schemes for Automated Driving on Highways, pages 43–69. Springer International Publishing, Cham, 2017.

[223] M. Sugeno and M. Nishida. Fuzzy control of model car. Fuzzy Sets and Systems, 16(2):103 – 113, 1985.

[224] T. Hessburg and M. Tomizuka. Fuzzy logic control for lateral vehicle guidance. IEEE Control Systems Magazine, 14(4):55–63, Aug 1994.

[225] J. E. Naranjo, C. Gonzalez, R. Garcia, and T. de Pedro. Lane-change fuzzy control in autonomous vehicles for the overtaking maneuver. IEEE Transac- tions on Intelligent Transportation Systems, 9(3):438–450, Sep. 2008. 188 BIBLIOGRAPHY

[226] S. Chaib, M. S. Netto, and S. Mammar. H/sub /spl infin//, adaptive, pid and fuzzy control: a comparison of controllers for vehicle lane keeping. In IEEE Intelligent Vehicles Symposium, 2004, pages 139–144, June 2004. [227] Xinyu Wang, Mengyin Fu, Hongbin Ma, and Yi Yang. Lateral control of autonomous vehicles based on fuzzy logic. Control Engineering Practice, 34:1 – 17, 2015. [228] H. Asere, C. Lei, and R. Jia. Cruise control design using fuzzy logic controller. In 2015 IEEE International Conference on Systems, Man, and Cybernetics, pages 2210–2215, Oct 2015. [229] J. E. Naranjo, J. Reviejo, C. González,R. Garc´ıa, and T. de Pedro. A throttle and brake fuzzy controller: Towards the automatic car. In Roberto Moreno-D´ıazand Franz Pichler, editors, Computer Aided Systems Theory - EUROCAST 2003, pages 291–301, Berlin, Heidelberg, 2003. Springer Berlin Heidelberg. [230] Ching-Chih Tsai, Shih-Min Hsieh, and Chien-Tzu Chen. Fuzzy longitudinal controller design and experimentation for adaptive cruise control and stop & go. Journal of Intelligent & Robotic Systems, 59(2):167–189, 2010. [231] J. Pérez,V. Milanés,J. Godoy, J. Villagrá,and E. Onieva. Cooperative controllers for highways based on human experience. Expert Systems with Applications, 40(4):1024 – 1033, 2013. [232] Yuri Shtessel, Christopher Edwards, Leonid Fridman, and Arie Levant. Slid- ing Mode Control and Observation. Control Engineering. Springer New York, New York, NY, 2014. [233] Antonella Ferrara, editor. Sliding Mode Control of Vehicle Dynamics. Trans- port. Institution of Engineering and Technology, 2017. [234] ARIE LEVANT. Sliding order and sliding accuracy in sliding mode control. International Journal of Control, 58(6):1247–1263, 1993. [235] R. Rajamani, Han-Shue Tan, Boon Kait Law, and Wei-Bin Zhang. Demon- stration of integrated longitudinal and lateral control for the operation of automated vehicles in platoons. IEEE Transactions on Control Systems Tech- nology, 8(4):695–708, July 2000. [236] A. Ferrara and P. Pisu. Minimum sensor second-order sliding mode longitudinal control of passenger vehicles. IEEE Transactions on Intelligent Transportation Systems, 5(1):20–32, March 2004. [237] Lydie Nouvelièreand Saı¨d Mammar. Experimental vehicle longitudinal control using a second order sliding mode technique. Control Engineering Prac- tice, 15(8):943 – 954, 2007. Special Section on Modelling and Control for Participatory Planning and Managing Water Systems. BIBLIOGRAPHY 189

[238] Antonella Ferrara and Claudio Vecchio. Second order sliding mode control of vehicles with distributed collision avoidance capabilities. Mechatronics, 19(4):471 – 477, 2009. Robotics and Factory of the Future, New Trends and Challenges in Mechatronics.

[239] M. Canale, L. Fagiano, A. Ferrara, and C. Vecchio. Vehicle yaw control via second-order sliding-mode technique. IEEE Transactions on Industrial Electronics, 55(11):3908–3916, Nov 2008.

[240] H. Imine, L. M. Fridman, and T. Madani. Steering control for rollover avoidance of heavy vehicles. IEEE Transactions on Vehicular Technology, 61(8):3499–3509, Oct 2012.

[241] G. Tagne, R. Talj, and A. Charara. Higher-order sliding mode control for lateral dynamics of autonomous vehicles, with experimental validation. In 2013 IEEE Intelligent Vehicles Symposium (IV), pages 678–683, June 2013.

[242] S. Dominguez, A. Ali, G. Garcia, and P. Martinet. Comparison of lateral controllers for autonomous vehicle: Experimental results. In 2016 IEEE 19th In- ternational Conference on Intelligent Transportation Systems (ITSC), pages 1418–1423, Nov 2016.

[243] E. Alcalá,L. Sellart, V. Puig, J. Quevedo, J. Saludes, D. Vázquez,and A. López. Comparison of two non-linear model-based control strategies for autonomous vehicles. In 2016 24th Mediterranean Conference on Control and Automation (MED), pages 846–851, June 2016.

[244] Brian DO Anderson and John B Moore. Optimal control: linear quadratic methods. Courier Corporation, 2007.

[245] Eugene Lavretsky and Kevin A. Wise. Optimal Control and the Linear Quadratic Regulator, pages 27–50. Springer London, London, 2013.

[246] Sigurd Skogestad and Ian Postlethwaite. Multivariable feedback control : analysis and design. John Wiley & Sons, Hoboken, NJ, 2 ed.. edition, 2007.

[247] MathWorks. Control system toolbox, 2020. Available at https://mathworks.com/products/control.html.

[248] J. Doyle. Guaranteed margins for lqg regulators. IEEE Transactions on Automatic Control, 23(4):756–757, August 1978.

[249] A. A. Alam, A. Gattami, and K. H. Johansson. Suboptimal decentralized controller design for chain structures: Applications to vehicle formations. In 2011 50th IEEE Conference on Decision and Control and European Control Conference, pages 6894–6900, Dec 2011. 190 BIBLIOGRAPHY

[250] Assad Alam, Jonas M˚artensson,and Karl H. Johansson. Experimental evaluation of decentralized cooperative cruise control for heavy-duty vehicle platooning. Control Engineering Practice, 38:11 – 25, 2015.

[251] S. S. Stankovi´c,M. J. Stanojevic, and D. D. Siljak. Decentralized overlapping control of a platoon of vehicles. IEEE Transactions on Control Systems Technology, 8(5):816–832, Sep. 2000.

[252] S. S. Stankovi´c,M. J. Stanojevic, and D. D. Siljak. Stochastic inclusion principle applied to decentralized overlapping suboptimal lqg control of a platoon of vehicles. In EUROCON 2005 - The International Conference on ”Computer as a Tool”, volume 1, pages 318–321, Nov 2005.

[253] Tesheng Hsiao and M. Tomizuka. Design of position feedback controllers for vehicle lateral motion. In 2006 American Control Conference, pages 6 pp.–, June 2006.

[254] D. Kim, J. Kang, and K. Yi. Control strategy for high-speed autonomous driving in structured road. In 2011 14th International IEEE Conference on Intelligent Transportation Systems (ITSC), pages 186–191, Oct 2011.

[255] S. Jeon, K. Lee, H. Kim, and D. Kum. Path tracking control of autonomous vehicles using augmented lqg with curvature disturbance model. In 2019 19th International Conference on Control, Automation and Systems (IC- CAS), pages 1543–1548, Oct 2019.

[256] O. Ljungqvist, D. Axehill, and A. Helmersson. Path following control for a reversing general 2-trailer system. In 2016 IEEE 55th Conference on Decision and Control (CDC), pages 2455–2461, Dec 2016.

[257] Oskar Ljungqvist, Niclas Evestedt, Daniel Axehill, Marcello Cirillo, and Hen- rik Pettersson. A path planning and path-following control framework for a general 2-trailer with a car-like tractor. Journal of Field Robotics, 36(8):1345– 1377, 2019.

[258] Huibert Kwakernaak. H2-optimization — theory and applications to robust control design. Annual Reviews in Control, 26(1):45 – 56, 2002.

[259] G. Zames. Feedback and optimal sensitivity: Model reference transforma- tions, multiplicative seminorms, and approximate inverses. IEEE Transac- tions on Automatic Control, 26(2):301–320, April 1981.

[260] M. Lungu, R. Lungu, and D. Tutunea. Control of aircraft landing using the dynamic inversion and the h-inf control. In 2016 17th International Carpathian Control Conference (ICCC), pages 461–466, May 2016. BIBLIOGRAPHY 191

[261] A. Morand, X. Moreau, P. Melchior, M. Moze, and F. Guillemard. Crone cruise control system. IEEE Transactions on Vehicular Technology, 65(1):15– 28, Jan 2016.

[262] Patrick Lanusse, Rachid Malti, and Pierre Melchior. Crone control system design toolbox for the control engineering community: tutorial and case study. Philosophical Transactions of the Royal Society A: Mathematical, Physical and Engineering Sciences, 371(1990):20120149, 2013.

[263] L. Xu, W. Zhuang, G. Yin, and C. Bian. Stable longitudinal control of heterogeneous vehicular platoon with disturbances and information delays. IEEE Access, 6:69794–69806, 2018.

[264] F. Morbidi, P. Colaneri, and T. Stanger. Decentralized optimal control of a car platoon with guaranteed string stability. In 2013 European Control Conference (ECC), pages 3494–3499, July 2013.

[265] Roman Schmied and Patrizio Colaneri. Mixed h2-h∞ control for automated highway driving. Mechatronics, 57:63 – 72, 2019.

[266] Masayasu Shimakage, Shigeki Satoh, Kenya Uenuma, and Hiroshi Mouri. Design of lane-keeping control with steering torque input. JSAE Review, 23(3):317 – 323, 2002.

[267] C. Hwang and L. Chang. Trajectory tracking and obstacle avoidance of car- like mobile robots in an intelligent space using mixed h2/h∞ decentralized control. IEEE/ASME Transactions on Mechatronics, 12(3):345–352, June 2007.

[268] Fei-Xiang Xu, Xin-Hui Liu, Wei Chen, Chen Zhou, and Bing-Wei Cao. Im- proving handling stability performance of four-wheel steering vehicle based on the h2/h∞ robust control. Applied Sciences, 9(5):857, Feb 2019.

[269] S.Joe Qin and Thomas A. Badgwell. A survey of industrial model predictive control technology. Control Engineering Practice, 11(7):733 – 764, 2003.

[270] J. Richalet, A. Rault, J.L. Testud, and J. Papon. Model predictive heuristic control: Applications to industrial processes. Automatica, 14(5):413 – 428, 1978.

[271] Francesco Borrelli, Alberto Bemporad, and Manfred Morari. Predictive Con- trol for Linear and Hybrid Systems. Cambridge University Press, 2017.

[272] William S. Levine. Handbook of Model Predictive Control, chapter The Es- sentials of Model Predictive Control, pages 3–27. Springer International Pub- lishing, Cham, 2019. 192 BIBLIOGRAPHY

[273] J. A. Matute, M. Marcano, A. Zubizarreta, and J. Perez. Longitudinal model predictive control with comfortable speed planner. In 2018 IEEE International Conference on Autonomous Robot Systems and Competitions (ICARSC), pages 60–64, April 2018.

[274] Payman Shakouri, Andrzej Ordys, and Mohamad R. Askari. Adaptive cruise control with stop&go function using the state-dependent nonlinear model predictive control approach. ISA Transactions, 51(5):622 – 631, 2012.

[275] P. Shakouri, A. Ordys, D.S. Laila, and M. Askari. Adaptive cruise control system: Comparing gain-scheduling pi and lq controllers. IFAC Proceedings Volumes, 44(1):12964 – 12969, 2011. 18th IFAC World Congress.

[276] Dominik Lang, Thomas Stanger, Roman Schmied, and Luigi del Re. Op- timization and Optimal Control in Automotive Systems, chapter Predictive Cooperative Adaptive Cruise Control: Fuel Consumption Beneﬁts and Imple- mentability, pages 163–178. Springer International Publishing, Cham, 2014.

[277] T. Stanger and L. del Re. A model predictive cooperative adaptive cruise control approach. In 2013 American Control Conference, pages 1374–1379, June 2013.

[278] Roman Schmied, Harald Waschl, Rien Quirynen, Moritz Diehl, and Luigi del Re. Nonlinear mpc for emission eﬃcient cooperative adaptive cruise control. IFAC-PapersOnLine, 48(23):160 – 165, 2015. 5th IFAC Conference on Nonlinear Model Predictive Control NMPC 2015.

[279] F. Borrelli, P. Falcone, T. Keviczky, J. Asgari, and D. Hrovat. Mpc-based approach to active steering for autonomous vehicle systems. Int. J. of Vehicle Autonomous Systems, 3(2/3/4), 2005.

[280] T. Keviczky, P. Falcone, F. Borrelli, J. Asgari, and D. Hrovat. Predictive control approach to autonomous vehicle steering. In 2006 American Control Conference, pages 6 pp.–, June 2006.

[281] P. Falcone, F. Borrelli, H. E. Tseng, J. Asgari, and D. Hrovat. Linear time- varying model predictive control and its application to active steering systems: Stability analysis and experimental validation. International Journal of Robust and Nonlinear Control, 18(8):862–875, 2008.

[282] Paolo Falcone, H. Eric Tseng, Francesco Borrelli, Jahan Asgari, and Davor Hrovat. Mpc-based yaw and lateral stabilisation via active front steering and braking. Vehicle System Dynamics, 46(sup1):611–628, 2008.

[283] J. Kong, M. Pfeiﬀer, G. Schildbach, and F. Borrelli. Kinematic and dynamic vehicle models for autonomous driving control design. In 2015 IEEE Intelli- gent Vehicles Symposium (IV), pages 1094–1099, June 2015. BIBLIOGRAPHY 193

[284] Yongsoon Yoon, Jongho Shin, H. Jin Kim, Yongwoon Park, and Shankar Sas- try. Model-predictive active steering and obstacle avoidance for autonomous ground vehicles. Control Engineering Practice, 17(7):741 – 750, 2009.

[285] Z. A. Leman, M. Hatta Mohammad Ariﬀ, H. Zamzuri, M. A. Abdul Rah- man, and S. Amri Mazlan. Model predictive controller for path tracking and obstacle avoidance manoeuvre on autonomous vehicle. In 2019 12th Asian Control Conference (ASCC), pages 1271–1276, June 2019.

[286] Yiqi Gao, Theresa Lin, Francesco Borrelli, Eric Tseng, and Davor Hrovat. Predictive control of autonomous ground vehicles with obstacle avoidance on slippery roads. 01 2010.

[287] Yiqi Gao, Andrew Gray, Janick V Frasch, Theresa Lin, Eric Tseng, J Karl Hedrick, and Francesco Borrelli. Spatial predictive control for agile semi- autonomous ground vehicles. Proceedings of the 11th International Sympo- sium on Advanced Vehicle Control, VD11(2):1–6, 2012.

[288] A. Gray, Y. Gao, T. Lin, J. K. Hedrick, H. E. Tseng, and F. Borrelli. Pre- dictive control for agile semi-autonomous ground vehicles using motion primitives. In 2012 American Control Conference (ACC), pages 4239–4244, June 2012.

[289] A. Carvalho, Y. Gao, A. Gray, H. E. Tseng, and F. Borrelli. Predictive control of an autonomous ground vehicle using an iterative linearization approach. In 16th International IEEE Conference on Intelligent Transportation Systems (ITSC 2013), pages 2335–2340, Oct 2013.

[290] C. J. Ostafew, A. P. Schoellig, and T. D. Barfoot. Learning-based nonlinear model predictive control to improve vision-based mobile robot path-tracking in challenging outdoor environments. In 2014 IEEE International Conference on Robotics and Automation (ICRA), pages 4029–4036, May 2014.

[291] Chris J. Ostafew, Angela P. Schoellig, Timothy D. Barfoot, and Jack Collier. Learning-based nonlinear model predictive control to improve vision-based mobile robot path tracking. Journal of Field Robotics, 33(1):133–152, 2016.

[292] Alberto Bemporad, Manfred Morari, Vivek Dua, and Efstratios N. Pis- tikopoulos. The explicit linear quadratic regulator for constrained systems. Automatica, 38(1):3 – 20, 2002.

[293] S. Di Cairano, H. E. Tseng, D. Bernardini, and A. Bemporad. Vehicle yaw stability control by coordinated active front steering and diﬀerential braking in the tire sideslip angles domain. IEEE Transactions on Control Systems Technology, 21(4):1236–1248, July 2013. 194 BIBLIOGRAPHY

[294] Pierre Apkarian, Pascal Gahinet, and Greg Becker. Self-scheduled H∞ control of linear parameter-varying systems: a design example. Automatica, 31(9):1251 – 1261, 1995.

[295] T. Besselmann, J. L¨ofberg, and M. Morari. Explicit model predictive control for systems with linear parameter-varying state transition matrix. IFAC Proceedings Volumes, 41(2):13163 – 13168, 2008. 17th IFAC World Congress.

[296] T. Besselmann, J. L¨ofberg, and M. Morari. Explicit model predictive control for linear parameter-varying systems. In Proceedings of the 47th IEEE Conference on Decision and Control, pages 3848–3853, Dec 2008.

[297] T. Besselmann, J. L¨ofberg, and M. Morari. Explicit lpv-mpc with bounded rate of parameter variation. IFAC Proceedings Volumes, 42(6):7 – 12, 2009. 6th IFAC Symposium on Robust Control Design.

[298] T. Besselmann, J. L¨ofberg, and M. Morari. Explicit mpc for lpv systems: Sta- bility and optimality. IEEE Transactions on Automatic Control, 57(9):2322– 2332, Sept 2012.

[299] P. F. Lima, G. Collares Pereira, J. M˚artensson, and B. Wahlberg. Progress maximization model predictive controller. In 2018 21st International Con- ference on Intelligent Transportation Systems (ITSC), pages 1075–1082, Nov 2018.

[300] S´ebastienGros, Mario Zanon, Rien Quirynen, Alberto Bemporad, and Moritz Diehl. From linear to nonlinear mpc: bridging the gap via the real-time iteration. International Journal of Control, 93(1):62–80, 2020.

[301] Niels van Duijkeren, Tamas Keviczky, Peter Nilsson, and Leo Laine. Real-time nmpc for semi-automated highway driving of long heavy vehicle combinations. IFAC-PapersOnLine, 48(23):39 – 46, 2015. 5th IFAC Conference on Nonlinear Model Predictive Control NMPC 2015.

[302] V. L. Bageshwar, W. L. Garrard, and R. Rajamani. Model predictive control of transitional maneuvers for adaptive cruise control vehicles. IEEE Trans- actions on Vehicular Technology, 53(5):1573–1585, Sep. 2004.

[303] S. Li, K. Li, R. Rajamani, and J. Wang. Model predictive multi-objective vehicular adaptive cruise control. IEEE Transactions on Control Systems Technology, 19(3):556–566, May 2011.

[304] F. Bu, H. Tan, and J. Huang. Design and ﬁeld testing of a cooperative adaptive cruise control system. In Proceedings of the 2010 American Control Conference, pages 4616–4621, June 2010. BIBLIOGRAPHY 195

[305] Paolo Falcone, Francesco Borrelli, Jahan Asgari, H. Eric Tseng, and Davor Hrovat. A real-time model predictive control approach for autonomous active steering. Nonlinear Model Predictive Control for Fast Systems, Grenoble, France, 2006.

[306] P. Falcone, F. Borrelli, J. Asgari, H. E. Tseng, and D. Hrovat. Predictive active steering control for autonomous vehicle systems. IEEE Transactions on Control Systems Technology, 15(3):566–580, May 2007.

[307] P. Falcone, M. Tufo, F. Borrelli, J. Asgari, and H. E. Tseng. A linear time varying model predictive control approach to the integrated vehicle dynamics control problem in autonomous systems. In 2007 46th IEEE Conference on Decision and Control, pages 2980–2985, Dec 2007.

[308] P. Falcone, F. Borrelli, H. E. Tseng, J. Asgari, and D. Hrovat. A hierarchical model predictive control framework for autonomous ground vehicles. In 2008 American Control Conference, pages 3719–3724, June 2008.

[309] Julian P. Timings and David J. Cole. Minimum Maneuver Time Calculation Using Convex Optimization. Journal of Dynamic Systems, Measurement, and Control, 135(3), 03 2013. 031015.

[310] V. Turri, A. Carvalho, H. E. Tseng, K. H. Johansson, and F. Borrelli. Lin- ear model predictive control for lane keeping and obstacle avoidance on low curvature roads. In 16th International IEEE Conference on Intelligent Trans- portation Systems (ITSC 2013), pages 378–383, Oct 2013.

[311] E. Kim, J. Kim, and M. Sunwoo. Model predictive control strategy for smooth path tracking of autonomous vehicles with steering actuator dynamics. In- ternational Journal of Automotive Technology, 15(7):1155–1164, 2014.

[312] J. Ji, A. Khajepour, W. W. Melek, and Y. Huang. Path planning and tracking for vehicle collision avoidance based on model predictive control with multi- constraints. IEEE Transactions on Vehicular Technology, 66(2):952–964, Feb 2017.

[313] M. Graf Plessen, D. Bernardini, H. Esen, and A. Bemporad. Spatial-based predictive control and geometric corridor planning for adaptive cruise control coupled with obstacle avoidance. IEEE Transactions on Control Systems Technology, 26(1):38–50, Jan 2018.

[314] A. Gray, Y. Gao, T. Lin, J. K. Hedrick, and F. Borrelli. Stochastic predictive control for semi-autonomous vehicles with an uncertain driver model. In 16th International IEEE Conference on Intelligent Transportation Systems (ITSC 2013), pages 2329–2334, Oct 2013. 196 BIBLIOGRAPHY

[315] Alexander Liniger, Alexander Domahidi, and Manfred Morari. Optimization- based autonomous racing of 1:43 scale rc cars. Optimal Control Applications and Methods, 36(5):628–647, 2015.

[316] A. Liniger, X. Zhang, P. Aeschbach, A. Georghiou, and J. Lygeros. Racing miniature cars: Enhancing performance using stochastic mpc and disturbance feedback. In 2017 American Control Conference (ACC), pages 5642–5647, May 2017.

[317] Ashwin Carvalho, Yiqi Gao, St´ephanie Lefevre, and Francesco Borrelli. Stochastic predictive control of autonomous vehicles in uncertain environments. In 12th International Symposium on Advanced Vehicle Control, pages 712–719, 2014.

[318] Ashwin Carvalho, St´ephanie Lef´evre,Georg Schildbach, Jason Kong, and Francesco Borrelli. Automated driving: The role of forecasts and uncertainty—a control perspective. European Journal of Control, 24:14 – 32, 2015. SI: ECC15.

[319] D.Q. Mayne, J.B. Rawlings, C.V. Rao, and P.O.M. Scokaert. Constrained model predictive control: Stability and optimality. Automatica, 36(6):789 – 814, 2000.

[320] D. Q. Mayne, M. M. Seron, and S.V. Rakovi´c. Robust model predictive control of constrained linear systems with bounded disturbances. Automatica, 41(2):219 – 224, 2005.

[321] D. Q. Mayne, S. V. Rakovi´c,R. Findeisen, and F. Allg¨ower. Robust output feedback model predictive control of constrained linear systems. Automatica, 42(7):1217 – 1222, 2006.

[322] ”D. Q. Mayne, S. V. Rakovi´c,R. Findeisen, and F. Allg¨ower. Robust output feedback model predictive control of constrained linear systems: Time varying case. Automatica, 45(9):2082 – 2087, 2009.

[323] Pornchai Bumroongsri. Tube-based robust mpc for linear time-varying systems with bounded disturbances. International Journal of Control, Automa- tion and Systems, 13(3):620–625, Jun 2015.

[324] A. Richards. Robust model predictive control for time-varying systems. In Proceedings of the 44th IEEE Conference on Decision and Control, pages 3747–3752, Dec 2005.

[325] A. Gray, Y. Gao, J. K. Hedrick, and F. Borrelli. Robust predictive control for semi-autonomous vehicles with an uncertain driver model. In 2013 IEEE Intelligent Vehicles Symposium (IV), pages 208–213, June 2013. BIBLIOGRAPHY 197

[326] Yiqi Gao, Andrew Gray, H. Eric Tseng, and Francesco Borrelli. A tube-based robust nonlinear predictive control approach to semiautonomous ground vehicles. Vehicle System Dynamics, 52(6):802–823, 2014. [327] Chris J. Ostafew, Angela P. Schoellig, and Timothy D. Barfoot. Robust constrained learning-based nmpc enabling reliable mobile robot path tracking. The International Journal of Robotics Research, 35(13):1547–1563, 2016. [328] A. Katriniok and D. Abel. Ltv-mpc approach for lateral vehicle guidance by front steering at the limits of vehicle dynamics. In 2011 50th IEEE Conference on Decision and Control and European Control Conference, pages 6828–6833, Dec 2011. [329] A. Katriniok, J. P. Maschuw, F. Christen, L. Eckstein, and D. Abel. Optimal vehicle dynamics control for combined longitudinal and lateral autonomous vehicle guidance. In 2013 European Control Conference (ECC), pages 974– 979, July 2013. [330] C. E. Beal and J. C. Gerdes. Model predictive control for vehicle stabilization at the limits of handling. IEEE Transactions on Control Systems Technology, 21(4):1258–1269, July 2013. [331] S. M. Erlien, J. Funke, and J. C. Gerdes. Incorporating non-linear tire dynamics into a convex approach to shared steering control. In 2014 American Control Conference, pages 3468–3473, June 2014. [332] S. M. Erlien, S. Fujita, and J. C. Gerdes. Shared steering control using safe envelopes for obstacle avoidance and vehicle stability. IEEE Transactions on Intelligent Transportation Systems, 17(2):441–451, Feb 2016. [333] ISO 3888-1: 1999. Passenger cars–test track for a severe lane-change manoeuvre–part 1: double lane-change, 1999. [334] Pierre O. M. Scokaert and James B. Rawlings. Feasibility issues in linear model predictive control. AIChE Journal, 45(8):1649–1659, 8 1999. [335] Eric C. Kerrigan and Jan Maciejowski. Soft constraints and exact penalty functions in model predictive control. 09 2000. [336] P. F. Lima, M. Trincavelli, M. Nilsson, J. M˚artensson, and B. Wahlberg. Ex- perimental evaluation of economic model predictive control for an autonomous truck. In 2016 IEEE Intelligent Vehicles Symposium (IV), pages 710–715, June 2016. [337] P. F. Lima, J. M˚artensson, and B. Wahlberg. Stability conditions for linear time-varying model predictive control in autonomous driving. In Proceedings of the 56th IEEE Conference on Decision and Control, pages 2775–2782, Dec 2017. 198 BIBLIOGRAPHY

[338] P. F. Lima, G. Collares Pereira, J. M˚artensson,and B. Wahlberg. Experi- mental validation of model predictive control stability for autonomous driving. Control Engineering Practice, 81:244 – 255, 2018. [339] Oskar Ljungqvist and Daniel Axehill. A predictive path-following controller for multi-steered articulated vehicles, 2019. Submitted to 21st IFAC World Congress, 2020.

[340] Oskar Ljungqvist, Daniel Axehill, and Henrik Pettersson. On sensing-aware model predictive path-following control for a reversing general 2-trailer with a car-like tractor, 2020. [341] Oskar Ljungqvist, Daniel Axehill, Henrik Pettersson, and Johan L¨ofberg. Estimation-aware model predictive path-following control for a general 2- trailer with a car-like tractor, 2020. [342] James B. Rawlings and David Q. Mayne. Model Predictive Control : Theory and Design. Nob Hill Publishing, 2nd edition, 2016. [343] Aldo Sorniotti, Phil Barber, and Stefano De Pinto. Path Tracking for Au- tomated Driving: A Tutorial on Control System Formulations and Ongoing Research, pages 71–140. Springer International Publishing, Cham, 2017. [344] Matthijs Klomp, Mats Jonasson, Leo Laine, Leon Henderson, Enrico Regolin, and Stefan Schumi. Trends in vehicle motion control for automated driving on public roads. Vehicle System Dynamics, 57(7):1028–1061, 2019.

[345] A. Bemporad. Model predictive control design: New trends and tools. In Proceedings of the 45th IEEE Conference on Decision and Control, pages 6678–6683, Dec 2006. [346] Alberto Bemporad and Manfred Morari. Robust model predictive control: A survey. In A. Garulli and A. Tesi, editors, Robustness in identiﬁcation and control, pages 207–226, London, 1999. Springer London. [347] Carlos E. Garc´ıa,David M. Prett, and Manfred Morari. Model predictive control: Theory and practice—a survey. Automatica, 25(3):335 – 348, 1989. [348] Hans Joachim Ferreau, Christian Kirches, Andreas Potschka, Hans Georg Bock, and Moritz Diehl. qpOASES: a parametric active-set algorithm for quadratic programming. Mathematical Programming Computation, 6(4):327– 363, Dec 2014. [349] B. Stellato, G. Banjac, P. Goulart, A. Bemporad, and S. Boyd. OSQP: An operator splitting solver for quadratic programs. ArXiv e-prints, November 2017.

[350] Graham C. Goodwin and Kwai Sang Sin. Dover Publications, 1984. BIBLIOGRAPHY 199

[351] Frank L. Lewis, Draguna L. Vrabie, and Vassilis L. Syrmos. Optimal Control, chapter 2 - OPTIMAL CONTROL OF DISCRETE-TIME SYSTEMS, pages 19–109. John Wiley & Sons, Ltd, 2012. [352] Rolf Findeisen and Frank Allgöwer. Computational delay in nonlinear model predictive control. IFAC Proceedings Volumes, 37(1):427 – 432, 2004. 7th International Symposium on Advanced Control of Chemical Processes (AD- CHEM 2003), Hong-Kong, 11-14 January 2004. [353] R. Findeisen and P. Varutti. Stabilizing Nonlinear Predictive Control over Nondeterministic Communication Networks, pages 167–179. Springer Berlin Heidelberg, Berlin, Heidelberg, 2009. [354] Per Sahlholm, Henrik Jansson, Magnus Ostman,¨ and Karl Henrik Johansson. Automated speed selection for heavy duty vehicles. In IAVSD, Berkeley, CA, USA, 2007. [355] Oscar Törnrothand Truls Nyberg. Design and implementation of a strategy for path tracking on autonomous heavy-duty vehicles. Master’s thesis, LinköpingUniversity, Vehicular Systems, 2018. [356] S. V. Raković,E. C. Kerrigan, K. I. Kouramas, and D. Q. Mayne. Invariant approximations of the minimal robust positively invariant set. IEEE Trans- actions on Automatic Control, 50(3):406–410, March 2005. [357] D. Q. Mayne and W. Langson. Robustifying model predictive control of constrained linear systems. Electronics Letters, 37(23):1422–1423, Nov 2001. [358] Ilya Kolmanovsky and Elmer G. Gilbert. Theory and computation of disturbance invariant sets for discrete-time linear systems,. Mathematical Problems in Engineering, 4(4):317–367, 1998. [359] A. Jadbabaie, J. Yu, and J. Hauser. Unconstrained receding-horizon control of nonlinear systems. IEEE Transactions on Automatic Control, 46(5):776–783, May 2001. [360] Alessandro De Luca, Giuseppe Oriolo, and Claude Samson. Feedback control of a nonholonomic car-like robot. In Robot motion planning and control, pages 171–253. Springer, 1998. [361] M. Herceg, M. Kvasnica, C. N. Jones, and M. Morari. Multi-parametric toolbox 3.0. In 2013 European Control Conference (ECC), pages 502–510, July 2013. [362] K. I. Kouramas, S. V. Rakovic, E. C. Kerrigan, J. C. Allwright, and D. Q. Mayne. On the minimal robust positively invariant set for linear difference inclusions. In Proceedings of the 44th IEEE Conference on Decision and Control, pages 2296–2301, 2005. 200 BIBLIOGRAPHY

[363] Rajesh Rajamani. Vehicle Dynamics and Control. Mechanical Engineering Series. 2nd edition, 2012. ISBN: 978-91-7873-580-8 TRITA-EECS-AVL-2020:38 www.kth.se