Obstacle Avoidance with Safety Guarantees Feasibility of MPC-based Steering Algorithms Master’s thesis in Signals and Systems Stephan Heinrich Department of Signals and Systems CHALMERS UNIVERSITY OF TECHNOLOGY Gothenburg, Sweden 2015 Master’s thesis 2015:11 Obstacle Avoidance with Safety Guarantees Feasibility of MPC-based Steering Algorithms Stephan Heinrich Department of Signals and Systems Mechatronics Research Group Chalmers University of Technology Gothenburg, Sweden 2015 Obstacle Avoidance with Safety Guarantees Feasibility of MPC-based Steering Algorithms Stephan Heinrich © Stephan Heinrich, 2015. Examiner: Paolo Falcone, Department of Signals and Systems Mathias Lidberg, Department of Applied Mechanics Master’s Thesis 2015:11 Department of Signals and Systems Mechatronics Research Group Chalmers University of Technology SE-412 96 Gothenburg Telephone +46 31 772 1000 Cover: Simulation vehicle performing a lane change maneuver with projections of safe states on the road Typeset in LATEX Gothenburg, Sweden 2015 iv Obstacle Avoidance with Safety Guarantees Feasibility of MPC-based Steering Algorithms Stephan Heinrich Department of Signals and Systems Department of Applied Mechanics Chalmers University of Technology Abstract Enabled by the increasing computational power available in embedded microcon- trollers, model predictive control (MPC) for automotive applications has been ex- tensively studied in the past ten years and has attracted industry, among others, for autonomous driving applications. Implementing an MPC control algorithm to determine an optimal steering action in a lane change maneuver involves iteratively solving a Finite Time Constrained Optimal Control Problem (FTCOCP). Hence, in a situation where the environment is restricted by lane boundaries and obstacles, the FTCOCP might be unfeasible because collision avoidance constraints represent strict boundaries that cannot be violated if passenger safety shall be guaranteed. Intuitively, in such problems in- feasibility is more likely to occur as the vehicle velocity increases. Furthermore, because of the problem structure, classical results from MPC theory for guarantee- ing feasibility cannot be efficiently applied. This thesis investigates methods to predict an upper bound on the vehicle’s velocity leading to feasibility guarantees for an MPC-based lane change algorithm. By ex- ploiting invariant set theory and reachability analysis tools, we focus on the problem of (offline) characterizing the set of states for which a solution to the lane change MPC problem exists for a given speed. An MPC controller is implemented in a vehicle and test data from lane change ma- neuvers ranging from 50-100 km/h is collected at the AstaZero proving grounds. The applicability of the proposed method is evaluated by comparison with experimental data. Keywords: model predictive control, vehicle, feasibility, convex optimization, obsta- cle avoidance, advanced driver assistance systems v Acknowledgements During the past year working on this thesis I had the pleasure to work with a number of amazing people that dedicated their time and effort to support me in this process. First of all I would like to thank my examiners Paolo Falcone and Mathias Lidberg for making this thesis possible and supporting me throughout this period. I don’t think I could have asked for a better team of advisers in this field. Your support in both vehicle dynamics as well as control theory let me consider and challenge ideas and concepts that I was not even aware of at the time when I started this project. Working along side you really shaped my approach work, to keep challenging existing thoughts and think outside of the box. Next I would like to thank Chris Gerdes and the DDL at Stanford. Joining their lab during the summer was an incredible time. I would not have wanted to miss the late nights coding and the sizzling hot track days together with Team Marty for anything. The DDL is full of exceptional people and being part of that for some time was a very humbling experience. I would also like to thank my friends and colleagues in the Mechatronics group as well as the Vehicle Dynamics group at Chalmers. Working together with you has been an exceptionally welcoming and supportive experience. I would also like to thank my parents and my brother for supporting me through all this time. Cheering me up when things got challenging, and always having my back while I was far away from home let me get through the ups and downs of this process and makes me feel confident for future challenges to come. This work would not have been possible without the help and support of all these exceptional people. Stephan Heinrich, Gothenburg, 11/2015 vii Contents List of Figures x List of Tables xii 1 Introduction 1 1.1 Literature Review . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2 1.2 Problem Statement . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3 1.3 Structure of the Thesis . . . . . . . . . . . . . . . . . . . . . . . . . . 4 2 Invariant Set Theory and Reachability Analysis 7 2.1 Convex Sets . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7 2.1.1 Hyperplanes & Halfspaces . . . . . . . . . . . . . . . . . . . . 7 2.1.2 Polyhedra and Polytopes . . . . . . . . . . . . . . . . . . . . . 9 2.2 Operations on Polytopes . . . . . . . . . . . . . . . . . . . . . . . . . 9 2.2.1 Convex Hull . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9 2.2.2 Vertex Enumeration . . . . . . . . . . . . . . . . . . . . . . . 9 2.2.3 Projection . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9 2.2.4 Minkowsky sum . . . . . . . . . . . . . . . . . . . . . . . . . . 10 2.3 Representations of Convex sets . . . . . . . . . . . . . . . . . . . . . 11 2.3.1 H-Representation . . . . . . . . . . . . . . . . . . . . . . . . . 11 2.3.2 V-Representation . . . . . . . . . . . . . . . . . . . . . . . . . 11 2.3.3 Properties of H- and V-Representations . . . . . . . . . . . . . 11 2.4 Invariant Sets . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12 2.4.1 Reach()-Set . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12 2.4.2 Pre()-Set . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12 2.4.3 Control Invariant Set . . . . . . . . . . . . . . . . . . . . . . . 13 2.4.4 N-Step Controllable Set . . . . . . . . . . . . . . . . . . . . . 13 2.5 Example: A Simple Discrete Integrator . . . . . . . . . . . . . . . . . 14 3 Vehicle System Modelling 17 3.1 Two Track Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18 3.2 Bicycle Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18 3.3 Tire Models . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20 3.3.1 Linear Tire Model . . . . . . . . . . . . . . . . . . . . . . . . 20 3.3.2 Brush Tire Model . . . . . . . . . . . . . . . . . . . . . . . . . 20 3.4 Linear Bicycle model . . . . . . . . . . . . . . . . . . . . . . . . . . . 21 3.5 Force Input Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22 ix Contents 3.6 Linear Time Varying Model . . . . . . . . . . . . . . . . . . . . . . . 22 3.7 Safe Handling Envelope . . . . . . . . . . . . . . . . . . . . . . . . . 23 4 Model Predictive Control 25 4.1 Receding Horizon Principle . . . . . . . . . . . . . . . . . . . . . . . . 26 4.2 Modelling of Constraints . . . . . . . . . . . . . . . . . . . . . . . . . 27 4.3 CVXgen . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29 5 Feasibility by Construction 31 5.1 Analytical Approach Using Hyperrectangle Constraints . . . . . . . . 33 5.2 Numerical Approach . . . . . . . . . . . . . . . . . . . . . . . . . . . 37 5.2.1 Reachability Calculations with MPT-Toolbox . . . . . . . . . 37 5.2.2 Inner Approximation of Polytopes . . . . . . . . . . . . . . . . 39 5.2.3 Numerical Control Invariant Sets . . . . . . . . . . . . . . . . 41 5.2.4 Numerical N-step Controllable Sets . . . . . . . . . . . . . . . 42 6 Controller Design 45 6.1 Controller Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . 45 6.1.1 Motion Prediction . . . . . . . . . . . . . . . . . . . . . . . . 46 6.1.2 System Constraints . . . . . . . . . . . . . . . . . . . . . . . . 47 6.2 Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48 6.2.1 Horizon length . . . . . . . . . . . . . . . . . . . . . . . . . . 49 6.2.2 Simulation environment . . . . . . . . . . . . . . . . . . . . . 50 7 Experimental Testing 53 7.1 Test Scenario . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53 7.2 Test Vehicle . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54 7.2.1 Testing Equipment . . . . . . . . . . . . . . . . . . . . . . . . 54 7.2.2 Vehicle Parameters . . . . . . . . . . . . . . . . . . . . . . . . 55 7.3 Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56 8 Discussion 61 9 Conclusion 65 Bibliography 65 A Analytical Projection I A.1 Fourier Motzkin Projection Algorithm . . . . . . . . . . . . . . . . . I A.2 Pre(X ) of the AFI-Model using Hypercube Constraints . . . . . . . . II A.3 One-Step Controllable Set for the AFI-Model with Hyperrectangle Constraints . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . III B Inner Approximation of Symmetric Polytopes V C Experimental Testing IX C.1 CVXgen Optimization Problem Statement . . . . . . . . . . . . . . . IX C.2 Controller Parameters . . . . . . . . . . . . . . . . . . . . . . . . . . XI x List of Figures 1.1 Vehicle approaching slower traffic on a multilane road . . . . . . . . . 1 1.2 Spatial localization and boundaries of a lane change maneuver . . . . 4 1.3 Discrete visualization of location from where feasibility of the maneu- ver can be guaranteed. Blue = high speed, green = medium speed, red = low speed. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5 2.1 A convex and a non-convex set in R2 . . . . . . . . . . . . . . . . . . 8 2.2 R2 divided into two halfspaces by a hyperplane . . . . . . . . . . . . 8 2.3 Projection of a polytope in R2 onto the x1-subspace . . . . . . . . . . 10 2.4 Minkowsky sum of two polytopes . . . . . . . . . . . . . . . . . . . . 10 2.5 Convex set in H-representation . . . . . . . . . . . . . . . . . . . . . 11 2.6 Convex set in V-representation . . . . . . . . . . . . . . . . . . . . . . 11 2.7 Evolution of the n-step controllable set for the discrete integrator . . 15 3.1 Road coordinate system . . . . . . . . . . . . . . . . . . . . . . . . . 17 3.2 Two track vehicle bodel . . . . . . . . . . . . . . . . . . . . . . . . . . 18 3.3 Bicycle model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19 3.4 Comparison between tire models . . . . . . . . . . . . . . . . . . . . . 21 3.5 Scaling of the Safe Handling Envelope for different velocities . . . . . 24 4.1 Receding horizon principle . . . . . . . . . . . . . . . . . . . . . . . . 25 5.1 Projection of feasible states for a car approaching a static obstacle . . 32 5.2 Hyperrectangle shaped inner approximation of the 1-step controllable set for the discrete integrator . . . . . . . . . . . . . . . . . . . . . . 34 5.3 Calculation of the N-step controllable set without approximation . . . 38 5.4 Symmetric road geometry for lane change predictions . . . . . . . . . 39 5.5 Inner approximation algorithm for symmetric polytopes . . . . . . . . 40 5.6 Slices of the control invariant set for 70 km/h . . . . . . . . . . . . . 41 5.7 Projections onto the lateral position and slices at β = 0, r = 0 and ψ = 0 of n-step controllable sets for a lane change maneuver with 70 km/h . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42 5.8 Slices at β = 0 of different N -step controllable set for the lane change maneuver at 70 km/h . . . . . . . . . . . . . . . . . . . . . . . . . . . 43 6.1 Visualization of the influence of the correction step on obstacle rep- resenation in the horizon . . . . . . . . . . . . . . . . . . . . . . . . . 47 xi List of Figures 6.2 Representation of the environment boundaries as system constraints at discrete locations . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48 6.3 S-Function Builder block that implements the CVXgen code . . . . . 49 6.4 Simulation results of the double lane change with different prediction- horizon lengths . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50 6.5 CarMaker Simulink Framework . . . . . . . . . . . . . . . . . . . . . 50 7.1 Double lane change track . . . . . . . . . . . . . . . . . . . . . . . . . 53 7.2 SR 60 Orbit steering robot mounted in the test vehicle . . . . . . . . 54 7.3 Network topology of the experimental setup . . . . . . . . . . . . . . 55 7.4 Brush tire approximation from measurement data . . . . . . . . . . . 56 7.5 Experimental results from the double lane change maneuver with 50 km/h with 25 m pop-up obstacle . . . . . . . . . . . . . . . . . . . . 58 7.6 Experimental results from the double lane change maneuver with 70 km/h with 30 m pop-up obstacle . . . . . . . . . . . . . . . . . . . . 59 7.7 Experimental results from the double lane change maneuver with 100 km/h with 45 m pop-up obstacle . . . . . . . . . . . . . . . . . . . . 60 8.1 Comparison of experimental results with numerical feasibility predic- tions for lane change with 70 km/h . . . . . . . . . . . . . . . . . . . 62 xii List of Tables 6.1 Specification of the prediction horizon . . . . . . . . . . . . . . . . . . 50 7.1 Double lane-change track dimensions . . . . . . . . . . . . . . . . . . 53 7.2 Volvo S60 vehicle parameters . . . . . . . . . . . . . . . . . . . . . . 56 7.3 Safe handling envelope violation during double lane change maneuvers 57 C.1 Controller parameters used in experimental testing . . . . . . . . . . XI xiii List of Tables xiv 1 Introduction With car manufacturers and tech companies running an arms race towards making the autonomous vehicle a reality, the field of vehicle motion control attempts to solve the sub-problem of using steering, braking and throttle to control the motion of the vehicle within its environment. Figure 1.1: Vehicle approaching slower traffic on a multilane road This thesis is focused on vehicle motion control in autonomous lane change scenarios as shown in figure 1.1. As a vehicle approaches slower traffic on a multilane road, the control algorithm steering the car needs to find a way to avoid the obstacle in its path. Model predictive control (MPC) has proven to be an efficient approach for these types of control problems [8]. The MPC controller studied in this thesis finds the sequence of optimal steering commands to avoid the slower vehicle and change the lane as the solution of a finite time optimal control problem. minimize: a function of steering subject to: vehicle dynamics collision avoidance lane boundaries (1.1) By using models of the vehicle dynamics and the environment, the optimization algorithm predicts an optimal path together with the necessary steering actions to drive it. It is intuitively clear for every driver that the existence of such a path to avoid a slower vehicle ahead is very much dependent on the vehicle’s own speed, the speed of the preceding vehicle as well as the dimensions of the road. For the situation depicted in figure 1.1 the controller may easily find a solution when the car is driving 20 km/h faster than the truck but it may be impossible to calculate a path to avoid the truck if it is standing still and the vehicle is travelling 130 km/h. 1 1. Introduction With model predictive control becoming more and more popular for vehicle motion control, and extensive body of work on this subject has been developed in recent years. 1.1 Literature Review In the mid 2000s Borelli, Falcone et al. [4], [11], [12] applied online model predictive control to implement steering algorithms for autonomous road vehicles. Using a Linear Time Varying (LTV) approach, they linearized the vehicle dynamics around the previous optimization results and the current velocity of the vehicle. They were able to implement real time capable controllers for path tracking with promising results. In their work they assumed a known trajectory and they succeeded in tracking predefined paths. To account for a changing environment, such as unexpected obstacles, Falcone et al. [13] introduced a hierarchical approach that allowed online replanning of the trajectories which act as a reference for the low level path following controller. This general structure is widely adopted in literature with multiple different approaches on how this top level trajectory planning problem can be solved efficiently. In their work Falcone et al. used a vehicle representation as a point mass to represent the situation as a convex quadratic programming (QP) problem that can be solved efficiently. Funke [16] et al. used a parametrizable sequence of clothoids to plan efficient tra- jectories for obstacle avoidance. Both approaches use the path curvature and the available road friction to determine an upper bound on the velocity for which the path is assumed to be feasible. In a similar approach, Gray et al. [20] used a path planning algorithm based on a set of parametrizable, predefined motion primitives that can be combined to achieve efficient trajectory calculations. Maintaining the approach of hierarchically separated path planning and path follow- ing, Gao et al. [18] used a general purpose nonlinear solver (NPSOL) to approach the path planning problem with fewer simplifications. By solving the general nonlinear problem they were able to determine lateral and longitudinal velocity of the desired path in one step. At the same time they still implemented an efficient QP-based path following algorithm as a low level steering controller. While this approach is very general, the solution of the path planning algorithm does not provide conver- gence guarantees and is limited to relatively slow controller sampling rates of 200 ms in their experiments. Such a slow path replanning frequency may not be sufficient to control vehicles travelling at highway speeds. The vehicle will only start reacting to a sudden appearance of an obstacle after it has been detected and incorporated into the solution of the path planner. At typical highway speeds of 130 km/h this means that the vehicle will travel for more than 7 m before the path planning algorithm has calculated a new solution and the steering controller can start to react to the new situation. To avoid the use of computationally expensive general purpose nonlinear solvers, Carvalho et al. [7] used an approximation based on sequential quadratic program- ming (SQP) to repetitively solve and relinearize the problem in one time step while 2 1. Introduction using an efficient QP-solver algorithm. While the computation time of this approach can be manually bounded by limiting the number of solver iterations, the approach cannot guarantee convergence either. In an alternative approach Hsu and Gerdes [24] as well as following work by Beal, Erlien and Funke [1], [2], [9], [10], [17] demonstrated a steering controller that com- bines vehicle stability boundaries and environmental boundaries in one hierarchy level to be able to avoid obstacles without a separate path replanning step. By in- troducing environmental constraints in the low level steering controller, it becomes capable of reacting directly to unexpected obstacles that suddenly appear. They showed experimental results at controller update rates up to 100 Hz. To achieve this frequency they linearize the problem around the current vehicle velocity and utilize efficient problem specific solvers for convex optimization in combination with powerful computing hardware. While there has been extensive work and great results from different model pre- dictive control approaches for vehicle motion control, the overall trade-offs between generality and accuracy of the solution on the one side and computation time on the other side still remain a challenge and the optimal approach to this problem is yet an open field of research. Trajectory planning algorithms that utilize parametrized maneuvers or nonlinear optimization can yield pre-calculated trajectories and desired velocity profiles but typically suffer a trade-off between computational complexity and a need for sim- plifications that neglect some aspects of the vehicle dynamics. Approaches that utilize convex QP-solvers have shown to be effective in line fol- lowing, vehicle stabilization and obstacle avoidance but the problem statements are linearized and parametrized by velocity. While this is not problematic for simple path tracking controllers, where the current vehicle speed provides a good approxi- mation over a relatively short prediction horizon, this property becomes more critical for controllers that consider longer horizons. These extended horizons are typically needed if road boundaries and obstacles are to be considered. For problem formu- lations of that type the velocity may not be assumed as given and the choice of the desired velocity along the horizon will determine the existence of the solution of the optimization problem. 1.2 Problem Statement For this thesis we consider a controller similar to [9] which considers both, vehicle handling limits as well as environmental boundaries in a linearized, convex prob- lem formulation. For this type of controller the problem statement (1.1) becomes parametrized by velocity. We consider the application of this controller in a highway lane change maneuver as shown in figure 1.1 where the road is restricted by solid boundaries that have to be avoided at all costs. To describe the environmental boundaries in this problem the absolute velocity of the ego vehicle and the differential velocity between the ego and the obstacle in the way can be used to estimate the location where the vehicles would collide. Assuming constant velocities we can locate where the potential collision would occur as shown in figure 1.2a. Figure 1.2b illustrates how such problem can be described by lane 3 1. Introduction boundaries and the location where the ego vehicle must have translated to the other lane can be described. Therefore this problem can be treated equally to an obstacle avoidance maneuver with a stationary object. (a) Lane change situation and projected location of the collision (b) Lane boundaries and target location Figure 1.2: Spatial localization and boundaries of a lane change maneuver Without trying to precalculate a path or attempting to solve the parametrized con- vex optimization problem of the MPC controller for multiple different velocities, we aim to determine an upper bound on the velocity such that we can guarantee that a solution to the problem exists and the vehicle can pass the obstacle. We attempt to classify the sets of vehicle states, located at discrete locations before the pro- jected point of collision, from where, for discrete constant velocities, feasibility for the controller can be guaranteed and the the vehicle can safely pass the preceding vehicle. This upper bound would allow us to classify whether the current vehicle speed is safe to attempt the lane change or if heavy braking must be considered as an alternative measure to ensure the passenger safety. Findings in [16] indicate that, for serious obstacle maneuvers where the distance to the obstacle and the vehicle speed becomes critical, full braking and consecutive steering is an adequate approach to an obstacle avoidance maneuver. Figure 1.3 visualizes the idea behind this approach. By illustrating and examining states, for which the maneuver is feasible for different, distinct velocities we we can check if the current vehicle state is feasible. For a high speed, shown in blue, the vehicle’s current state is not contained in the locations for which a feasible trajectory can be guaranteed. For lower speeds as indicated in yellow or red the feasibility for the maneuver may be guaranteed. 1.3 Structure of the Thesis In the following three chapters the necessary methods and tools used in this work are introduced. An overview of the necessary basics in invariant set theory and reachability analysis is provided in chapter 2. In chapter 3 the vehicle models used to mathematically describe the motion of the vehicle for simulation and control 4 1. Introduction Figure 1.3: Discrete visualization of location from where feasibility of the maneuver can be guaranteed. Blue = high speed, green = medium speed, red = low speed. design are presented. Chapter 4 introduces the fundamentals of model predictive control used in this thesis and explains some of the challenges and restrictions of using convex optimization in online MPC which completes the introductory topics. Chapter 5 provides the core study of the problem at hand. Different approaches are examined and the challenges involved are illustrated. An approach for a solution that allows the use of this concept is proposed. To gather comparison data, chapter 6 describes the design and implementation of a model predictive controller that is capable of controlling the vehicle in a lane change maneuver. Experimental testing procedures and data from real world autonomous lane change maneuvers are illustrated in chapter 7 for comparison with the theoretical predic- tions. The closing chapters 8 and 9 provide a comparison between the experimental data and the theoretical calculations derived in chapter 5 and a discussion on the findings of the thesis. 5 1. Introduction 6 2 Invariant Set Theory and Reachability Analysis Invariant set theory and reachability analysis are a powerful tools to mathematically describe and evaluate possible trajectories of dynamic systems within their state- space. This chapter provides an introduction to the methods from these fields that are used to evaluate the feasibility of the highway lane change maneuver. For a detailed background on the computational geometry involved, the reader is referred to G. Ziegler’s Lectures on Polytopes [30]. A background on the applications of these general concepts in the field of constrained optimization and control is presented in Constrained Optimal Control for Linear and Hybrid Systems by F. Borrelli [5]. The following definitions provide a condensed excerpt from [5] and [30] of the most important concepts used in this work. For a further background a survey of applications of invariant set theory in control can be found in [3]. 2.1 Convex Sets A Set S ∈ Rn is called convex if λz1 + (1− λ)z2 ∈ S for all z1, z2 ∈ S, λ ∈ [0, 1]. (2.1) This can be interpreted as that any line that connecting any two points in the set is also contained in the set. This concept is visualized in figure 2.1. 2.1.1 Hyperplanes & Halfspaces A Hyperplane is described by a vector a ∈ Rn, a 6= ∅ and a scalar b ∈ R and is formed by S = { x ∈ Rn | aTx = b } . (2.2) Geometrically a hyperplane can be interpreted as the set of points orthogonal to a normal vector with a fixed offset from the origin. The set of points located on either side of a hyperplane can be described by a halfspace. A halfspace is defined by a linear inequality in the form S = { x ∈ Rn | aTx ≤ b } . (2.3) Both hyperplanes as well as halfspaces are convex sets. Figure 2.2 illustrates a hyperplane and the two halfspaces created by it in 2 dimensions. 7 2. Invariant Set Theory and Reachability Analysis (a) Convex set (b) Non-convex set Figure 2.1: A convex and a non-convex set in R2 (a) Straight line illustrating a hyper- plane in R2 (b) Halfspace in R2 Figure 2.2: R2 divided into two halfspaces by a hyperplane 8 2. Invariant Set Theory and Reachability Analysis 2.1.2 Polyhedra and Polytopes Polytopes and polyhedra are geometrical objects with flat sides. They are sets that can be described by the intersection of a finite number of halfspaces. This thesis follows the definitions from Ziegler [30]. A polytope is a bounded polyhedron which means that it does not contain any ray of the type {x + ty | t ≥ 0} for any y 6= 0. This means its outer bound is defined in all directions and it has a finite volume in Rn. 2.2 Operations on Polytopes This section introduces basic operations on Polytopes. 2.2.1 Convex Hull The tightest polytope that encloses a set of points is called its convex hull. For a set of points V = {V i}NVi=1 with V i ∈ Rn it is defined as conv(V ) = x ∈ Rn | x = NV∑ i=1 αiV i, 0 ≤ αi ≤ 1, NV∑ i=1 αi = 1  . (2.4) 2.2.2 Vertex Enumeration Vertex enumeration is the dual operation of the convex hull. If the hyperplanes bounding a polytope are known, vertex enumeration is the process used to calcu- late the extreme points of a polytope. Vertex enumeration is typically an expensive operation as the calculation is exponential in the the number of facets and com- mon algorithms to solve the vertex enumeration problem are the double description method or reverse search [5]. Given a polytope P the set of its vertices V is calculated using the vertex enumer- ation operation V = {Vi}NVi=1 = vert(P). (2.5) 2.2.3 Projection By projecting a polytope P ∈ Rn+m , P = { [x′ y′]′ ∈ Rn+m | P xx+ P yy ≤ P c } ⊂ Rn+m (2.6) onto a lower dimensional subspace Rn, a new lower dimensional polytope projx(P) = {x ∈ Rn | ∃ y ∈ Rm | P xx+ P yy ≤ P c} ⊂ Rn (2.7) is decribed. Figure 2.3 illustrates the concept of a projection geometrically from 2 dimensions to one dimension. The projection includes all values in R1 for which the original 9 2. Invariant Set Theory and Reachability Analysis polytope P is defined in R2. This idea can be generalized to arbitrary dimensions. The projection describes the set of states in the remaining dimensions for which the set was defined in any location of the dimension that is being removed in the projection. Figure 2.3: Projection of a polytope in R2 onto the x1-subspace 2.2.4 Minkowsky sum The Minkowsky sum is an operation on two polytopes P and Q that forms another polytope. The sum is the set that contains all possible combinations of sums of the points p ∈ P and q ∈ Q. The concept is visualized in figure 2.4. P ⊕Q := {p+ q ∈ Rn | p ∈ P , q ∈ Q} (2.8) Figure 2.4: Minkowsky sum of two polytopes 10 2. Invariant Set Theory and Reachability Analysis 2.3 Representations of Convex sets 2.3.1 H-Representation For the computational representation of sets, two forms of notations are used. The H-representation of a polyhedron describes the intersection of a finite number of closed halfspaces in Rn P = {x ∈ Rn | Ax ≤ b} . (2.9) The set of inequalities Ax ≤ b describes a finite number of halfspaces bounding the polytope in Rn. Figure 2.5: Convex set in H-representation 2.3.2 V-Representation An alternative notation to describe a bounded polyhedron is the vertex represen- tation. By describing the location its extremal points a polytope can be uniquely identified. A V-polytope is generated by the convex hull of a finite set of points V = {Vi}Nvi=1, where the convex hull is the smallest convex set containing all the points. P = conv(V ) (2.10) Figure 2.6: Convex set in V-representation 2.3.3 Properties of H- and V-Representations Any polytope in H-representation can be represented by a V-representation and vice versa. For a mathematical proof the reader is referred to [30]. The H-representation of a polytope can be calculated from a V-polytope using the convex hull operation. To move from a H-representation to a V-representation the vertex enumeration operation is used. 11 2. Invariant Set Theory and Reachability Analysis BothH- and V-polytopes can contain redundancies. A polytope inH-representation in Rn bounded by m halfspaces is said to be in minimal representation if the removal of any row [ a(i,1), ..., a(i,n) ] x ≤ bi, i = 1, ...,m (2.11) would change the polytope. Similarly a V-polytope is said to be in minimal representation if the removal of any point Vj would change it. A minimal V-polytope is described by the set of its vertices. A non-minimal representation can contain an arbitrary number of points within the interior of the polytope. 2.4 Invariant Sets This section introduces a number of set definitions in the context of discrete time linear systems. Denote by f() the state update function of a discrete time linear system with subject to external inputs u(k). x(k + 1) = f(x(k), u(k)) = Ax(k) +Bu(k) (2.12) System states and inputs are subject to the constraints x(k) ∈ X , u(k) ∈ U (2.13) which are described by the convex polytopes X , {x |Hx ≤ h} U , {u |Huu ≤ hu} . (2.14) The system is assumed to be n-dimensional in state and subject to m-inputs. 2.4.1 Reach()-Set The one step reachable set for the system (2.12) describes the set of states that can be reached by the function f() under some input u ∈ U . It is defined for a set of initial states S and described by: Reach(S) , {x ∈ Rn | ∃ x(0) ∈ S, ∃ u(0) ∈ U s.t. x = f(x(0), u(0))} (2.15) 2.4.2 Pre()-Set As the dual of the one step reachable set, Pre() sets describe the set of the system (2.12) that can evolve to a target set S in one step under some input u ∈ U . Pre(S) , {x ∈ Rn | ∃u ∈ U s.t. f(x, u) ⊆ S} (2.16) The algorithm presented in [5] to calculate the Pre() for a non-autonomous system is based on the description of the state and input constraints by the polytopes X and U in H-representation. 12 2. Invariant Set Theory and Reachability Analysis Pre(S) is calculated by a linear mapping of the state constraints through the system dynamics. Pre(S) , {x ∈ Rn | ∃ u ∈ U s.t. f(x, u) ∈ S} , { x ∈ Rn | ∃ u ∈ Rm s.t. [ HA HB 0 Hu ] [ x u ] ≤ [ h hu ] } (2.17) This definition yields a resulting polytope in the n + m-dimensional state-input space. To describe Pre(X ) in the n-dimensional state space a projection operation as described in section 2.2.3 can be used. 2.4.3 Control Invariant Set Generalizing this idea, we can describe a set of states C for which there exist an input u ∈ U such that the system can remain within C ⊆ X for all future states. The set C ⊆ X is characterized as a control invariant set for the system (2.12) if and only if C , {xk ∈ Rn | ∃ uk ∈ U , xk+1 ∈ C} (2.18) The set that contains all invariant sets for a system (2.12) subject to the constraints (2.13) is called maximal control invariant set C∞. The following algorithm provided in [5] shows the calculation for the maximal control invariant set: Data: State Update Function f(x, u), Set of State Constraints X , Set of Input Constraints U Result: C∞ k = 0 ; Ω0 = X ; Ωk+1 = Pre(Ωk + 1) ∩ Ωk ; while Ωk+1 6= Ωk do k = k + 1 ; Ωk+1 = Pre(Ωk) ∩ Ωk; end C∞ = Ωk+1; Algorithm 1: Calculation of the Maximal Control Invariant Set 2.4.4 N-Step Controllable Set For an arbitrary target set O ⊆ X the set of states that can be driven to an arbitrary target set O in N -steps is called the N-step controllable set KN(O). It is defined by: KN(O) , { x0 ∈ Rn | ∃ {uk ∈ U}N−1 0 , {xk ∈ X}N−1 0 , xN ∈ O } (2.19) This set is computed in a very similar way as the maximal control invariant set by iteratively calculating the Pre() and determining its intersection with the state 13 2. Invariant Set Theory and Reachability Analysis constraints. Algorithm 2 presets the procedure to calculate the N-step controllable Sets for a linear discrete time system. Data: Target Set O, Number of Iterations N , State Update Function f = (A,B), State Constraints X , System Input Constraints U Result: KN(O) K0(O) = O ; for k ∈ [0, 1, 2, ..., N ] do Kk+1(O) = Pre(Kk) ∩ X end Algorithm 2: Calculation of the N-Step Controllable Set 2.5 Example: A Simple Discrete Integrator The meaning of these definitions can be illustrated by a small example in two dimen- sions. Consider a discrete time integrator system with a sampling time of ts = 0.1 s defined by [ x1(k + 1) x2(k + 1) ] = [ 1 ts 0 1 ] [ x1(k) x2(k) ] + [ 0 ts ] u(k) (2.20) subject to the state and input constraints X , x ∈ R2 |  1 0 −1 0 0 0 0 −1  [ x1(k) x2(k) ] ≤  1 1 1 1   U , { u(k) ∈ R | [ 1 −1 ] u(k) ≤ [ 1 1 ]} . (2.21) Using algorithm 2 we can calculate the N -step controllable set for the target set X . For the resulting set of states we can guarantee that the future trajectory can remain within the state constraints X for N -steps while using control action contained in U . Iterating this procedure for a finite number of steps yields an outer approximation of the maximal control invariant set introduced in section 2.4.3. The results of this calculation are shown in figure 2.7. The one-step maximal control- lable set is only slightly reshaped compared with the original state constraints. Two additional hyperplanes were added to introduce tighter bounds on the polytope. Af- ter multiple iteration steps the shape of the set becomes skewed. The approximation becomes round shapes and the number of hyperplanes increases significantly. The shape can be intuitively explained by examining the system equations (2.20). Consider the case when the derivative state x2 = 1. Due to the input constraints for each discrete step x2 can only decrease by 0.1. This means to avoid that the state x1 to violates the constraints in the future we can calculate how much x1 will change over the period when x2 is driven from one to zero to avoid a further increase of x1. 14 2. Invariant Set Theory and Reachability Analysis (a) 1-step controllable set (b) 20-step controllable set Figure 2.7: Evolution of the n-step controllable set for the discrete integrator Using this approach we can derive that x1 ≤ 1− 0.1 ∗ (1 + 0.9 + 0.8 + ...+ 0.1) = 0.45. (2.22) Therefore x1 may not be larger than 0.45 if x2 = 1. Assume we would attempt to formulate a controller that ensures that the state and input constraints of the system are never violated. It is clear that there exists no solution for such a controller if the current state of the system was outside the maximum control invariant set. The existence of a feasible control law to solve the problem depends on the initial state of the system. 15 2. Invariant Set Theory and Reachability Analysis 16 3 Vehicle System Modelling In this chapter we introduce the dynamic models of the vehicle used in this work. We describe the motion in the vehicle reference frame in terms of longitudinal and lateral velocities u and v and the yaw rate r. In this thesis we describe the vehicle’s the position on a straight multilane road. The coordinate s describes the position along the road, e denotes the the lateral offset from the center of a desired lane and ψ measures the heading error relative to the road orientation as illustrated in figure 3.1. Figure 3.1: Road coordinate system Assuming small angles for the vehicle heading angle ψ and u � v we can describe the motion of the vehicle in this coordinate system by ψ̇ =r ė = sin(ψ) u+ cos(ψ)v ≈ ψ u+ v ṡ = cos(ψ)u− sin(ψ)v ≈ u− ψ v ≈ u. (3.1) For each position along the road s we define lane boundaries emin(s) ≤ e(s) ≤ emax(s) (3.2) limiting the available lateral space on the road. The vehicle is subject to longitudinal forces Fx as well as lateral forces Fy. The normal tire forces are denoted by Fz. The vehicle is characterized by its mass m and its moment of inertia Iz. The wheelbase is denoted by l and the distances from front and rear axle to the location of the center of gravity are denoted by a and b and the track width is d. The steering angle of the front wheels is δ and the tire slip angles are α. 17 3. Vehicle System Modelling 3.1 Two Track Model The two track model is a general model to describe the planar motion of a vehicle. It takes into account the forces acting on the vehicle on the four individual wheels. It is derived from first principles by calculating the force balances for lateral and longitudinal acceleration and a moment balance for the yaw rotation as shown in figure 3.2. The interaction with the road of each of the individual tires is modelled by a separate forces {Fxi}4 i=1 and {Fyi}4 i=1 oriented longitudinal and lateral to the tire’s heading direction. The equations for the two track model are shown in (3.3) m (u̇− rv) = cos δ (Fx1 + Fx2)− sin δ (Fy1 + Fy2) + Fx3 + Fx4 m (v̇ + ru) = cos δ (Fy1 + Fy2) + sin δ (Fx1 + Fx2) + Fy3 + Fy4 Iz ṙ =a (cos δ (Fy1 + Fy2) + sin δ (Fx1 + Fx2))− b (Fy3 + Fy4) + d (sin δ (Fy1 − Fy2) + cos δ (Fx2 − Fx1)− Fx3 + Fx4) . (3.3) The equations (3.3) are highly nonlinear due to the trigonometric functions and the multiplicative coupling between the states. While this model is well suited for simulation applications, simpler models are better suited for control design. Figure 3.2: Two track vehicle bodel 3.2 Bicycle Model To formulate the model predictive control problem as a convex optimization problem, for which efficient solver exist, a less complex and linearized version of the vehicle model is necessary. The equations in (3.3) can be simplified by lumping the left and right tire forces together as illustrated in figure 3.3. 18 3. Vehicle System Modelling Fyf =Fy1 + Fy2 Fyr =Fy3 + Fy4 (3.4) Furthermore we assume small angles for the steering angle sin δ =δ cos δ =1 . (3.5) Combining equations (3.3) and using the assumptions (3.4) and (3.5) we can derive the equations of motion for the planar rigid one track model, commonly called the bicycle model u̇ =Fxf + Fxr − δFyf m + rv v̇ =Fyf + Fyr + δFxf m − ru ṙ =aFyf − bFyr Iz . (3.6) Figure 3.3: Bicycle model We can describe the side slip angle β of the vehicle at its center of gravity and its small angle approximation by β = tan−1 ( v u ) ≈ v u . (3.7) For the bicycle model in figure 3.3 we can denote the front and rear tire slip angles by αf =arctan ( v + ar u ) − δ ≈ β + a u r − δ αr =arctan ( v − br u ) ≈ β − b u r. (3.8) 19 3. Vehicle System Modelling 3.3 Tire Models Since the entire interaction of a vehicle with the road is based on the distribution of forces through four small tire contact patches, each approximately the size of a hand, the modelling of this interaction is crucial to the modeling of the vehicle motion as a whole. Pacejka [28] describes several state-of-the-art tire models. In this thesis two different tire models are used. Due to the necessity of analytical calculation and the restriction to linear model formulations for control design, this thesis makes use of simple parameterizable tire models. 3.3.1 Linear Tire Model The linear tire model is such a model that assumes a simple linear relationship between tire side slip angle and the generation of lateral tire force. This model is fairly accurate for small slip angles but the accuracy quickly decreases as the lateral force reaches approximately half its maximum value as shown in figure 3.4. The tire model is simply characterized by the so called cornering stiffness Fy = −Cαα. (3.9) 3.3.2 Brush Tire Model The more complex brush tire model is an approximation of the tire forces under the assumption that the contact patch of the tire is subject to a parabolic vertical load distribution along the contact patch. Due to the slip angle of the tire the lateral deflection of the tire is larger towards the rear of the contact patch. Thus individual parts of the contact patch can be either in adhesive or sliding friction. The brush tire model used in this thesis is similar to the the one proposed by Fiala [15] and was presented by Fromm in [21] and used by Hindiyeh and Gerdes in [2] as well as in multiple following pieces of work e.g. by Erlien and Gerdes in [9]. The advantage of the brush tire model is that it gives a more accurate description of the vehicle tires behavior by modelling the saturation of the lateral tire force depending on the road friction coefficient. At the same time the brush tire model sill only requires two parameters which can easily be identified from experimental data. The saturation of tire forces is modelled as shown in equation (3.10) and can be applied to individual tires as well as lumped front or rear axles Fy = −Cα tanα + C2 α 3µFz | tanα| tanα− C3 α 27µ2F 2 z tanα3 , |α| ≤ αsl −µFzsgnα , |α| > αsl. (3.10) The lateral force is saturated at its peak value for slip angles where the entire contact patch is sliding [23]. This value can be stated analytically and results in αsl = arctan3µFz Cα . (3.11) 20 3. Vehicle System Modelling While more advanced tire models can capture additional phenomena, such as re- duced friction for high tire slip angles or the coupling of longitudinal and lateral tire forces, the models used in this thesis disregards these effects. Since the controller in this thesis attempts to control the vehicle only in the region up to the peak friction value, accounting for these effect is not necessary in this context. An example tire curve from the brush tire model in comparison with the linear model and a more complex Magic Tire Formula is shown in figure 3.4. The Magic Tire Formula and the brush tire model have matching peak friction values. All three models are parametrized with the same cornering stiffness. The figure shows how, in addition to the saturation effect, the Magic Tire Formula also accounts for the decrease in lateral force past when the tire starts sliding. −25 −20 −15 −10 −5 0 5 10 15 20 25 −1 0 1 ·104 Slip Angle [°] La te ra lF or ce [N ] Linear tire model Brush tire model Magic Tire Formula Figure 3.4: Comparison between tire models 3.4 Linear Bicycle model During straight line vehicle motion we can assume u = u δ = 0 v = 0 r = 0. (3.12) By linearizing the equations of motion of the bicycle model (3.6) for small deviation around the constant velocity u and conditions for straight line motion (3.12) we can derive the linear bicycle model v̇ =Fyf + Fyr m − ru ṙ =aFyf − bFyr Iz . (3.13) 21 3. Vehicle System Modelling Using the approximation of the vehicle side slip from equation (3.7), (3.6) can be stated in terms of sidslip and yaw rate as β̇ =Fyf + Fyr mu − r ṙ =aFyf − bFyr Iz . (3.14) 3.5 Force Input Model By combining a linear rear tire model (3.9) and the approximation for the rear slip angle (3.8) we can state the rear tire force as a function of the vehicle states β and r as Fyr = −Cαrαr = −Cαr ( β − b u r ) . (3.15) By replacing the rear tire force in the linear bicycle model (3.14) with equation (3.15), the lateral vehicle dynamics are derived as a function of the lateral force generated by front steering Fyf as β̇ = −Cαr mu β + ( bCαr mu2 − 1 ) r + 1 mu Fyf ṙ = bCαr Izz β − b2Cαr Izzu r + a Izz Fyf . (3.16) Since the effects from the front tire enter the dynamics of the bicycle model simply as a lateral force input, the non-linearity of the front tire can be captured outside the dynamic equations. This is not possible for the rear tire as the rear tire forces are directly coupled with the vehicle states. In state space formulation the equations of motion of the linear force input model (3.16) can be stated as[ β̇ ṙ ] = [ −Cαr mu bCαr mu2 − 1 bCαr Izz − b2Cαr Izzu ] [ β r ] + [ 1 mu a Izz ] Fyf . (3.17) 3.6 Linear Time Varying Model To obtain a better approximation of the nonlinear rear tire properties illustrated in figure 3.4 in dynamic maneuvers, the rear tire force can be linearized around the current rear slip angle instead of the origin as shown in model (3.17). Using a first order Taylor expansion of the brush tire model 3.10, the lateral rear tire force Fyr at a specific operating point αr is described by a current lateral tire force F yr and a local cornering stiffness C̃αr. Fyr = F yr + C̃αr (αr − αr) (3.18) 22 3. Vehicle System Modelling By replacing the rear tire model (3.15) with (3.18) in the linear bicycle model (3.14) the model (3.17) can be modified to become [ β̇ ṙ ] = − C̃αr mu bC̃αr mu2 − 1 bC̃αr Izz − b2C̃αr Izzu  [β r ] + [ 1 mu a Izz ] Fyf +  (F yr+C̃αrαr) mu −b(F yr+C̃αrαr) Izz  . (3.19) Using this technique, the model is able to account for the saturation effects in- corporated in the brush tire model. This approach can be used for control design when the current rear sideslip can be measured from high precision sensors to in- crease model accuracy over a short prediction horizon. Therefore the time varying model (3.19) yields better accuracy in dynamic maneuvers than the linear force in- put model in equation (3.17) and still fulfils the requirements imposed to be used in efficient optimization algorithms. The model was used by Beal and Gerdes [1] for model predictive control applications at the limits of handling. However it can only be used when a good estimate for the rear side slip angle is available. For the feasibility predictions derived in this work this model cannot be used since we don’t have any knowledge about the future vehicle motion and therefore are forced to used the LTV model with the rear tire linearization around the origin as presented in equation (3.17) 3.7 Safe Handling Envelope To ensure handling stability for the vehicle models 3.17 and 3.19 yaw rate and sideslip must be bounded. For excessive values of yaw rate and sideslip a vehicle can become unstable and spin out [1]. Hsu and Gerdes [24] showed an approach that uses model predictive control in combination with a safe handling envelope that imposes limits on yaw rate and side slip that can help a vehicle avoid approaching these situations before they occur. As a limit on yaw rate they use the maximum possible steady state yaw rate rmax based on the vehicle tire friction µ. Using the linear bicycle model from equation (3.14) and setting β̇ = 0 and ṙ = 0 we can solve for the yaw rate limits |r| ≤ rmax =  Fyrmax (1+b/a) mu | Fyfmax ≥ b a Fyrmax Fyrmax (1+a/b) mu | Fyfmax < b a Fyrmax . (3.20) Using the description for the rear slip angle αr = β− b u r, a natural limit for the side slip arises. By using equation (3.11) and limiting the sideslip to the value where the rear tire reaches its maximum force αr ≤ αrsl the side slip bound becomes b u r − αrsl ≤ β ≤ b u r + αrsl . (3.21) Both bounds on this safe handling envelope scale naturally with the longitudinal velocity u which makes them applicable over a large range of velocities. In their work Beal and Gerdes showed that for any point on the safe handling envelope a controller is capable of keeping the vehicle within these boundaries. These constraints describe 23 3. Vehicle System Modelling −10 0 10 −40 −20 0 20 40 Side slip β [deg] Ya w R at e r [d eg /s ] 50 km/h 80 km/h 100 km/h Figure 3.5: Scaling of the Safe Handling Envelope for different velocities an invariant set for yaw rate and side slip that will be used in further control design and reachability studies in this work. 24 4 Model Predictive Control This chapter introduces the basic concepts of model predictive control to illustrate some challenges for utilizing MPC as a real time control method for vehicle motion control. This chapter uses notations and derivation from [5]. Model predictive control is an optimal control approach typically aimed at solving control problems in the presence of constraints. These can be actuator constraints such as limited steering angles or steering rates in a car or constraints that describe requirements on the system state. This type of constraint can for example be a limit on a maximum allowed velocity in some direction or a formulation of road boundaries that shall not be crossed. By minimizing or maximizing a cost function the optimal inputs to the system can be determined by solving an optimization problem. Depending on the size of the problem the solution of such optimization problem is a computationally expensive process. The approach became popular in the process industry in the 1980s. Model predic- tive control provided the possibility to solve complex multi-variable control problems that were earlier difficult to handle. As these system typically involved very slow dynamics, it was sufficient to adjust input over the period of minutes and model pre- dictive control schemes could be applied with the limited computing power available [26]. Figure 4.1: Receding horizon principle 25 4. Model Predictive Control Enabled by the increasing computing power available in embedded systems, model predictive control for vehicle motion became a vibrant field of research in the past 10 years [8]. 4.1 Receding Horizon Principle The general idea of receding horizon control consists of repetitively solving a finite time optimal control problem as illustrated in figure 4.1. The behavior of the system is predicted over a discrete time horizon using a mathematical model of the system. From the resulting optimal input sequence, the first element is applied as the next input to the system. After the sampling time ts the optimal control problem is solved again based on the updated measurements and the new prediction horizon which is shifted one step with respect to the previous calculation [26]. A receding horizon controller can be formulated for any type of nonlinear, time- invariant system x(k + 1) = g (x(k), u(k)) (4.1) subject to a set of constraints x ∈ X u ∈ U (4.2) in the form of a nonlinear programming problem [5]. Given a discrete time horizon length N and the vector of future inputs U0→N , [ u′0, ....u ′ N−1 ] (4.3) we can define an objective function J0→N (x0, U0→N) , p(xN) + N−1∑ k=0 q(xk, uk) (4.4) that consists of stage costs q (xk, uk) for each time step of the prediction horizon and a terminal cost p(xN). We can formulate a general finite time optimal control problem for a current state x(0) and the desired set of terminal set Xf as J∗0→N = min: U0→N J0→N (x(0), U0→N) subj. to: xk+1 = g(xk, uk), k = 0, ..., N − 1 h (x(t), u(t)) ≤ 0, k = 0, ..., N − 1 uk ∈ U , k = 0, ..., N − 1 xk ∈ X , k = 0, ..., N − 1 xN ∈ Xf x0 = x(0). (4.5) The application of this control approach in a system with hard real time constraints requires the solution of the optimization problem in finite time. This cannot be 26 4. Model Predictive Control guaranteed for an arbitrary nonlinear program as shown in (4.5). While there have been applications of general purpose nonlinear solver algorithms in automotive ap- plications in recent years, they cannot provide convergence guarantees and have so far been limited to relatively large sampling times due to high computational complexity [18]. The majority of model predictive control approaches for vehicle motion control in the past years has made use of efficient solver algorithms for convex optimization problems [1], [2], [7], [9], [10]. These algorithms have the advantage of efficiently solving large scale optimization problems. On the downside they require the user to simplify the general problem (4.5) and approximate it such that the problem formulation can be represented by a linear system with a convex cost function and affine constraints. To fulfil these requirements the nonlinear system in (4.1) must be linearized such that it can be represented in the form x(t+ 1) = Ax(t) +Bu(t) (4.6) subject to the state and input constraints X = {x | Hx ≤ h} U = {u | Huu ≤ hu} . (4.7) For the vehicle models introduced in chapter 3 we showed how the nonlinear model (3.6) can be linearized as shown in (3.17) to fit these requirements. The general objective function (4.4) can replaced by a quadratic version J0 (x0, U0→N) , x′NPxN + N−1∑ k=0 x′kQxk + u′kRuk (4.8) to ensure that the cost function is convex. The terms Q and R are positive semidef- inite matrices that assign cost to individual states and inputs over the prediction horizon. The positive semidefinite matrix P assigns the terminal cost to the final state. This allows to describe the optimal control problem as a quadratic program J∗0 = min: U0→N J0 (x(0), U0→N)) subj. to: xk+1 = Axk +Buk, k = 0, ..., N − 1 Hkxk ≤ hk, k = 1, ..., N Hukxk ≤ huk , k = 0, ..., N − 1 x0 = x(0) (4.9) for which efficient algorithms exist that can be used in real time constrained optimal control. 4.2 Modelling of Constraints Solving a finite time optimal control problem (4.9) for a linear system (4.6) subject to state and input constraints (4.7) there may exist initial conditions x0 ∈ X such 27 4. Model Predictive Control that there exist no solutions to the problem without violating either input or state constraints. Considering the simple discrete integrator example introduced in section 2.5, it is easy to find an initial state x0 such that there does not exist any feasible trajectory for a controller to keep the system states within X for a finite length horizon. For the initial state x0 = [1 1]′ there exist no allowed control action u ∈ U such that the state x remains in X in the next time step. If an MPC controller is faced with this situation the outcome resulting behavior is dependent on the actual implementation of the solver used. Since no valid result can be calculated it becomes unclear what control action should be taken. Considering a car heading for an obstacle at highway speed this certainly presents a situation that should be avoided. A common way of avoiding infeasibility is to formulate state constraints as soft con- straints [6]. Additional slack variables λi are introduced in the constraint inequalities that are penalized extensively in the cost function if they becomes non-zero. While input constraints in real systems are usually limited by physical constraints, state constraints often reflect desired behavior. Therefore is may be acceptable in many situations that it is acceptable that the state constraints can be slightly violated to allow the existence of a valid solution. Using soft constraints the optimization problem (4.9) can be formulated as J∗0 = min: U0→N J0(x(0), U0→N) + l(λ) subj. to: xk+1 = Axk +Buk, k = 0, ..., N − 1 Hkxk ≤ hk + λ, k = 1, ..., N Hukxk ≤ huk , k = 0, ..., N − 1 x0 = x(0) λ ≥ 0 (4.10) with the additional cost term l(λ) = 0 , λ = 0 � J0(x(0), U0) , λ 6= 0. (4.11) If l(λi) is much larger than the original cost function for all values λ 6= 0 then the optimal solution remains the same as long as there exists a feasible solution to the problem. If the problem becomes infeasible the solution will be the optimal trajectory to bring the system back into the desired region of the state space. While in many control problems state constraints are often desired values rather than fixed physical limits this is a very useful strategy. A disadvantage of this strategy however is, the increased complexity of the problem due to the additional variables. Also the introduction of cost terms that are several orders of magnitude larger than the original cost function J , issues with numerical accuracy and convergence may arise and the system can become ill-conditioned. In the lane change problem introduced in section 1.2 infeasibility can arise because the velocity sequence u1,...N−1 chosen to linearize the vehicle model 3.17 for the indi- vidual steps of the prediction horizon is too high. To avoid these cases of infeasibility 28 4. Model Predictive Control the velocity profile for linearization should be chosen such that infeasibility does not occur. 4.3 CVXgen To solve convex optimization problem as illustrated in equation (4.9) CVXgen is used in this work. CVXgen is a code generator that can generate fast custom C-code for solving convex optimization problems [27]. It provides an online interface to specify the optimization problem and generates C-code that can be implemented into any embedded system that is capable of compiling C-code. The code is optimized for fast calculations and is almost branch free to allow consistent and predictable execution times. The simple QP-problem in equation (4.9) can be described in CVXgen as the fol- lowing. minimize quad(x[N], P) + sum[k=1..N-1](quad(x[k], Q) + quad(u[k-1], R)) subject to x[k+1] == A*x[k] + B*u[k], k=0..N-1 # dynamics constraints. H[k]*x[k] <= h[k], k=1..N # state constraints H_u[k]*u[k] <= h_u[k], k=0..N-1 # input constraints x[0] == x0 end 29 4. Model Predictive Control 30 5 Feasibility by Construction As introduced in chapter 1.2 we consider lane change maneuver on a multilane road. The moving obstacle is projected onto a fixed position on the road according to the differential velocity between the ego vehicle and the preceding vehicle and and treated as a static obstacle on the road at the location where the ego vehicle must be in the other lane to avoid a collision. An MPC-controller (see section 4.1) attempts to solves a finite time optimal control problem (4.9) to calculate a sequence of optimal steering inputs and a trajectory such that the vehicle (??) can be steered to safely avoid the obstacle ahead of it. The controller minimizes a cost function (4.8) that penalizes deviation from the desire lane as well as violent steering maneuvers to ensure smooth motion of the the vehicle if possible. The optimal control action is subject to the dynamics of the vehicle model (??), environmental constraints due to road boundaries and obstacle location (see section ??) as well as tire force limitations (3.10) incorporated into the handling limits as described in section 3.7. The finite time optimal control problem is stated as minimize: Cost function (4.8) (5.1a) subject to: Vehicle dynamics(3.19) (5.1b) Safe handling envelope (3.20) , (3.21) (5.1c) Environmental boundaries (3.2) (5.1d) Designing a steering control algorithm (5.1) where the path is restricted by solid obstacles and lane boundaries poses a control problem where infeasibility may occur. As shown in section 2.5 there may not exist a solution to the problem for some initial conditions. The existence of the solution is also dependent on the velocity used to parametrize the vehicle dynamics (3.19). The problem (5.1) is time varying and standard results to guarantee feasibility from literature do not apply. Therefore this chapter aims at using tools introduced in chapter 2 to guarantee persistent feasibility for the problem (5.1). Given a certain distance to an obstacle, fixed lane boundaries and vehicle capabilities and the state of the vehicle, there exists an upper bound on the velocity for which feasibility of the problem (5.1) can be ensured. In [14] Falcone et al. proposed an algorithm for model based threat assessment by using methods from reachability analysis and set invariance theory. By targeting 31 5. Feasibility by Construction a set of safe states within their lane, they predicted threats for lane crossing and vehicle instability over a finite time horizon. Using a similar approach the feasibility of the constrained model optimal control problem (5.1) can be studied. By calculating a control invariant set C (algorithm 1 in section 2.4.3) for the vehicle model (3.17) subject to the same constraints as in (5.1) we predict the vehicle states that ensure persistent feasibility and safe driving conditions for future states within one lane of the road. If an obstacle is blocking one lane of the road we use this control invariant set within a single lane C as a target set located in the open lane as illustrated in figure 5.1. Using algorithm 2 (section 2.4.4) n-step controllable sets K1,...(C) are determined that guarantee that for all states contained in these sets there exists a sequence of inputs such that the state can be driven to the the target set C in the safe lane. With the linear vehicle model (3.17) and road coordinates (3.1), the vehicle motion can be predicted in the state space X = [β , r, ψ, e]′ by the discrete, parametrized motion model for discrete steps in s in the form X(k + 1) = A(u)x(k) +B(u)Fyf . (5.2) Given the longitudinal velocity of the vehicle u and the distance to the obstacle ∆s in s direction, the number of steps for the controller until the obstacle is reached is given by N = ∆s/u. With the motion model 5.2 being parametrized by the velocity u the control invariant set C(u) and the n-step controllable sets KN(C, u) both become functions of the velocity. If the state x0 of the vehicle is contained in the N-step controllable set KN(C, u) for a fixed velocity u, then persistent feasibility for the maneuver can be guaranteed for that velocity. This concept is illustrated in figure 5.1. The checkered line depicts the location of the control invariant set C that guarantees persistent feasibility for future steps. The green lines illustrate locations of the n-step controllable sets K1,...(C). While the image can only show projections of the sets onto the road, the sets are bounded in the four dimensional state space of the vehicle. Figure 5.1: Projection of feasible states for a car approaching a static obstacle Since the vehicle model is parametrized by the velocity we aim to solve the cal- culation of KN(C, u) analytically in section 5.1 to obtain an upper bound on the 32 5. Feasibility by Construction velocity that guarantees persistent feasibility. An analytical solution would allow to determine that velocity threshold as an online calculation in a vehicle. Section 5.2 shows an alternative approach to the problem. By studying the problem offline, it is possible to perform these calculations numerically for a range of ve- locities. An algorithm implemented in a vehicle could then use these precalculated sets to online determine a velocity for which such a maneuver becomes feasible. The complexity of the online calculation can be lowered significantly at the cost of extensive offline calculations and significant data storage necessary. 5.1 Analytical Approach Using Hyperrectangle Con- straints This section investigates the possibility to solve the problem of finding an upper bound on the velocity for the lane change maneuver in an analytical way. The computation to determine such an upper bound should be quick and efficient to allow implementation in embedded computers used in vehicles. At the very least, the calculation should be less computationally expensive than what is needed to repetitively attempt to solve the optimization problem for different velocities until an feasible solution is found. As shown in section 2.5, one disadvantage of algorithm 2 to calculate N-step con- trollable sets is, that the complexity of each iteration grows as more hyperplanes are added to the state constraints. If an algorithm can be found that yields an efficient calculation of an inner approximation of the one-step controllable set K1(X ) and can be represented in a structure such that is has the same structure and the same number of hyperplanes as the target set X , such algorithm may be used to effi- ciently calculate multiple iterations of algorithm 2 (section 2.4.4) to calculate n-step controllable sets. Since the physical road boundaries are represented by simple limitations on the lat- eral position the simplest form to approximate the constraints in (5.1) is a bounded polytope in the shape of a hyperrectangle. A hyperrectangle is a box type geometric shape in a higher dimension N > 3 where each state is limited by a single upper and a lower bound. A simplified illustration of the approach in two dimensions is shown in figure 5.2 for the discrete integrator introduced in section 2.5 in two dimensions. The invariant set C, previously shown in figure 2.7, is approximated by an inner rectangle as shown in figure 5.2a. Then the one-step controllable set is calculated in 5.2b and approximated by another inner rectangle in 5.2c. For the vehicle performing the lane change maneuver, we consider the discretized version of the linear force input model (3.17) with the road coordinate system (3.1)  v(k + 1) r(k + 1) ψ(k + 1) e(k + 1)  =  1− Cαrts mu ts ( bCαr mu − u ) 0 0 bCαrts Izzu 1− b2Cαrts Izzu 0 0 0 ts 1 0 ts 0 tsu 1   v(k) r(k) ψ(k) e(k) +  ts m ats Izz 0 0 Fyf(k). (5.3) 33 5. Feasibility by Construction −1 −0.5 0 0.5 1 −1 −0.5 0 0.5 1 State D er iv at iv e (a) Control invariant set C and hyperrect- angle shaped approximation X −1 −0.5 0 0.5 1 −1 −0.5 0 0.5 1 State D er iv at iv e (b) 1-step controllable set K1(X ) of the hyperrectangle shaped inner approxima- tion X −1 −0.5 0 0.5 1 −1 −0.5 0 0.5 1 State D er iv at iv e Control invariant set C Hyperrectangle approximation X 1-step controllable set K1(X ) 1-step approximation (c) Inner approximation of 1-step controllable set K1(X ) Figure 5.2: Hyperrectangle shaped inner approximation of the 1-step controllable set for the discrete integrator 34 5. Feasibility by Construction We can formulate the state constraints shown in equation (5.4) as a target set X . Considering a straight road we can assume the constraints on lateral velocity, yaw rate and heading to be symmetric. X ,  1 0 0 0 −1 0 0 0 0 1 0 0 0 −1 0 0 0 0 1 0 0 0 −1 0 0 0 0 1 0 0 0 −1   v(k) r(k) ψ(k) e(k)  ≤  vmax vmax rmax rmax ψmax ψmax eXmax −eXmin  (5.4) Using algorithm 2 we attempt to calculate K1(X , u) for the system (5.3) analytically. By performing the the matrix calculations presented in chapter 2.4.2 we can derive Pre(X ). The result of this first step of algorithm 2 is shown in appendix A.2. In the second part of the algortithm to derive the 1-step controllable set, the projection (see section 2.2.3) of Pre(X ) onto the R4-state space has to be calculated. Using the Fourier-Motzkin algorithm provided in appendix A.1, the set can be pro- jected analytically. The one-step controllable set is derived by intersecting the re- sult of the projection with the set of state constraints at the location of the one-step controllable set. The result is shown in appendix A.3. Assigning an inner hyperrect- angle into this set of constraints is generally possible without extended numerical calculations. The results show that in principle it is possible to efficiently find an inner approxi- mation of the set K1(X ) in the shape of a hyperrectangle set that was derived from hyperrectangle type constraints X . To examine the applicability of these results we can take a closer look at constraints three and four from the one-step controllable set shown in A.3 [ ts 0 tsu 1 ts 0 tsu −1 ]  v(k − 1) r(k − 1) ψ(k − 1) e(k − 1)  ≤ [ eXmax −eXmin ] . (5.5) Rewriting this equation in terms of the lateral position of the one-step controllable set e(t− 1) these hyperplanes can be stated as e(k − 1) ≤ eXmax − ts v(k − 1)− ts uψ(k − 1) e(k − 1) ≥ eXmin + ts v(k − 1) + ts uψ(k − 1). (5.6) These two equations simply represent the linear mapping of the boundaries on the lateral position e for one discrete time step of the motion model (5.3). Since the model does not have any direct input term on the lateral position, this mapping is only dependent on the lateral velocity and the lateral motion due to the heading direction. Assuming ts > 0 and u > 0, which is true for regular driving situations, we can again assign a new hyperrectangle approximation into these constraints. With the 35 5. Feasibility by Construction target set 5.4 being symmetric in all dimensions we can assume the same for the approximation of the one-step controllable set. The boundaries on the approximation of the one-step controllable set are defined by −vmax(k − 1) ≤ v(k − 1) ≤ vmax(k − 1) −rmax(k − 1) ≤ r(k − 1) ≤ rmax(k − 1) −ψmax(k − 1) ≤ ψ(k − 1) ≤ ψmax(k − 1) emin(k − 1) ≤ e(k − 1) ≤ emax(k − 1). (5.7) With vmax(k − 1) ≥ 0, rmax(k − 1) ≥ 0, ψmax(k − 1) ≥ 0, emin(k − 1) ≤ 0 and emax(k − 1) ≥ 0 this set has the same geometrical structure as the target set (5.4). To fit this hyperrectangle into the polytope K1(X ), all inequalities have to be fulfilled for all values inside the hyperrectangle. Since K1(X ) is a convex set, it is sufficient to test if all the extremal values of the hyperrectangle fulfil the constraints. The combinations of these extremal values in all dimensions correspond with the vertices of the hyperrectangle. Inserting the extremal values for e(k − 1), v(k − 1) and ψ(k− 1) into equations 5.6 yield the following inequalities that must be true for the hyperrectangle to be a valid inner approximation. e(k − 1) ≤ eXmax − tsvmax(k − 1)− ts u ψmax(k − 1) (5.8a) e(k − 1) ≤ eXmax + tsvmax(k − 1)− ts u ψmax(k − 1) (5.8b) e(k − 1) ≤ eXmax − tsvmax(k − 1) + ts u ψmax(k − 1) (5.8c) e(k − 1) ≤ eXmax + tsvmax(k − 1) + ts u ψmax(k − 1) (5.8d) e(k − 1) ≥ eXmin − tsvmax(k − 1)− ts u ψmax(k − 1) (5.8e) e(k − 1) ≥ eXmin + tsvmax(k − 1)− ts u ψmax(k − 1) (5.8f) e(k − 1) ≥ eXmin − tsvmax(k − 1) + ts u ψmax(k − 1) (5.8g) e(k − 1) ≥ eXmin + tsvmax(k − 1) + ts u ψmax(k − 1) (5.8h) With eXmin = −eXmax and all other variables on the right hand side being positive it is clear that the inequalities (5.8a) and (5.8h) represent the tightest bounds on e(k − 1). Therefore we can neglect the other inequalities as they will hold true if these two are fulfilled. The relevant bounds on the lateral position of the one-step controllable set are described by eXmin+tsvmax(k−1)+ts uψmax(k−1)≤e(k−1)≤eXmax−tsvmax(k−1)−ts uψmax(k−1). (5.9) For vmax(k − 1) > 0 and ψmax(k − 1) > 0 we can conclude that emax(k − 1) < eXmax and emin(k − 1) > eXmin. The lateral bounds of the hyperrectangle approximation of the one-step controllable set are shrinking in each iteration step if we attempt to assign a hyperrectangle with a nonzero range in the derivative states v and ψ. Considering an obstacle maneuver in which the safe target set is to the side of the obstacle while a vehicle is heading straight towards the obstacle renders this approach impractical. For the lane change maneuver the target set of the N-step 36 5. Feasibility by Construction controllable set calculation is located in the an open lane this set needs to expand for each iteration to eventually enclose the vehicle state. This analytical result can also be confirmed in the simple case of a discrete integrator as shown in figure 5.2. From the figure it is clear that an approximation of the one- step controllable set in the shape of a rectangle is always smaller that the target set X if the target set has the shape of a rectangle. Therefore a simple approximation of the n-step controllable set using hyperrectangle constraints cannot be used to efficiently calculate iterations of n-step controllable sets through a lane change maneuver. 5.2 Numerical Approach As an alternative approach to the analytical calculations presented in section 5.1 we use numerical calculation to determine an upper bound on the velocity that guarantees feasibility during a lane change maneuver. Using offline calculations we can pre-calculate trajectories of n-step controllable sets for a range of velocities as shown in figure 1.2. If sufficient trajectories of n-step controllable sets for different velocities are stored, simple set membership tests can be used to test which velocity allows a safe avoid- ance of an obstacle if the vehicle is located at a certain distance from the obstacle. 5.2.1 Reachability Calculations with MPT-Toolbox For numerical calculations the Multi-Parametric Toolbox (MPT) [22] in its current version 3.0.4 is used. The toolbox is developed by the Automatic Control Laboratory at the ETH Zürich in Switzerland and provides a Matlab interface to implement state of the art solver techniques for parametric optimization problems. MPT-toobox version 3.0 provides an interface for algorithms to perform both el- ementary operations on polytopes as explained in section 2.2 as well a integrated algorithms for reachability analysis as described in section 2.4. Examining the algorithms for the control invariant set 1 and for n-step controllable sets 2 in section 2.4, the main part of both operations is an iterative cycle of calcu- lating the Pre() of a set, projecting the resulting polytope in the state-input space back onto the original state space and calculating the intersection with the state constraints at that iteration step. The Pre() calculation is a simple matrix operation that can be quickly performed for large number of half spaces as shown in equation (2.17). The intersection with additional state constraints for a polytope in H-representation is performed by ap- pending the additional constraints to the set of halfspaces bounding the polytope. To perform the projection of the Pre() onto the original [β, r, ψ, e] state space the MPT toolbox provides a number or different algorithms. This step is by far the most computationally expensive operation in this iterative calculation. The most consistent results were obtained using the MPLP-algorithm. As an alter- native the toolbox provides two algorithms based on Fourier elimination, with and without intermediate redundancy elimination, and one algorithm that computes the projection based on a vertex representation. 37 5. Feasibility by Construction Attempting to calculate the invariant set results of the four dimensional vehicle model using the Fourier-elimination based algorithms lead to a Matlab crash after only ten iterations. The vertex based algorithm crashed with a Matlab error after only three iterations. While the exact cause of these errors could not be determined, it is evident that the implementations of these algorithms are not robust for four dimensional system with the complexity of the model 5.3. The MPLP-algorithm is the only algorithm provided in the toolbox that is capable of reliably performing higher dimensional projections with large numbers of halfspaces. Figure 5.3 illustrates the performance of the different projection algorithms provided in the MPT-toolbox and how the complexity and the n-step controllable set increases if no means for simplification are taken. 5 10 15 101 102 103 Iteration N um be r of ha lfs pa ce s MPLP Fourier iFourier Vrep (a) Complexity of the N-step control- lable set 5 10 15100 101 102 103 Iteration C al cu la tio n tim e[ s] (b) Calculation time per iteration Figure 5.3: Calculation of the N-step controllable set without approximation As shown by the example of the discrete integrator in section 2.5, calculating n-step controllable sets of systems containing integrators yield in skewed shaped polytopes. Therefore the accuracy of this calculation is directly correlated with the number of halfspaces used in the approximation. When calculating such sets the number of halfspaces increases with each iteration step. While this does increases the accuracy of the representation it also makes the calculation of the next step more computa- tionally expensive. Figure 5.3 shows that this increase in calculation time is exponential almost expo- nential in the number of halfspaces bounding resulting polytopes. Starting from eight initial constraints the calculation results in more than 500 hyperplanes in less than 20 iterations. The calculation time increases from 1.5 seconds for the first iteration to five minutes. These calculations were performed with Matlab 2011a and the MPT toolbox 3.2 on a quad core Intel i7 CPU with 2.5 Ghz. From the results it is clear that this trend makes the calculation of long trajectories with regular com- puting hardware impossible without suitable approximations algorithms that bound the calculation time. 38 5. Feasibility by Construction 5.2.2 Inner Approximation of Polytopes To allow the calculation of the n-step controllable set for longer distances as well as to achieve a result from control invariant set calculations an approximation algorithm is necessary. Since any subset of an n-step controllable set is also an n-step controllable set for the system with the desired target set and every subset of an invariant set is still an invariant set for the system for both calculations the approximation algorithm must ensure that an approximated polytope Papprox ⊆ P . Goubault, Mullier et al. [19] state in their paper on inner approximated reacha- bility analysis: "Computing a tight inner approximation of the range of a function over some set is notoriously difficult, way beyond obtaining outer approximations". While there is a large number of algorithms for outer approximation of polytopes in literature, there is rather limited material on inner approximation. In this thesis a heuristic approach for an inner approximation of a convex polytope is developed that allows an efficient reduction in the number of half spaces in a polytope. The algorithm utilizes the assumption that the polytope considered is symmetric with respect to the origin. This allows the algorithm to correct for possible numerical biases in the other steps of the iterative calculations studied in this work. Due to numerical rounding effects during the projection is can happen that the symmetry of the set is lost. The system constraints 5.1 provide symmetric boundaries on the system states β, r and ψ. For a vehicle driving in a single lane the bounda on the lateral position e are symmetric as well. Therefore all sets calculated using algorithm 1 will be symmetric as well. For the sets K1,...(X ) calculated using algorithm 2 we can ensure the same property if we simply assume that the preceding traffic is located in both outer lanes on a 3 lane road. With this assumption we can define symmetric bounds on the lateral position emin ≤ e ≤ emax relative to the symmetric control invariant set C as shown in figure 5.4. The lateral limits are defined for the center of gravity of the vehicle, therefore they are offset from the road boundaries by half the vehicle width and a safety margin. Figure 5.4: Symmetric road geometry for lane change predictions 39 5. Feasibility by Construction The algorithm implemented is illustrated in figure 5.5 in two dimensions. The halfspaces of the polytope shown in 5.5a are not exactly symmetrical which is a common case since they are results of numerical calculations. By comparing the normal vectors of the half spaces, opposing facets can be identified and sorted in pairs, as illustrated in 5.5b. By using averages of the normal vectors, the symmetry of the polytopes can be en- forced as shown in the third step. The distance to the origin of the new hyperplanes is modified such that all vertices on the original facets are on or outside the modified facets. Illustration 5.5d shows the final inner approximation of the polytope. By combining facets that have small angle differences, the number of hyperplanes can be reduced. Averaging the normal vectors and assigning a new distance to the origin such that all previous vertices are on or outside the newly created hyperplane leads to a inner approximation. In the figure the blue and purple halfspaces are combined to reduce complexity. This procedure can be performed iteratively until the number of halfspaces is reduced to a bearable level. Using this approach the symmetry of the polytope can be enforced and the number of hyperplanes is reduced. This approach can be generalized for higher dimensions. The implementation of the algorithm is shown in appendix B. (a) Input polytope bounded by six halfspaces (b) Halfspaces ordered in pairs (c) Numerical correction of pairs of halfspaces (d) Inner approximation with four halfspaces Figure 5.5: Inner approximation algorithm for symmetric polytopes 40 5. Feasibility by Construction While this algorithm has no claim for optimality, it does provide a reliable method for inner approximation such the sets calculated in algorithm 1 for the control invariant set, as well as algorithm 2 for the N-step controllable sets, can be calculated with bounded numbers of hyperplanes. 5.2.3 Numerical Control Invariant Sets Using the inner approximation algorithm presented, calculations of invariant sets and n-step controllable sets can be performed. By iterating algorithm 1 (section 2.4.3) for the discretized linear force input model (3.17) and envelope constraints (3.20) , (3.21) a safe, control invariant set C(u) within one lane can be determined for a specific velocity u. Since the calculations are performed offline, it is possible to utilize these algorithm although they are very computationally expensive. Figure 5.6 shows slices through the invariant set of the vehicle within one lane for the vehicle driving at 70 km/h. The polytope shown is an inner approximation with 160 halfspaces and 1018 vertices. The set is calculated in 100 iteration steps of algorithm 1 with an approximation combining halfspaces within two degrees in each step. As the set is defined in four dimensions, it is not possible to visualize the shape of the set in a single comprehensive figure. Therefore slices and projections are used to gain an understanding of the nature of the shape of these representations. −0.2 00.2 −0.200.2 −0.5 0 0.5 Yaw Rate [rad/s] Heading Angle [rad] La te ra lp os iti on [m ] (a) Slice at β = 0 rad −0.2 00.2 −0.20 −0.5 0 0.5 Yaw Rate [rad/s] Heading Angle [rad] La te ra lp os iti on [m ] (b) Slice at β = 0.10 rad Figure 5.6: Slices of the control invariant set for 70 km/h The shape of the set reveals some properties that are intuitively understandable. In the lateral position on the road it is bounded by the physical bounds described by the width of the road and the width of the vehicle. Along the lateral boundaries is is clear that a positive heading angle at the negative lateral road boundary is no problem because it brings the vehicle back towards the center of the track in the next step. In the same position negative heading is not allowed in the slice at zero 41 5. Feasibility by Construction sideslip because the vehicle would violate the road boundary in the next step. For a sideslip of 0.1 rad some amount of negative heading is possible since the lateral the lateral motion due to the heading angle can be compensated by the sideslip. 5.2.4 Numerical N-step Controllable Sets To calculate the N-step controllable sets KN for the vehicle in lane change maneuver algorithm 2 (section 2.4.4) is used. Starting from the control invariant set C(70km/h) shown in figure 5.6, the algorithm uses the safe handling envelope constraints (3.20) , (3.21) together with boundaries on the lateral position (3.2) that restrict the allowed region to the adjacent lanes to calculate n-step controllable sets. For each iteration step N , the N-step controllable set predicts the set of states for which it can be guaranteed that a feasible trajectory to the target set exists, located a distance ∆s = N ts u ahead of the obstacle. Figure 5.7 shows a bird’s eye view on the predictions for the lane change maneuver with 70 km/h. Projections in green limit the lateral positions on the road where there exist any states for which feasibility of the future trajectory can be guaranteed. The markers in red indicate slices of the sets at β = 0, r = 0 and ψ = 0. This corresponds to the locations from where the vehicle can start a lane change maneuver to pass the obstacle if is currently driving straight. The road is delimited by solid black lines and the dashed black lines delimit the region in which the center of the car is allowed to move. Figure 5.7: Projections onto the lateral position and slices at β = 0, r = 0 and ψ = 0 of n-step controllable sets for a lane change maneuver with 70 km/h The evolution of the N-step controllable set is shown in figure 5.8. Slices at β = 0 are provided for different iteration steps. The different slices illustrate the expan- sion of the set over the backwards trajectory. Since significant lateral motion and therefore a significant expansion of the set in the lateral direction is mainly caused by driving at significant heading angles, we can see that the set initially gets more skewed. Large negative lateral positions become possible rather quickly for signifi- cant positive heading angles. 42 5. Feasibility by Construction (a) N = 0 (b) N = 25 (c) N = 50 (d) N = 75 Figure 5.8: Slices at β = 0 of different N -step controllable set for the lane change maneuver at 70 km/h 43 5. Feasibility by Construction When the set spans the entire lateral width of the road as depicted in figure 5.8c, it continues to expand further to also include states with smaller heading angles for negative lateral position and larger heading angles for positive lateral positions. Eventually the set expands to a size such that it contains the target set C laterally located at the center of each of the lanes 44 6 Controller Design To gather experimental data to compare a real vehicle’s capabilities with the predic- tions derived in the previous chapter, an MPC-controller based on similar assump- tions (5.1) is implemented. The general concept of the controller implementation follows the work from Erlien, Funke and Gerdes [9]. In their paper they formulate a shared vehicle control for a steer-by-wire vehicle that tracks a driver’s steering intent while allowing interven- tions if the driver input would lead to unsafe situations. In [17] Funke applies the the concept to fully autonomous vehicles for path tracking with obstacle avoidance. In this work we assume the reference trajectory to be a straight line within a highway lane that the controller attempts to track. If an obstacle is detected in in that lane, the controller has to intervene and calculate an optimal steering trajectory to avoid the obstacle and return to the original lane. 6.1 Controller Formulation The controller aims to determine an optimal steering input force Fyf for the vehicle model (3.19) such that the vehicle can safely navigate through the environment while trying to stay as close to the desired lateral position as possible and maintaining a smooth motion of the vehicle by minimizing the objective function min Fyf nnear+nfar∑ k=nnear+1 σenv ( S(k) env )2 + nnear+nfar∑ k=1 σsh|S(k) sh | (6.1a) + nnear+nfar∑ k=nnear+1 σe|e(k)|+ σψ ( ψ(k) )2 (6.1b) + nnear−1∑ k=0 σunear ( F (k) yf )2 + nnear+nfar−1∑ k=nnear σufar ( F (k) yf )2 (6.1c) + nnear−1∑ k=0 σdunear ( F (k+1) yf − F (k) yf )2 + nnear+nfar−2∑ k=nnear σdufar(F (k+1) yf − F (k) yf )2. (6.1d) These possibly conflicting goals are managed by assigning different weighted cost terms σx to the objectives formulated in equation (6.1). Forcing the vehicle to stay inside the road boundaries and avoiding obstacles (3.2) is enforced by soft constraints with the slack variable Senv. Second highest priority is the avoidance of possibly dangerous driving situations by violating the vehicle’s 45 6. Controller Design handling capabilities. To ensure the vehicle stability the safe handling envelope (3.20), (3.21) is enforced with soft constraints using the slack variable Ssh. These two cost terms (6.1a) with σenv � σsh are enforcing safety for the maneuver. Both terms are significantly larger than all other cost terms in (6.1). The lane tracking objective σe (6.1b) penalizes the lateral deviation from the desired path. To avoid severe steering maneuvers when the vehicle is required to travel in a different lane due to an obstacle, this objective is enforced by a linear term. To influence the trajectory taken while the vehicle is changing the lane to avoid an obstacle, additional terms are introduced. A small cost σψ on the vehicle heading ψ helps avoid oscillations and provides damping to the system. Additional cost terms on the input force (6.1c) as well as the rate of the input (6.1d) allow reducing the severity of the steering action when the vehicle has to navigate around an obstacle and return to its original path. 6.1.1 Motion Prediction The MPC controller predicts the vehicle motion over the discrete time horizon using two different motion models (3.17) and (3.19) x(k+1) =A(k) d (x(0), tnear, Vx)x(k) +B (k) d (x(0), tnear, Vx)F (k) yf + d (k) d (x(0), tnear, Vx) k ∈ [0...nnear − 1] (6.2a) x(k+1) = A (k) d (0, tcorr, Vx)x(k) +B (k) d (0, tcorr, Vx)F (k) yf k = nnear (6.2b) x(k+1) = A (k) d (0, tfar, Vx)x(k) +B (k) d (0, tfar, Vx)F (k) yf k ∈ [nnear + 1...nfar − 1]. (6.2c) The prediction horizon is split into a near horizon (6.2a) and a far horizon (6.2c) and a correction step (6.2b). A small discretization time is chosen for the near horizon and a significantly longer step size is chosen for the far horizon. For the near horizon k = [0, ..., nnear] the controller uses the linear time varying vehicle model (3.19) which is a bicycle model linearized at the current velocity u = u0 and current rear tire slip angle αr0 . This allows to account for the rear tire saturation effects. For the prediction horizon k = [nnear + 1, ..., nfar] controller uses the linear force input model (3.17) which is also linearized at the current vehicle velocity but assumes a rear tire linearization at αr = 0. This is necessary because for the prediction horizon further ahead, an estimation of the future rear tire slip angle is becomes inaccurate since the future motion of the vehicle is yet unknown. In their paper Erlien, Funke and Gerdes [9] use a 10-step horizon with a sampling time of 10 ms to capture the vehicle dynamics in the near future and a 30-step far horizon with a sampling time of 200 ms to cover a longer distance farther ahead of the vehicle for path planning and obstacle avoidance. The combination of the two horizons adds up to a total prediction time horizon of 6.1 s. 46 6. Controller Design The motion model also implements a correction time step (6.2b) between the near and the far horizon with variable length as proposed in [10]. Since lateral bound- aries can only be assigned at the discretization points, any obstacle detected in the prediction horizon has to be extended in the direction of travel to span between discretization points. By shortening the first step time of the far horizon dynamically, the spatial location of all other time steps in the far horizon can remain spatially fixed while the car is cruising towards the obstacle. This allows the optimization to account for the fact that the ego vehicle is approaching the obstacle in each sampling interval as illustrated in figure 6.1. Without the correction step, the optimization would not account for the fact that it is getting closer until the obstacle would be captured by the next far horizon discretization point. Since this involves multiple sampling intervals of the controller it would lead to non-smooth control actions. (a) Vehicle approaching an obstacle in the far horizon (b) Location of discretization points a short time later without cor- rection step (c) Location of discretization points a short time later with correc- tion step Figure 6.1: Visualization of the influence of the correction step on obstacle repre- senation in the horizon 6.1.2 System Constraints The system is subject to a number of constraints to ensure vehicle safety. The environmental envelope which models lane boundaries as well as rigid obstacles (3.2) is most critical to passenger safety. These boundaries are implemented as a 47 6. Controller Design soft constraint (6.3a) and enforced over the far horizon as illustrated in figure 6.2. Henvx (k) ≤ Genv + S(k) env k ∈ [nnear + 1...nnear + nfar − 1] (6.3a) Hshx (k) ≤ Gsh + S (k) sh k ∈ [1...nnear + nfar] (6.3b) F (k) yf ≤ Fyf,max k ∈ [0...nnear + nfar − 1] (6.3c) |F (k) yf − F (k−1) yf | ≤ ∆Fyf,maxnear k ∈ [0...nnear − 1] (6.3d) The safe handling envelope introduced in (3.20) and (3.21) is enforced by constraint . To make sure that the optimization results can actually be achieved by the vehicle, the input to the system has to be bounded according to physical limits. Equation (6.3c) assigns a limit on the maximum lateral front force and the constraint (6.3d) assigns limits on the maximum rate of change in the input. The total limit of the lateral force is limited by the lateral force that the front tires can physically transmit. The rate of change is limited by the steering rate of the robot as well as the tire properties. The constraint for the rate of change of the input force is only enforced over the near horizon since it would not have any effect for sampling rates in the order of 200 ms in the far horizon. The steering actuator is fast enough to achieve any commanded force input which renders the constraint in the far horizon unnecessary. Figure 6.2: Representation of the environment boundaries as system constraints at discrete locations 6.2 Implementation The controller specified in sections 6.1 to 6.1.2 is implemented using a custom solver generated by CVXgen as presented in chapter 4.3. The detailed formulation to generate the custom solver used in experimental testing is shown in appendix C.1. CVXgen provides an online interface to generate and download the C-code for the solver together with a Matlab-MEX interface for the embedded solver and an imple- mentation of the specified problem statement using the general purpose CVX-solver that is not optimized for embedded execution. This enables the possibility to com- pare the custom solver with a more general solver to make sure it performs according to specification. To ensure that simulation results are consistent with the implemen- tation in the vehicle, the custom solver C-code is also used for simulations in this work. 48 6. Controller Design The C-code is integrated into a Simulink model using an S-Function Builder Block. This block offers a simple interface in which inputs and outputs to the S-function are specified and a wrapper function is used to feed the solver code with the numerical data of the optimization problem. The interface of the block is shown in figure 6.3. Figure 6.3: S-Function Builder block that implements the CVXgen code 6.2.1 Horizon length As described in section 6.1.1 the controller uses a predicion horizon that is split up into a near horizon to account for vehicle dynamcis and a far horizon for path planning. In their work Erlien et al. [9] use a 10-step near horizon with a sampling time of 10 ms and a 30-step far horizon with a sampling time of 200 ms. Using this configuration and exploiting the possibility of CVXgen to specify sparse matrices yields in a complexity of slightly over 4000 non-zero KKT-entries which is approximately the limit of the solver generator. The generated code with that horizon length performs well in simulation but implementation on the available real-time hardware do not allow the execution of the controller in real-time. On the embedded hardware used in the experimental setup described in chapter 7.2.1 the convergence time of the solver exceeds the desired sampling time of the controller. To allow a real-time implementation the near horizon is modified to 5 steps with 20 ms time step and the far horizon is shortened to 20 steps as illustrated in table 6.1. Figure 6.4 shows simulation results for the double lane change comparing a controller with a 30-step far horizon and a 10ms sampling rate with the controller that uses the reduced horizon length at 20 ms sampling rate. The results indicate that the modifications do not change the controller’s performance in a double lane change maneuver. For low velocities a long look ahead distance is not necessary because the motion of the vehicle is not significantly limited by the dynamics. For velocities above 20 m/s a, four second look ahead more than sufficient to cover the entire trajectory of the double lane change before the vehicle has to start steering. Therefore the reduction in horizon length does not negatively affect the performance of the controller for this maneuver. 49 6. Controller Design 60 80 100 120 140 160 −2 0 2 4 6