SHARED VEHICLE CONTROL USING SAFE DRIVING ENVELOPES FOR OBSTACLE AVOIDANCE AND STABILITY

Transcription

1 SHARED VEHICLE CONTROL USING SAFE DRIVING ENVELOPES FOR OBSTACLE AVOIDANCE AND STABILITY A DISSERTATION SUBMITTED TO THE DEPARTMENT OF MECHANICAL ENGINEERING AND THE COMMITTEE ON GRADUATE STUDIES OF STANFORD UNIVERSITY IN PARTIAL FULFILLMENT OF THE REQUIREMENTS FOR THE DEGREE OF DOCTOR OF PHILOSOPHY Stephen M. Erlien March 215

2 215 by Stephen Michael Erlien. All Rights Reserved. Re-distributed by Stanford University under license with the author. This work is licensed under a Creative Commons Attribution- Noncommercial 3. United States License. This dissertation is online at: ii

3 I certify that I have read this dissertation and that, in my opinion, it is fully adequate in scope and quality as a dissertation for the degree of Doctor of Philosophy. J Gerdes, Primary Adviser I certify that I have read this dissertation and that, in my opinion, it is fully adequate in scope and quality as a dissertation for the degree of Doctor of Philosophy. Stephen Boyd I certify that I have read this dissertation and that, in my opinion, it is fully adequate in scope and quality as a dissertation for the degree of Doctor of Philosophy. Marco Pavone Approved for the Stanford University Committee on Graduate Studies. Patricia J. Gumport, Vice Provost for Graduate Education This signature page was generated electronically upon submission of this dissertation in electronic format. An original signed hard copy of the signature page is on file in University Archives. iii

4 This dissertation is dedicated to my family. iv

5 Abstract Recent trends in automotive crash statistics suggest a dual role of technology in both saving and threatening the lives of American drivers. Advancements in automotive safety like Anti-Lock Braking Systems and Electronic Stability Control have led to a significant reduction in automotive fatalities over the last decade. However, the ubiquity of technology, mainly the cellular phone, has led to a dramatic increase in fatalities attributed to distracted driving. To address this challenge, auto manufacturers are empowering modern vehicles with even more technology. Advanced sensors provide real-time information about the surrounding environment. By-wire actuators, which allow drivers indirect command of the vehicle through an electronic pathway, enable vehicle safety systems to share control with a driver through augmentation of the driver s commands. This technology combination gives safety systems an unprecedented amount of authority to react to the vehicle s newly perceived world. Leveraging these advancements in vehicle actuation and sensing, this dissertation presents a shared control framework for obstacle avoidance and stability control using safe driving envelopes. One of these envelopes is defined by the vehicle handling limits while the other is defined by spatial limitations imposed by lane boundaries and obstacles. A Model Predictive Control (MPC) scheme determines at each time step if the current driver command allows for a safe vehicle trajectory within these two envelopes, intervening only when such a trajectory does not exist. A sparsity seeking objective in the MPC formulation serves as a simple and effective approach to shared control between a human driver and an automated machine. In this way, the controller seeks to identically match the driver s commands whenever possible v

6 while avoiding obstacles and preventing loss of control. Computationally efficient models of the environment, the vehicle, and the handling limits allow for real-time prediction of dangerous scenarios over a 4 (s) horizon. This advanced warning enables the use of brake actuation to ensure safe vehicle trajectories that adhere to both safe envelopes, providing an envelope of protection through augmentation of a driver s steering, braking, and throttle commands. The optimal control problem underlying the controller is inherently non-convex but is solved as a set of convex problems allowing for reliable, real-time implementation that is executed at 1 (Hz). This approach is validated on an experimental vehicle working with human drivers to negotiate obstacles in low friction environments. vi

7 Acknowledgment Writing a PhD dissertation is a humbling experience that provides a unique perspective on the realization that one s accomplishments are exceedingly dependent on the help and support of others. I would like to take a moment to thank some of the people that helped make this research possible and greatly influenced my time at Stanford. First of all, I would like to thank my advisor, Chris Gerdes. I never had the pleasure of having Chris as a professor in class; however, over the last five years, he taught me many valuable insights into control theory, vehicle dynamics, and life in general. Chris does an outstanding job of providing incredible resources for his students, giving me the opportunity to work on exciting projects and exceptional test beds. His enthusiasm to jump into those test beds and experience the research results first-hand provided me great encouragement and motivation throughout my PhD. The healthy work-life balance Chris works hard to maintain provides both a great lab atmosphere and an inspiring goal for his students. It s hard to imagine a better PhD advisor than Chris. I would also like to thank my defense committee members. Professor Per Enge enthusiastically agreed to serve as the chair of my committee. His classes on GPS were some of the most enjoyable I had at Stanford. It was a great pleasure to have Professor Kochenderfer on my committee, after having read his work on airplane collision avoidance in the early years of my PhD, which greatly influenced my own research. Serving also as my reading committee, Professor Stephen Boyd and Professor Marco Pavone significantly expanded my understanding of the technical aspects of this dissertation. The classes I had with these two professors were some of the most mathematical courses I have ever taken, enjoyed, and applied to real-world problems. vii

8 Next I would like to thank NISSAN MOTOR Co., Ltd. and the project team members Yoshitaka Deguchi, Hikaru Nishira, and Susumu Fujita for sponsoring this research. I would like to specifically thank Hikaru Nishira for his previous work on predictive collision avoidance and his technical guidance throughout this project, both of which greatly influenced this research. I had the distinct pleasure of working closely with Susumu Fujita while he was a visiting researcher at Stanford. He was a great resource, providing invaluable insights from industry and a sounding board for technical brainstorming. I would also like to thank the people that had the largest impact on my day-to-day graduate school experience: the members of the Dynamic Design Lab. Having been a part of the lab for the better part of a decade, I ve had the pleasure of watching the faces in the lab change, but the core values remain the same. The DDL has always been an exceptionally supportive and welcoming lab, making the otherwise daunting PhD experience actually a lot of fun. The DDL is full of exceptionally bright people who are always willing to help out, and there is no way I could have done the cool things I did in my PhD without their help and support. I would also like to thank the amazing administrative team behind the scenes keeping the lab running smoothly, including Adele, Erina, Jo, Elizabeth, and Jennifer. I would also like to thank my friends at Stanford. I think a school is defined by the amazing people who attend it, and the friends I ve met here have far exceeded even the high expectations I had for Stanford. You are all exceptional people who played a huge role in my growth and development over the last few years. I would especially like to thank the Chipotle Crew for your support, encouragement, and frequent burrito dinners. I would like to also thank my family, to whom this dissertation is dedicated. I know it took much effort to continue to be such a large part of my life while I was many states away from home. The support and guidance you provided was invaluable. My brothers, Alex and Jake, came out to visit many times throughout my time at Stanford, bringing a little of home with them every time. My parents, Karen and Mike, have always been extremely supportive and patient with me. They are always willing and available to listen to my concerns and struggles, even when viii

9 those conversations get exceedingly technical, and provide much needed advice and encouragement. And lastly, I would like to thank the newest member of my family, Elisha, who, after building robots with me in my early years at Stanford, eventually agreed to be my wife. You ve been such an important pillar of my life in the last six years. Your overflowing optimism and enthusiasm for learning are infectious, helping me stay positive and motivated throughout my graduate school career. Without the support and help of all these exceptional people, this research would not have been possible. ix

10 Contents Abstract Acknowledgment v vii 1 Introduction Motivation Shared Control of Vehicles Envelope Control Dissertation Contributions Combined Obstacle Avoidance and Vehicle Stability Convex Approach to Shared Human-Machine Control Convex Approximation of Tire Nonlinearity Over a Long Horizon Braking in Response to Steering Infeasibility Dissertation Outline Safe Envelopes for Shared Steering Control Introduction Vehicle Model Velocity States Position States Envelope Definitions Stable Handling Envelope Environmental Envelope MPC Formulation x

11 2.4.1 Two Time Scale Prediction Horizon Convex Optimization Problem(s) Varying Time Steps in Prediction Horizon Matching the Driver s Command Implementation Experimental Validation Discussion Design Decisions, Justifications, and Extensions Considering Vehicle Shape Terminal Cost on Vehicle Heading Curved Roads Quadratic Environmental Slack Cost Biasing to Match the Driver Cooperation Between Controller and Driver Steering Feel Design for Active Steering Predictive Haptic Feedback Discussion Predicting Rear Tire Saturation Introduction Modified MPC Plant Model Comparison to Other Approaches Safe Driving Envelopes Stable Handling Envelope Environmental Envelope MPC Formulation Simulation Results Experimental Results Discussion xi

12 5 Envelope Control using Braking and Steering Challenge of Longitudinal and Lateral Control Braking as Steering Feasibility Problem Braking to Prevent Envelope Violations Braking Policies Constant Speed Assumption Ensuring the Horizon Always Recedes Tire Force Coupling MPC Formulation Friction Circle Constraint Considering Front Longitudinal Forces Only Optimal Control Problem Feasibility of the Constant Speed Trajectory Experimental Results Proportional Braking Policy Fixed Braking Policy Extension to Brake- and Throttle-By-Wire Future Work Discussion Conclusion Future Work Fully Autonomous Vehicles Haptic Feedback User Studies Application to Racing Implementable Ethics Leveraging Advances in Parallel Computing Outlook Bibliography 155 xii

13 List of Tables 2.1 Vehicle Model Notation Steering Controller Parameters X1 Vehicle Parameters Prediction errors for recommended minimum radius curves for a lateral offset of one lane, e = 3.6 (m) Prediction Horizon Parameters P1 Vehicle Parameters Maximum Speed without Collision Braking and Steering Controller Parameters Proportional Braking Parameters Fixed Braking Parameters xiii

14 List of Figures 1.1 Vehicle states and axes Bike model schematic Brush tire model with affine approximation at ᾱ Stable handling envelope Generating the environmental envelope. Start with a) a collection of obstacles along the reference line, b) discretize in the s direction, c) extend objects in s direction to align with discretization and identify feasible gaps between objects, and then d) connect adjacent gaps into tubes (two of them in this example) which define a maximum (e (k) max) and minimum (e (k) min ) lateral deviation from the reference line at each time step, k Representation of the environment re-evaluated a short time later (a) without a correction time step and (b) with a correction time step Comparison of the l 1 and l 2 norms ( illustrating the ) different behavior of the norms at small values of F yf,driver F () yf,opt versus larger values An environment with 3 obstacles arranged in a manner that results in the worst-case number of possible tubes, 2 n = 8. A single trajectory from each tube is illustrated. Many of the tubes unnecessarily weave around obstacles and can be ignored without affecting the optimal steering command of the controller Stanford s X1, an all electric, throttle- and steer-by-wire research testbed with automatic brakes and haptic force feedback steering system... 4 xiv

15 2.9 Experiment on a low friction surface with a single obstacle in the middle of the road with a larger obstacle further down and on the left side of the road Double lane change (ISO ) on a low friction surface Simulation: Conservative behavior at road boundary as a result of the crude vehicle width model Schematic of required distance from vehicle s CG to environmental envelope for various vehicle orientations Plot of required distance from CG to environmental envelope for various vehicle orientations Plot of approximation error, ( d L d L ), vs vehicle orientation relative to the reference line Simulation: Less intrusive behavior at road boundary when using environmental constraint (3.6) with ψ = 8 [deg] Simulation: Controller properly orients the vehicle when confronted with narrowly spaced obstacles Simulation: Without a terminal cost, the planned trajectories arc into the road boundaries Simulation: With the terminal cost, the planned trajectories take the form of lane changes. In this example, q T = Curvature at point p that is located at a distance s along the reference path is K(s) = 1, where K > for left hand turns R(s) 3.1 Simulation: Controller augments driver s command to navigate around an obstacle while negotiating a turn Experiment: Driver bouncing off environmental boundary; l 1 -norm slack penalty function leads to harsh interventions by controller Experiment: Driver bouncing off environmental boundary; quadratic slack penalty function leads to smooth interventions by controller.. 68 xv

16 3.13 Simulation: Avoidance scenario with the driver turning into the obstacle and road boundary, with ρ () = 1.2 and ρ (i) = for i >. The steering command and predicted trajectory are smooth but the steering command rarely tracks the driver Simulation: Avoidance scenario with the driver turning into the obstacle and road boundary, with ρ () = 4.8 and ρ (i) = for i >. Steering matches driver but at the expense of aggressive steering commands Simulation: Avoidance scenario with the driver turning into the obstacle and road boundary, with ρ () = 1.2, ρ (1:3) =.5, and ρ (i) = for i > 3. This tuning provides a good balance between driver autonomy and smooth steering Conventional Steering System Force Feedback (FFB) Steering System Steering feel during augmentation Experiment: Steering feel emulator using δ, the actual road wheel angle, to generate the artificial steering feel Experiment: Steering feel emulator using δ driver, the driver s commanded road wheel angle, to generate the artificial steering feel Simulation: At t = 1 [s], controller matches driver s steer command but plans to augment the command thereafter with almost 2 [deg] of augmentation planned for the future t = 2 [s] Simulation: At t = 2 [s], controller still matches driver s steer command despite previously planning an augmentation. The planned trajectory has increased in severity relative to the planned trajectory at t = 1 [s] Simulation: Haptic signal generated from (3.34) with K haptic = 15 [Nm/rad] and k haptic = 4 for single obstacle avoidance scenario using an open-loop driver model Simulation: Haptic signal generated from (3.34) with K haptic = 15 [Nm/rad] and k haptic = 6 for single obstacle avoidance scenario using an open-loop driver model. Note the start delay in the haptic signal relative to Figure xvi

17 3.25 Experimentation: With predictive haptic feedback enabled, augmentation of the driver s steer command by the controller is reduced as the driver and controller cooperate to navigate the environment. Haptic signal was generated from (3.34) with K haptic = 15 [Nm/rad] and k haptic = Bicycle model schematic Brush tire model with affine approximation at ᾱ Stable handling envelope The environmental envelope is a representation of a) a collection of obstacles along the nominal path using b) tubes (two of them in this example) which define a maximum (e (k) max) and minimum (e (k) min ) lateral deviation from the nominal path at each time step, k Stanford s P1, an all electric, throttle- and steer-by-wire research testbed1 4.6 Arrangement of obstacles and road boundaries used in the following simulations. Vehicle travels in the direction indicated by the arrow Double lane change maneuver on low friction surface at 12 [m/s] Double lane change maneuver on low friction surface at 16 (m/s) Double lane change maneuver on low friction surface at 18 (m/s) Comparison of the planned safe trajectory midway through the double lane change manuever on low friction surface at 18 [m/s] Stanford s X1, an all electric, throttle- and steer-by-wire research testbed with automatic brakes and haptic force feedback steering system Experiment using X1 in double lane change on low friction µ =.55 with no environmental envelope violation Experiment using X1 in double lane change on low friction µ =.55 with slight violation of the environmental envelope at instance Controller plans a single lane change in response to blocked lane Predicted envelope violations for same maneuver at three different vehicle speeds xvii

18 5.3 Maximum predicted envelope violation for the same maneuver as a function of vehicle speed Block diagram of steering only envelope controller presented in previous chapters Block diagram of braking and steering envelope controller Obstacle blocking the lane enters the prediction horizon as the vehicle travels at 8 (m/s) Braking in excess of (5.4) moves the end of the prediction horizon to toward the vehicle causing the obstacle to leave the horizon Brake acceleration required to fix the end of the prediction horizon in space Friction circle concept illustrated using a g-g diagram Coupled tire force model Friction circle approximated as the intersection of n = 12 half-spaces An experimental ISO double lane change on a low friction surface (µ.5) without braking An experimental ISO double lane change on a low friction surface (µ.5) with braking in proportion to predicted envelope violation Predicted envelope violation and corresponding predicted rear tire lateral forces at instance 1 from the experiment illustrated in Figure Predicted envelope violation and corresponding predicted rear tire lateral forces at instance 2 from the experiment illustrated in Figure Predicted envelope violation and corresponding predicted rear tire lateral forces at instance 3 from the experiment illustrated in Figure An experimental ISO double lane change on a low friction surface (µ.5) with a fixed brake amount in response to predicted envelope violation A simulated panic brake scenario where a pop-up obstacle appears at instance 1 causing the driver to immediately steer the vehicle and, at instance 2, apply aggressive braking in a panic attempt to avoid collision145 xviii

19 5.19 Friction circle for the front axle during the simulated panic brake maneuver on a low friction surface (µ.5) with T brake = Friction circle for the front axle during the simulated panic brake maneuver on a low friction surface (µ.5) with T brake = xix

20 Chapter 1 Introduction 1.1 Motivation The automobile is the primary means of transportation in the United States, providing an unprecedented degree of mobility that millions of Americans rely on daily [6]. This mobility comes at a cost: in 212, an estimated 5.6 million traffic accidents resulted in 33,561 traffic fatalities and over 2.3 million injuries in the United States alone [57], making motor vehicles the leading cause of death for young Americans aged 11 to 27 [3] [57]. This marks an increase in the number of traffic fatalities over the prior year, which is fortunately only a recent trend. For six years in a row, the overall number of motor vehicle fatalities in the United States steadily declined, from 43,51 in 25 to 32,367 in 211, a historic low. Although many factors contributed to this decline including safety improvements to roadways, reduction in drunk driving, and increased use of safety belts, part of the decline is attributed to advances in vehicle safety systems like Antilock Brakes (ABS) and Electronic Statibly Control (ESC) [15]. In particular, the National Highway Traffic Safety Administration (NHTSA) estimates that ESC systems reduce single vehicle car crashes by 36 percent and fatal rollover crashes by 7 percent [18], and these systems are estimated to have saved 1,144 lives in 212 alone [58]. Despite these significant gains in vehicle safety, the number of motor vehicle fatalities attributed to distracted drivers has risen dramatically in the United States 1

21 CHAPTER 1. INTRODUCTION 2 over the last decade. This increase is largely attributed to cell phone use, specifically texting, according to a recent study by Wilson and Stimpson, which estimated that from 21 to 27 texting alone led to 16, additional driving fatalities. From 25 to 28, when the overall number of fatalities was on the decline, the number of fatalities attributed to distracted driving rose 28 percent [75]. Safety systems like ABS and ESC, which seek to prevent loss of control of the vehicle, can do little to prevent collisions resulting from a distracted driver. In response to this growing trend, recent Advanced Driver Assistance System (ADAS) developments aim at taking the surrounding vehicle environment into account to avoid traffic accidents. The first of these systems utilize sensors like RADAR, LIDAR, and/or cameras to perceive the environment and employ emergency braking in response to pending collisions [41]. Manufacturers like Volvo already provide these types of systems in production vehicles with a focus on collision mitigation [3]. Another new technology further empowering modern vehicles is steer-by-wire. Steer-by-wire removes the mechanical connection between the hand wheel and the road wheels, decoupling the driver s steering command from the actual steer angle of the vehicle s front wheels. In addition, the forces felt at the hand wheel are decoupled from those present at the road wheels, enabling more control over the design of the steering feel of the vehicle [9]. In 213, Nissan Motor Co. introduced the first production vehicle, the Inifiniti Q5, equipped with steer-by-wire with the primary features of side wind correction and rejection of road noise in the hand wheel [16]. Beyond these initial features, steer-by-wire also provides vehicle control systems the possibility of unprecedented control over the lateral dynamics of the vehicle. In addition, tire road friction estimates and vehicle states can be made available in realtime in vehicles equipped with steer-by-wire as demonstrated by Hsu et al. [39]. Steer-by-wire greatly enhances the actuation and sensing capabilities of a vehicle. With these capabilities appearing in more modern vehicles, active safety systems have the potential to make cars much safer. The best way to utilize these advancements in technology to improve passenger safety however remains an open-ended question. Fully automated driving is a tempting solution, but it requires automating the many social and contextually dependent

22 CHAPTER 1. INTRODUCTION 3 aspects of driving. Urmson et al. [72] of the winning team of the DARPA Urban Grand Challenge describe the difficulty in replacing the human driver in a fully autonomous system. They suggest that autonomous systems would benefit significantly from at least some form of human assistance. Considering the statistics presented at the beginning of this chapter relative to the miles driven illustrates the exceptional capabilities of human drivers on the whole: in the U.S. in 212, the average number of vehicle miles traveled per fatality was just shy of 9 million [57]. This indicates the exceptional ability of human drivers in the aggregate. Shared control, in which a human driver and an automated system work together, presents an opportunity to retain these critical abilities of human drivers while still leveraging the improved capabilities of automated vehicle systems. Humans and automation are uniquely suited for different tasks, and when teamed up, a synergistic effect is possible [67]. Automated systems can respond precisely and quickly to well-defined tasks where humans tend to respond less consistently and make more mistakes as the complexity and the frequency of these tasks increase. On the other hand, humans have a unique ability to detect and contextualize new patterns and to reason inductively, whereas automated systems struggle with these tasks [6]. In addition, shared control enables human drivers who enjoy driving to continue to do so with improved safety. For these reasons, there has been much interest in how best to share control between a human driver and a highly automated vehicle. 1.2 Shared Control of Vehicles Prior work in shared vehicle control explores various levels of human involvement, employing a number of approaches to address the balance between human versus computer control. In addition, many approaches differ on the assumptions made about the future behavior or intentions of the driver. Some of the earliest work in this area focuses on passive guidance. Kawabe et al. [43] present a Model Predictive Control (MPC) framework that leverages information about the surrounding environment to generate optimal paths to help guide a human driver. This optimal path, which involves both longitudinal and lateral maneuvers,

23 CHAPTER 1. INTRODUCTION 4 serves as a summary of environmental information that is intuitive to a human driver. Simulation results validate the approach, and specifics on how the generated paths could be presented to the driver is left open-ended. This approach provides guidance to the driver without actively controlling the vehicle and represents an extreme end of the spectrum of human involvement that leaves the human driver always in full control of the vehicle. Another approach is to explicitly switch control between a human driver and an automated controller. Minoiu-Enache et al. [21] present a lane departure avoidance system for steer-by-wire vehicles that switches control between a human and a number of control laws depending on the measured attentiveness of the driver. The steering torque applied to the hand wheel by the driver indicates the driver s attentiveness, although the authors suggest a number of other methods for gauging driver s attentiveness could be used as well. The system is implemented on a prototype vehicle equipped with an electric power steering system with the controller influencing the steer angle of the vehicle through an assistance torque on the steering column. A focus of the work is on the stability analysis of the switching system with an unknown human driver in the loop. Gray et al. [36] use a hierarchical nonlinear MPC (NMPC) approach for path planning and path tracking that switches control to and from the driver as a function of driver attentiveness and/or aggressiveness of the planned maneuver. Aggressive maneuvers are defined by a max front tire slip angle, and the driver s attentiveness is monitored using auxiliary vehicle sensors such as in-vehicle cameras. When the planned optimal trajectory exceeds this aggressiveness threshold or if the driver is deemed to be inattentive, control switches from the driver to the MPC controller. The NMPC path planner uses motion primitives, which are generated offline, to build a dynamically feasible trajectory in real-time to avoid obstacles in the environment. This path is then tracked by a low level NMPC path tracking controller. Experiments in a test vehicle on icy and slippery test tracks validate the approach. The motion primitives in this work address challenges in previous work that rely on a simplified point-mass model to generate the trajectories. This simplified representation of a vehicle led to planned trajectories that were not dynamically feasible and led to poor

24 CHAPTER 1. INTRODUCTION 5 path tracking performance. However, even with the motion primitive approach, Gray et al. acknowledge that separating the path tracking from the path following in the presence of model mismatch and external disturbances allows for tracking errors that may become large enough to render the planned maneuver infeasible to track. Instead of switching control, other approaches to shared control involve the interpretation of the driver s intention and a controller that seeks to track this interpreted intent. Gao et al. [35] present both a single and a hierarchical NMPC approach to vehicle control that assume a given trajectory represents the driver s intent. The single NMPC approach deviates from the driver s intended path to avoid obstacles and is shown to have poor performance or even be unstable without the use of an invariant set terminal cost or a sufficiently long prediction horizon. The hierarchical approach includes a second NMPC controller that recomputes a new trajectory in the presence of obstacles using a simplified point-mass vehicle model. The feasibility issues resulting from the use of the point-mass model are addressed in a follow up work that uses a motion primitive based NMPC path planner instead [34]. Although successful experiments on icy roads demonstrate the effectiveness of this approach to vehicle stability and obstacle avoidance, the validity of the assumption that a predefined path represents the driver s intentions is not addressed. Saleh et al. [65] present a lane keeping driver assistance system formulated as an H2-preview horizon optimal control problem using a driver-vehicle-road (DVR) model to model the driver s behavior and intentions. The presented DVR model is based on the hypotheses that drivers use visual information to navigate in their lane. This model attempts to fully capture many aspects of the human driver including, but not limited to, processing delays of the human visual system, the neuromuscular system of the human arm, and human reflex gains and time constants. The parameters of this driver model are estimated using experiments from multiple drivers on a driver simulator. A significant portion of the work focuses on robustness guarantees in the presence of uncertainty in the driver s behavior and/or mismatch between the actual driver and the driver model. Experiments on a driving simulator reveal improved lane keeping performance, and future work will address expanding the driver model to possibly include driver adaptation to the steering assistance system itself.

25 CHAPTER 1. INTRODUCTION 6 Another approach to shared control uses a final steer command which is a blend of a human driver and an optimal controller. Anderson et al. [6] use a constraint-based, pathless MPC approach to shared control of a teleoperated ground vehicle where the controller s influence on the final command increases with the severity of the predicted maneuver. The front wheel slip angle defines the severity of the predicted maneuver, and the final steer command applied to the vehicle is a linear blend of the MPC optimal command the driver s command based on this severity metric. When the MPC optimal peak front slip angle is low, a majority of the final steering command comes from the driver s command, and when it is high, a larger portion of the final steer command comes from the MPC optimal solution. The driver is not restricted to a predefined path; however, in the presence of obstacles, the controller restricts the driver to a heuristically defined safe corridor through the obstacles. Experimental results with an off-road, teleoperated vehicle with simulated losses of communication, which disrupted the visual feed to the teleoperator, demonstrate the effectiveness of this approach to shared control of teleoperated vehicles. In contrast to all of these approaches, the approach proposed in this dissertation ensures vehicle safety by defining and enforcing safe driving envelopes. The predictive nature and constraint handling capabilities of MPC make it an attractive framework for implementing this approach to shared control. The driver s commands are directly incorporated into the MPC problem formulation, and matching the driver s present command becomes a control objective that is evaluated against the additional objectives of collision avoidance and vehicle stability. By considering only the driver s present command, no model or interpretation of the driver s intentions is required. Additionally, no logic or heuristics are required to determine when to switch control between human and controller; the controller is always in control of the vehicle but is biased to identically match the driver s command whenever it is safe to do so. In this way, the proposed controller implements a form of envelope control, which is characterized by safe regions, or envelopes, of the state space in which a human operator is free to operate with the controller intervening only to ensure operation remains within these safe regions.

26 CHAPTER 1. INTRODUCTION Envelope Control Envelope control is widely used in the aircraft industry. These systems allow pilots to freely operate the aircraft within a safe operating regime defined by aircraft load, pitch, bank, and speed limitations [73]. The control system intervenes to prevent aircraft instability near and beyond the edges of this safe envelope. Both Airbus and Boeing implement envelope control in their aircraft, but their implementations differ on the extent of human versus computer control. Boeing s system makes use of haptic feedback via the yoke to inform pilots of the safe envelope bounds, but allows the pilot to override these bounds by applying more force on the yoke. This leaves the pilot with ultimate control over the aircraft. In comparison, the approach taken by Airbus implements the safe envelope as hard constraints that the pilot is unable to override. This leaves the automated system always in control of the aircraft [59]. Stability envelopes have also been applied to the automotive field, and a number of envelopes have been proposed for use with vehicle stabilization schemes. Inagaki et al. [4] present one of the earliest analyses of vehicle stability that defines a stable region with respect to vehicle states in the phase plane. Inagaki chooses vehicle sideslip and sideslip rate as the phase plane variables for his analysis because of their relatively low variability with vehicle speed and because vehicle stability is intrinsically related to the side slip motion of the vehicle. The sideslip of a vehicle, β, is defined as the angle between the vehicle s heading and the vehicle s velocity vector as shown in Figure 1.1. He proposes an open region between the saddle points of this phase plane as a safe envelope for vehicle stability, and validates this choice with experimental results using a direct yaw moment control system to enforce this safe envelope. Focusing on limiting the saturation of the tires to prevent loss of control, Hsu and Gerdes [38] propose a vehicle stabilization envelope that limits the peak forces of the front and rear wheels. A real-time approach to friction estimation in addition to the proposed envelope proves effective in stabilizing a test vehicle on a limited friction surface. Building upon this work, Beal and Gerdes [1] present a stable handling envelope that combines the phase plane design of Inagaki s approach and Hsu s explicit consideration of tire saturation. Beal s envelope is defined in the phase

27 CHAPTER 1. INTRODUCTION 8 Z r Yaw Rate X! Sideslip V Y Figure 1.1: Vehicle states and axes plane and limits the yaw rate of the vehicle as well as bounds the rear tire forces below their peak to prevent rear tire saturation. Vehicle sideslip and yaw rate serve as the phase plane variables that capture this stability envelope. Yaw rate, r, is defined as the rotational velocity of the vehicle as illustrated in Figure 1.1 and is easier to measure than sideslip rate. A model predictive controller enforces the envelope using front steering on vehicles equipped with steer-by-wire. Bobier and Gerdes [13] also present a closed envelope for safe driving defined in the sideslip and yaw rate phase plane. Bobier s envelope is defined using isolines in the phase plane. Although derived separately, the envelopes of Beal and Bobier agree quite closely. Bobier s envelope is slightly larger to incorporate more naturally stable regions of the phase plane in vehicles with strong under-steering characteristics. Bobier demonstrates this envelope on a steer-by-wire vehicle using a sliding-mode control framework. Recent applications of MPC for vehicle control enforce constraints on the maximum allowable rear tire force for stability [42]. In addition to these rear tire constraints, Turri et al. [71] explores a model predictive controller for path tracking and stabilization using different tire models throughout the prediction horizon. In

28 CHAPTER 1. INTRODUCTION 9 the latter portion of the horizon, the tire models are much more conservative to encourage trajectories with a level of robustness with respect to modeling this critically important property for vehicle stability. In all of these approaches, the nonlinear tire dynamics present at high slip angles are a dominant consideration for vehicle stability. The envelopes proposed by Beal and Bobier include additional constraints beyond tire force limitations that result in closed envelopes which define safe sets. Safe sets ensure system stability [61] and are also used in aviation envelope design [5]. The concept of envelope protection can be extended to collision avoidance in addition to stability. Again, the aviation industry provides an example of this already in production. Now mandated world-wide on larger aircraft, the Traffic Alert and Collision Avoidance System (TCAS) provides warnings and guidance to pilots if they approach too close to other aircraft [47]. Although this system does not actively control the aircraft, its goal is to ensure safety without interfering with normal, otherwise safe operations, and can therefore be thought of as a form of envelope protection. The next generation system slated to become the national standard is called the Airborne Collision Avoidance System X (ACAS X) and seeks to greatly improve upon the capabilities of TCAS. In particular, an emphasis of the new system is to reduce the occurrence of false positive alerts, emphasizing the envelope control paradigm of minimum interference while still ensuring safety [44]. With regards to safe envelopes for collision avoidance for ground vehicles, a number of approaches have been proposed to generate in real-time collision-free trajectories that lie in the obstacle-free regions in space. In the trajectory generation for Stanley, the winner of the DARPA Grand Challenge, Thrun et al. [69] perform a 2D search over a number of base maneuvers which consist of swerves and nudges. The chosen trajectory minimizes interference with obstacles, avoids leaving the lane, and minimizes deviation from the base trajectory while adhering to kinematic and dynamic constraints of a vehicle model. The base maneuvers and final trajectory are defined as lateral offsets from a fixed base trajectory that may not be obstacle-free. Hundelshausen et al. [24] use virtual, tentacle-like structures as constructs for the perception and identification of the safe regions in the environment with these tentacles

29 CHAPTER 1. INTRODUCTION 1 also serving as motion primitives in the generation of safe trajectories. Generating collision-free trajectories by piecing together motion primitives is also the approach used by Gao et al. [34] and Gray et al. [36] as described previously in Section 1.2. Attia et al. [7] present an approach to generating collision-free trajectories without the use of pre-computed motion primitives. In their approach, a collision-free trajectory is generated using parametric cubic splines that ensure a smooth final trajectory. The splines are constrained to a validity area, or reachable driving area within the vehicle s lane, to ensure the vehicle does not collide with the environment. The smoothness of the computed trajectory implicitly adheres to kinematic and dynamic constraints related to the vehicle. All of these approaches to collision avoidance use a path planning/path tracking paradigm. There are fewer examples in the literature of vehicle navigation frameworks that do not rely on path tracking. The shared control framework presented by Anderson et al. [6], which was previously discussed in Section 1.2, uses a pathless, MPC approach that represents safe regions of the environment by subdividing it into homotropies. These homotropies are defined by the set of trajectories that lies within them, and they greatly facilitate trajectory generation. Although safe trajectories are generated, they are not tracked. Rather, the controller continuously responds to the driver s inputs by computing a new trajectory at every time step. In this way, the driver is not restricted to a predetermined path; however, the driver cannot move between homotropies and is restricted to operate within a single, heuristically determined homotropy. In comparing the envelope approaches to stability with the envelope approaches to collision avoidance, it is observed that the collision avoidance applications commonly leverage predictive, model-based planning and control techniques. Although these predictive approaches appear in some stability applications, most notably the work by Beal and Gerdes [1], almost all the collision avoidance applications require some model-based planning in order to ensure vehicle safety. The design of the proposed envelope control framework follows this lead in using model predictive control and draws much inspiration from the envelope control examples described above.

30 CHAPTER 1. INTRODUCTION 11 This dissertation describes an approach to shared vehicle control using two envelopes to represent safe driving: one envelope is defined by environmental obstacles and lane boundaries and the other is defined by the vehicle handling limits. With regards to the production aircraft envelope controllers, the proposed framework follows the lead of Airbus, with ultimate control coming from the control system and not the human operator. This ensures vehicle stability and safety regardless of the human driver s input. However, the stability envelope used in the proposed envelope controller is enforced as a softened constraint to allow for precedence of other objectives, which is similar to the approach of Boeing. In the case of Boeing s design, the overriding objective is the autonomy of the human pilots; whereas in the proposed envelope controller, the overriding objective is collision avoidance. The proposed envelope controller builds directly on the work of Beal and Bobier with regards to the design of the stability envelope. The extension to their work comes in considering this stability envelope over a much longer horizon. This allows for simultaneous consideration of both vehicle stability and obstacle avoidance. In addition to collision avoidance and vehicle stability objectives, the proposed envelope control framework prioritizes minimizing intrusiveness to the driver; the controller intervenes only when the driver s steering command does not allow for a safe trajectory within the two safe envelopes. This follows a key motivation of the next generation aircraft avoidance system, ACAS X, which seeks to minimize unnecessary interventions. To this end, the proposed controller allows the driver much freedom before making any change to the driver s commands even if this results in subsequent aggressive maneuvers. With regards to environment representation, the proposed framework was in part inspired by the approach taken by Anderson et al. in subdividing the world into homotopies to facilitate efficient trajectory generation. The coordinate system used to model the motion of the vehicle in space is described as lateral offsets from a fixed base trajectory, or reference line, similarly to the approach taken by Thrun et al. for Stanley s path planner. In addition to these aspects that were inspired by previous research, many facets of the proposed envelope control framework are unique in design.

31 CHAPTER 1. INTRODUCTION Dissertation Contributions The contributions of this dissertation focus on a number of modeling and control formulations which enable real-time trajectory optimization with consideration of driver autonomy, obstacle avoidance, and vehicle stability Combined Obstacle Avoidance and Vehicle Stability The proposed envelope controller implements Model Predictive Control (MPC) and uses two envelopes to explicitly consider the sometimes competing objectives of vehicle stability and collision avoidance. The challenge of the different time scales over which these objectives should be evaluated is addressed using variable length time steps in the prediction horizon of the MPC implementation. This enables look ahead times long enough for obstacle avoidance while still capturing the fast dynamics of the vehicle in the near term without excessive computational burden overall. Applying an explicit stability envelope over this long horizon enables prediction of and early adjustments to vehicle stability challenges that arise from environmental conditions. In addition, the simple representations of the environment and the vehicle s handling limits enables fast, reliable real-time implementation as demonstrated on a vehicle test bed in limited friction environments Convex Approach to Shared Human-Machine Control In the proposed approach, the envelope controller determines if the current driver command allows for a safe vehicle trajectory within two safe drive envelopes, intervening only when such a trajectory does not exist. Use of a sparsity seeking objective in the MPC formulation enables a computationally efficient implementation of this approach to shared control with a human operator. Not only does this provide a minimally invasive implementation of envelope control that identically matches the driver when it is safe to do so, it results in MPC solutions which can be directly used to produce directional haptic feedback signals. This haptic guidance intuitively communicates the controller s intentions to the driver and encourages cooperation between

32 CHAPTER 1. INTRODUCTION 13 them as demonstrated in experimentation on real vehicles. This simple and effective approach to shared control could be more broadly applied to general human-machine applications Convex Approximation of Tire Nonlinearity Over a Long Horizon The saturation of the tires is a key consideration in the stability of a vehicle, but this nonlinearity poses a significant challenge to real-time optimization and control. Previous work by Beal and Gerdes [1] presented a method for capturing the effects of tire saturation in a model suitable for real-time optimization; however, with regards to the rear tires, their approach only works for short time horizons of a few hundred milliseconds. In this dissertation, the successive linearization technique provides an affine approximation of the nonlinear tire model throughout a prediction horizon of multiple seconds. This enables consideration of rear tire saturation in an MPC implementation suitable for obstacle avoidance in addition to vehicle stability Braking in Response to Steering Infeasibility To ensure vehicle safety in situations where the vehicle s speed is unsafe for conditions, the proposed control framework incorporates brake actuation ensure vehicle safety in a wide range of driving scenarios. The challenging task of combined lateral and longitudinal control is cast as a much simpler feasibility problem whose solution is the maximum safe vehicle speed for the given combination of road conditions and environmental hazards. In this way, the controller determines the amount of longitudinal force that can be safely commanded at the current time step, ensuring that braking to prevent long-term envelope violations does not produce new violations in the near-term. In addition, this desired longitudinal force can come directly from the driver in vehicles equipped with brake- and throttle-by-wire, creating a comprehensive envelope controller capable of ensuring vehicle safety through augmentation of the driver s steering, throttle, and brake commands.

33 CHAPTER 1. INTRODUCTION Dissertation Outline This dissertation presents the development, analysis, and validation of the model predictive envelope control framework introduced in this chapter. The remaining chapters are organized as follows: Chapter 2: Safe Envelopes for Shared Steering Control Chapter 2 provides an overview of the envelope control framework used throughout this dissertation. This chapter focuses on the definitions of the safe envelopes and the formulation of the control problem as a set of convex optimization problems that can be solved reliably in real-time. Experiments using vehicle test beds provide validation of the proposed approach to shared steering control. Chapter 3: Design Decisions, Justifications, and Extensions Chapter 3 introduces a number of additional complexities to the basic steering only framework presented in Chapter 2 and addresses both theoretical and practical implementation details. Motivated by simulation and experimental results, these additions do not change the underlying formulation introduced in Chapter 2, but provide improved performance at the expense of additional complexity. Chapter 4: Predicting Rear Tire Saturation The nonlinear nature of tire dynamics poses a challenge in predicting and modifying vehicle behavior in real-time. Chapter 4 describes how the successive linearization technique can approximate nonlinear tire dynamics along the entire prediction horizon without additional computational burden. Enabled by this modeling complexity, the envelope controller can identify situations in which violation of the safe driving envelopes is unavoidable using only steering actuation. Simulation and experimental results demonstrate interesting interactions between the occasionally competing objectives of vehicle stability and collision avoidance. Chapter 5: Envelope Control using Braking and Steering Previous chapters focus on control using steering only; however, situations may arise in which steering alone cannot ensure safe vehicle operation within both safe

34 CHAPTER 1. INTRODUCTION 15 driving envelopes. In these situations, brake actuation is required. In this chapter, the envelope control problem is cast as a convex feasibility problem with regards to vehicle speed. Using predicted envelope violations as a feedback signal for longitudinal actuators, a comprehensive steering and braking envelope controller is presented along with simulation and experimentation results. Chapter 6: Conclusion The dissertation concludes with an evaluation of the envelope control framework presented in the previous chapters along with a discussion of future developments and research directions.

35 Chapter 2 Safe Envelopes for Shared Steering Control This chapter introduces the envelope control framework which will serve as the foundation of this manuscript. Although subsequent chapters present additional extensions, the ideas and analysis of Chapter 2 are central and applicable to the remainder of this dissertation. In this chapter, the envelope controller focuses on a single actuator: front steering. In addition to the safety benefits provided by this single actuator controller, the steering-only controller presented in this chapter enables the development of a more comprehensive envelope controller capable of influencing both the lateral and the longitudinal dynamics of the vehicle. The basic envelope framework presented in this chapter will serve as the foundation for this comprehensive envelope controller. The majority of this chapter has been submitted and is under review for publication in the IEEE Transactions on Intelligent Transportation Systems in 214 [22]. 2.1 Introduction The envelope control framework presented in this chapter uses two envelopes to represent safe driving. One envelope incorporates environmental obstacles and lane boundaries and the other is defined by the vehicle handling limits. The driver s present 16

36 CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL 17 steering command is directly incorporated into the problem formulation, resulting in a controller which determines at each time step if the driver s command allows for a safe trajectory within these two safe envelopes, intervening only when such a trajectory does not exist. In this way, the driver is allowed to control the vehicle s steering so long as his actions will not lead to collision or loss of control. The predictive nature and constraint handling capabilities of MPC make it an attractive framework for implementing this approach to shared control. The MPC implementation used in this work makes use of variable length time steps in the prediction horizon to enable look ahead times long enough for obstacle avoidance while still capturing the fast dynamics of the vehicle in the near term without excessive computational burden overall. This results in a control scheme that is simple enough for fast, real-time consideration of vehicle stability and obstacle avoidance which is validated on a vehicle testbed in limited friction environments. The remainder of this chapter is structured as follows. Section 2.2 outlines the vehicle model used by the real-time controller. Section 2.3 derives the safe driving envelopes and describes the methodologies for generating these envelopes in realtime. Section 2.4 presents the MPC formulation along with the underlying convex optimization problem(s) to be solved at each time step. Lastly, experimental results demonstrate smooth integration of the driver s and controller s commands as well as the combined stabilizing and obstacle avoidance capabilities of the control framework. 2.2 Vehicle Model The vehicle model used in the MPC controller is a bicycle model with five states: two velocity states and three position states. In this chapter, front steering is the only actuator considered, and the vehicle is assumed to be equipped with steer-by-wire technology which enables the steer angle of the front road wheels (δ) to differ from the driver s commanded front steer angle (δ driver ) which is inputted to the hand wheel. Also, the controller shares control with the driver in steering only, leaving the driver in full control of the vehicle s longitudinal dynamics. Without a direct influence on the vehicle s speed, the controller simply reacts to changes in speed dictated by the driver

37 CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL 18 b U y a β α f δ F yr α r r e U x F yf s Reference Line Figure 2.1: Bike model schematic and, for simplicity, assumes the present vehicle speed will be maintained throughout the prediction horizon. Therefore, the vehicle model used by the controller assumes a constant longitudinal speed Velocity States The velocity states are sideslip (β) and yaw rate (r) as defined in Figure 2.1. The vehicle s sideslip can be expressed as: β = arctan ( Uy U x ) (2.1) U y U x (2.2) where U y and U x are the lateral and longitudinal velocities in the body fixed frame, respectively, and the assumption that U x U y gives the simplified expression. Assuming U x is constant, the vehicle s velocity states have the following equations of motion:

38 CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL 19 β = F yf + F yr mu x r (2.3) ṙ = af yf bf yr I zz (2.4) where F y[f,r] is the lateral tire force of the [front, rear] axle, m is the vehicle mass, I zz is the yaw inertia, and a and b are the distances from the center of gravity to the front and rear axles, respectively. The tire slip angle in the front (α f ) and rear (α r ) can be expressed as: α f = ( arctan β + ar ) δ U x β + ar α r = ( arctan β br ) U x where small angle approximations give linear expressions. U x δ (2.5) β br U x (2.6) This approximation is validated by the stability constraints used by the controller as discussed in Section The brush tire model proposed by Fiala [29] and presented in the following form by Pacejka [62] gives a useful model of the relationship between F y[f,r] and α [f,r] : F y = C α tan α + C2 α 3µF z tan α tan α C3 α tan 3 α, 27µ 2 Fz 2 µf z sgn α, α < arctan otherwise ( ) 3µF z C α = f tire (α) (2.7) where µ is the surface coefficient of friction, F z[f,r] is the normal load, and C α is the tire cornering stiffness. This tire force model is illustrated in Figure 2.2. The nonlinearity of the tire forces poses a significant challenge to real-time optimization. To address this challenge, the vehicle model used by the MPC controller

39 CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL 2 describes the vehicle s behavior by using tire forces and not steer angles. Front tire force (F yf ) serves as the input to the model and is mapped to δ using (2.5) and (2.7): δ = β + ar U x f 1 tire (F yf) (2.8) where real-time estimates of β and r are assumed to be available and f 1 tire is computed numerically and implemented as a 2D lookup table with inputs of rear slip angle and surface friction estimate. Use of F yf as the model input allows for a linear vehicle model which considers front tire saturation. For the rear tires, a linearization of the brush tire model at a given rear tire slip angle (ᾱ r ) models rear tire force (F yr ) as an affine function of α r : F yr = F yr Cᾱr (α r ᾱ r ) (2.9) where F yr = f tire (ᾱ r ) and Cᾱr is the equivalent cornering stiffness at ᾱ r. This approximation is also illustrated in Figure 2.2. Choosing the current rear slip angle, α r, to be ᾱ r in the initial time steps of the prediction horizon allows the MPC controller to explicitly consider rear tire saturation in the near-term prediction [1]. This will be discussed further in Section 2.4. For simplicity, tire model (2.7) is only a function of slip angle; however, force coupling due to the driver controlled longitudinal force could be directly included in the tire model as demonstrated in a real-time MPC scheme by Beal and Gerdes [1]. The equations of motion of the velocity states can now be expressed as affine functions of the states and input, F yf : β = ṙ = F yf + F yr Cᾱr (β br U x ᾱ r ) r (2.1) mu [ x ( )] af yf b Fyr Cᾱr β br U x ᾱ r (2.11) I zz

40 CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL Position States The position states of the vehicle are all in reference to a reference line that need not be obstacle-free. These states are heading deviation ( ψ), lateral deviation (e), and distance along the path (s) as defined in Figure 2.1. The equations of motion of the position states can be written as: ψ = r (2.12) ė = U x sin ( ψ) + U y cos ( ψ) (2.13) ṡ = U x cos ( ψ) U y sin ( ψ) (2.14) Using small angle assumptions for ψ and β, the above nonlinear equations can be approximated as linear functions of the vehicle states: ė U x ψ + U x β (2.15) ṡ U x U x β ψ U x (2.16) where, for small values of β and ψ, the product β ψ. The small angle assumption for β is a weaker assumption because, as described in Section 2.3.1, the controller bounds sideslip explicitly. The small angle assumption for ψ is a stronger assumption because the controller does not directly bound this state; however, to avoid collision with the road boundaries, the controller indirectly maintains ψ around zero as demonstrated in the experimental results. Combining (2.1), (2.11), (2.12), (2.15), and (2.16), a continuous state-space representation of the vehicle model can be expressed as: ẋ = A c (ᾱ r ) x + B c F yf + d c (ᾱ r ) (2.17)

41 CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL 22 with x = A c (ᾱ r ) = B c = [ ] T β r ψ s e [ b Cᾱr mu 2 x 1 b2 Cᾱr I zzu x 1 U x U x Cᾱr mu x b Cᾱr I zz 1 mu x ] T a I zz d c (ᾱ r ) = [ Fyr+ᾱ r Cᾱr mu x b( ] F yr+ᾱ r Cᾱr) T I zz U x where subscript c denotes a continuous time model and A c (ᾱ r ) indicates matrix A c is linearized around ᾱ r.

42 CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL 23.5 Tire Lateral Force Fy.5 1 x 14 Brush tire model Affine approx F y Tire slip angle.5 α α -C α Figure 2.2: Brush tire model with affine approximation at ᾱ Table 2.1: Vehicle Model Notation Description Symbol Units Steer angle δ rad Longitudinal Speed U x m/s Lateral Speed U y m/s Sideslip β rad Yaw rate r rad/s Heading deviation from path ψ rad Lateral deviation from path e m Distance along the path s m Lateral tire force on [front,rear] axle F y[f,r] N Tire slip angle on [front,rear] axle α [f,r] rad

43 CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL Envelope Definitions To ensure safe operation of the vehicle, the controller confines the states of the vehicle to remain within two safe driving envelopes over a finite prediction horizon. The first of these is a stable handling envelope that ensures vehicle stability through constraints on the velocity states. The second is an environmental envelope that constrains the position states to ensure the vehicle trajectory is collision-free. The definitions of these envelopes and the methodologies to generate them in real time are presented in the following sections Stable Handling Envelope The stable handling envelope was originally presented by Beal and Gerdes [1]. However, other vehicle stability envelopes have been proposed [13] and could be incorporated into this framework as well. The stable handling envelope defines limits on the vehicle s velocity states as illustrated in Figure 2.3. This envelope is bounded by the steady-state yaw rate and the rear slip angle at peak tire force. These bounds reflect the maximum capabilities of the vehicle s tires; at any point within this envelope, a steering command exists such that the vehicle can safely remain inside Yaw Rate r Sideslip β.1.2 Figure 2.3: Stable handling envelope A steady-state analysis can be used to determine an appropriate bound on yaw

44 CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL 25 rate that does not exceed the friction capabilities of the vehicle. Considering the steady-state condition of (2.3), the steady-state yaw rate can be expressed as: r ss = F yf + F yr mu x (2.18) Neglecting the effects of weight transfer and assuming zero longitudinal tire forces, the following relationship holds: F yf,max + F yr,max = mgµ (2.19) where g is the gravitational constant. Combining (2.18) and (2.19) gives an expression for the maximum steady-state yaw rate which defines bounds 2 and 4 in Figure 2.3: r ss,max = gµ U x (2.2) Another important consideration for vehicle stability is the saturation of the rear tires. The final two bounds of the vehicle envelope serve to limit the rear slip angle to the angle at which lateral force saturates (α r,sat ). For brush tire model (2.7), this is expressed as: ( α r,sat = arctan 3 mgµ C αr ) a a + b (2.21) For the vehicle parameters and reduced friction surface used in the experiments described in Section 2.5, α r,sat = 7.2 (deg), giving validation to the small angle approximations made in (2.5) and (2.6). Using this expression as a bound on α r, the following bound on β can be determined from (2.6): β max = α r,sat + br U x (2.22) This maximum sideslip serves as the basis for bounds 1 and 3 in Figure 2.3. Assuming real-time estimates of µ, r, and U x are available, the vehicle envelope

45 CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL 26 described is easily calculated in real time and can be compactly represented as the following linear inequality for each time step k into the prediction horizon: H sh x (k) G sh (2.23) with H sh = G sh = 1 b U x 1 b 1 U x 1 α r,sat r ss,max α r,sat r ss,max where subscript sh denotes the stable handling envelope and x (k) indicates the vehicle state at the kth time step into the prediction horizon Environmental Envelope The environmental envelope is in reference to the same reference line as the position states of the vehicle model. It is represented as time-varying constraints on e, the lateral deviation from the reference line. At each time step, the trajectory of the vehicle over the prediction horizon is constrained to be within this envelope to ensure the trajectory is collision-free. As mentioned previously, this reference line need not be obstacle-free; therefore, the environmental envelope may require the vehicle to deviate from the reference line. Figure 2.4 illustrates the methodology to generate the environmental envelope from a collection of obstacles along the reference line as illustrated in Figure 2.4a.

46 CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL 27 As stated previously, the controller shares control with the driver in steering, leaving the driver in full control of the longitudinal dynamics of the vehicle. Without a direct influence on the vehicle s speed, the controller simply reacts to changes in speed dictated by the driver and for simplicity assumes the present vehicle speed will be maintained throughout the prediction horizon. This allows for the environment to be sampled at discrete points along the reference line, which correspond to the vehicle s future position k steps into the prediction horizon as illustrated in Figure 2.4b. In Figure 2.4c, the objects in the environment are extended to align with this discrete sampling, and feasible gaps between obstacles are identified producing a representation of the obstacle-free regions of the environment. Feasible gaps are defined as distances greater than the vehicle width. In his overview of planning algorithms, LaValle presents methods for decomposing the space around obstacles into simple cells, which reduce motion planning problems to graph searches. The feasible gaps described here can be thought of as a cell in a variant of the vertical cell decomposition described by LaValle [48]. Starting at the vehicle s current position and moving in the positive s direction, adjacent feasible gaps are linked using a graph search algorithm to form tubes through the environment like the two illustrated in Figure 2.4d. To avoid collision with the environment, the vehicle s future trajectory needs to be fully contained within one of these tubes. This concept of feasible tubes has also been used in motion planning for robotic arms as presented by Suh and Bishop [68]. Each tube defines a bound on e at each time step k and can be compactly written as the linear inequality: H env x (k) G (k) env (2.24)

47 CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL 28 with H env = G env = 1 1 e(k) max 1d d 2 buffer e (k) min 1d d 2 buffer where the subscript env denotes the environmental envelope, e (k) max and e (k) min indicate the lateral deviation bounds for time step k, d is the vehicle width, and d buffer specifies a preferred minimum distance between obstacles and the vehicle to ensure driver comfort. The additional distance of d buffer also accounts for the additional minimum gap between obstacles required as the vehicle s orientation changes. The environmental envelope is defined as the set of tubes generated as described above, and a vehicle trajectory is collision-free throughout the prediction horizon if and only if it satisfies inequality (2.24) for all k for any one tube in the environmental envelope.

48 CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL 29 e s Reference line Obstacle Lane boundary Feasible gap Tube a) k = b) c) e (1) max e min (1) e max (4) e min (4) d) e (1) max e (4) max e (4) min e (1) min Figure 2.4: Generating the environmental envelope. Start with a) a collection of obstacles along the reference line, b) discretize in the s direction, c) extend objects in s direction to align with discretization and identify feasible gaps between objects, and then d) connect adjacent gaps into tubes (two of them in this example) which define a maximum (e (k) max) and minimum (e (k) min ) lateral deviation from the reference line at each time step, k.

49 CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL MPC Formulation The controller s primary task is to ensure safe vehicle operation within the previously defined safe driving envelopes. With these objectives met, the controller is designed to be minimally invasive to the driver while avoiding harsh interventions. The first of these objectives is modeled in this work as identically matching the driver s present steering command, and the latter is modeled as a preference for future trajectories with smooth steering commands. These objectives can be expressed as an optimal control problem to be evaluated over a finite prediction horizon. However, the set of feasible, collision-free trajectories to be evaluated is a non-convex set due to the presence of obstacles making the underlying control problem difficult to solve directly. Instead, a simpler, convex sub-problem is solved for each tube in the environmental envelope, and the solutions to these sub-problems are compared to give the global optimum to the non-convex control problem underlying the MPC controller. This is possible because any convex combination of trajectories that are generated from vehicle model (2.17) and are contained within a tube will also be contained within that same tube. Therefore, each tube defines a convex set of collision-free trajectories which enables the use of fast convex optimization techniques to quickly identify an optimal trajectory within a given tube. Dividing the environment into possible tubes transforms a non-convex problem into a set of convex problems which can be quickly solved. These sub-problems could be solved in parallel to further improve performance Two Time Scale Prediction Horizon The obstacle avoidance objective of the controller necessitates a long enough prediction horizon to safely anticipate upcoming obstacles; however, if the controller execution time step, t s,mpc, is used as the prediction time step throughout the full horizon, the total number of time steps required is prohibitively large for real-time implementation. Alternatively, selecting a large time step for the full horizon degrades the controller s performance in predicting near-term vehicle behavior which is necessary when reacting to unexpected challenges to vehicle stability. In addition,

50 CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL 31 it is important to capture the limited slew capabilities of the steering system in the near-term when attempting to match the driver s steering command while adhering to the safe handling envelope. To address this issue, the prediction horizon used in this work is split into two portions. The initial portion is comprised of T split small time steps of size t s,mpc =.1 (s) to accurately capture near-term vehicle behavior and steering system slew rate limitations. The latter portion is comprised of larger time steps of size t s,long =.2 (s) to extend the horizon to incorporate upcoming obstacles in the long-term. There are T steps in the complete prediction horizon, which is largely dictated by computational limitations. These time step sizes provide a good balance between look ahead distance and environment resolution and are comparable to those used in other MPC implementations for vehicle control [35] Convex Optimization Problem(s) At each execution of the controller, optimization problem (2.25) is solved for each tube in the environmental envelope, and the optimal input corresponding to the tube with the lowest objective value is used. As is common with MPC, only the optimal input for the first step into the prediction horizon, F () yf,opt, is applied to the vehicle, and the optimization problem is re-solved for all tubes at the next time step without any regard for which tube generated the optimal input previously. Therefore, the driver is never restricted to a single tube, but is free to switch to whichever tube best meets the control objectives. In this way, the tube construct is used only to facilitate computation of the global solution to the non-convex optimal control problem underlying the controller, and, therefore, has no affect on the stability properties of the MPC controller. The control objectives outlined previously can be expressed as the following receding horizon optimal control problem:

51 CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL 32 minimize F yf,driver F () yf,opt (2.25a) + ( ) 2 γ (k) F (k) yf,opt F (k 1) yf,opt (2.25b) k [σ sh σ sh ] S (k+1) (2.25c) + k + l sh,opt ( 2 [σ env σ env ] S env,opt) (l) (2.25d) subject to x (k+1) = A (k) d x(k) + B (k) d F (k) yf,opt + d(k) d (k) F yf,opt F yf,max H sh x (k+1) G sh + S (k+1) sh,opt k =... (T 1) H env x (l) G (l) env + S (l) env,opt l = (T split + 1)... T F (i) (i 1) yf,opt F yf,opt F yf,max slew i =... T split (2.25e) (2.25f) (2.25g) (2.25h) (2.25i) where the variables to be optimized are the optimal input trajectory (F yf,opt ) and the safe driving envelope slack variables (S sh,opt, S env,opt ). Tunable parameters in this optimization problem are γ, which establishes the trade-off between a smooth input trajectory (2.25b) and matching the driver s present steering command (2.25a), and the slack variable costs (σ sh, σ env ). Instead of enforcing the envelopes as hard constraints, the slack variables penalize violations of the envelopes, ensuring optimization problem (2.25) always has a feasible solution. As a result of the two-part prediction horizon with different size time steps, different values of γ are used in each portion of the horizon to give a more uniformly smooth trajectory over the entire horizon, as

52 CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL 33 given by: γ (k) = γ near γ long k < T split otherwise (2.26) where γ near and γ long correspond to the near and long-term portions of the horizon, respectively, and γ near > γ long. Cost term (2.25a) expresses the desire to match the driver s command where F yf,driver is the front tire force corresponding to the driver s commanded front steer angle, δ driver. Brush tire model (2.7) provides the mapping from δ driver to F yf,driver : ( F yf,driver = f tire β + ar ) δ driver U x (2.27) Constraints (2.25g) and (2.25h) enforce the stable handling and environmental envelopes, respectively. These constraints are softened with slack variables, S sh,opt and S env,opt, to ensure optimization (2.25) is always feasible. With the choice of sufficiently large σ sh and σ env, cost terms (2.25c) and (2.25d) encourage zero-valued slack variables resulting in optimal vehicle trajectories which adhere to both safe driving envelopes whenever possible. A quadratic penalty function for the environmental envelope allows for smoother interactions at the envelope boundary and, along with the relative size of σ sh and σ env, establishes a hierarchy between the two safe driving envelopes. Choosing σ env σ sh enforces a preference for obstacle avoidance over vehicle stability in situations where the controller is forced to choose between adherence to one envelope in favor of the other as illustrated in the experimental results. To ease the computation of optimization problem (2.25) and simplify the generation of the environmental envelope, the environmental envelope is not enforced in the early portion of the prediction horizon. As will be illustrated in the experimental results, this assumption does not negatively affect the MPC controller s performance because the steering actuation has little influence on the position states of the vehicle over the short time duration of the initial prediction horizon time steps. Therefore, even if the environmental envelope was explicitly considered in the near-term horizon,

53 CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL 34 the controller would have little authority to enforce the envelope over such a short period of time. Instead, the predictive nature of the MPC controller steers the vehicle in anticipation of approaching obstacles, guiding the vehicle to be collision-free over the short initial time steps without directly enforcing the environmental envelope in the near-term. Constraint (2.25f) reflects the maximum force capabilities of the front tires and (2.25i) reflects the slew rate capabilities of the vehicle steering system. The slew rate constraint is not enforced during the later portion of the prediction horizon because the larger time steps make such a constraint ill-defined. The vehicle models used in constraint (2.25e) are zero order hold discretizations of the continuous vehicle model (2.17) given for the kth time step by the matrix exponential: A(k) d B (k) d d (k) d = exp A(k) c B (k) c d (k) c t (k) (2.28) s with [ A (k) c ] d (k) c = [ ] A c (α r () ) d c (α r () ) k < T split [A c () d c ()] T split k (T 1) where α () r is the current rear slip angle determined by (2.6) from real-time estimates of β and r. In this way, the vehicle model used in the near-term prediction horizon is a linearization of the nonlinear rear tire behavior at the current rear slip angle (α r () ) allowing for consideration of rear tire saturation in the near-term horizon. As a consequence of this changing rear tire model, discretization (2.28) is calculated on-line by the controller for the vehicle model used in the initial portion of the prediction horizon. In the remainder of the prediction horizon, the rear tire slip angle is not known prior to solving optimization (2.25); therefore, a linear rear tire model is used. The implications of this simplifying assumption are highlighted in the experimental results.

54 CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL Varying Time Steps in Prediction Horizon To enable consideration of approaching obstacles in the long-term without compromising the prediction of velocity states in the near-term, different length time steps are employed in the prediction horizon of the MPC formulation as described in Sec This results in the controller executing faster than the time step used in the long-term prediction horizon, and requires a corrective, variable length time step to ensure a consistent representation of the environment as illustrated in Figure 2.5. Following the methodology for generating the environmental envelope described in Section 2.3.2, the boundaries of an obstacle are extended to align with the discretization of the environment. Figure 2.5a shows the discretization of the same environment a short time later without an initial correction time step resulting in a different representation of the obstacle; however, if a correction time step of appropriate length is used initially, the obstacle representation does not change as illustrated in Figure 2.5b. This appropriate value of t corr is computed on-line by the controller using: t s,corr = ( ) t s,corr s U x t s,corr s U x > t s,mpc t s,corr s U x + t s,long otherwise (2.29) where t s,corr is the correction time step length on the previous execution of the controller and s is the change in the vehicle s distance along the reference line since the previous execution of the controller. Therefore, the sampling time, t s, used in (2.28) is not constant throughout the prediction horizon, but is instead given for the kth step into the horizon by: t (k) s = t s,mpc t s,corr k < T split k = T split t s,long T split < k (T 1) (2.3) where t s,mpc is the controller execution time step, t s,long is the longer time step used in the later portion of the horizon, and t corr is the corrective, variable length time step.

55 CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL 36 Obstacle Lane boundary Representation of obstacle (a) (b) Figure 2.5: Representation of the environment re-evaluated a short time later (a) without a correction time step and (b) with a correction time step Matching the Driver s Command Cost term (2.25a) captures the desire to match the driver s steering command. This objective is best expressed using the l 1 norm for two reasons. First, the desire to identically match the driver s command is better captured by the larger values of the l 1 norm at small deviations than higher order norms as illustrated in Figure 2.6. Second, in situations where significant deviation from the driver s command is required to ensure safety, it is desirable for the controller to ignore the driver as much as possible. The l 1 norm provides the best convex approximation to this objective because the value at large deviations is as small as possible while still being convex as discussed by Boyd and Vandenberghe [14] Implementation Optimization problem (2.25) is a convex quadratic program with a significantly sparse structure that can be leveraged to produce an efficient solver for real-time implementation [52]. For this work, CVXGEN, developed by Mattingley and Boyd [51], generates

56 ( CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL l 2 norm l 1 norm 3 () norm F F yf,driver yf,opt ( () F yf,driver F yf,opt Figure 2.6: Comparison of the ( l 1 and l 2 norms) illustrating the different behavior of the norms at small values of F yf,driver F () yf,opt versus larger values

57 CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL 38 Table 2.2: Steering Controller Parameters Parameter Symbol Value Units Surface friction coefficient µ.55 (none) m Vehicle longitudinal speed U x (measured) s Driver comfort distance d buffer.4 m Prediction horizon length T 3 (none) Near-term horizon length T split 1 (none) Controller time step size t s,mpc.1 s Correction time step size t s,corr (computed) s Later horizon time step size t s,long.2 s 1 Near-term smooth input weight γ near 5 kn 2 1 Long-term smooth input weight γ long 2 [ kn 2 1 rad Stable handing slack weight σ sh [6 6] 1 Environmental slack weight σ env 1 m 2 Driver s commanded steer angle δ driver (measured) rad s rad ] a custom, primal-dual interior point solver that is implemented on a single core of an i7 processor utilizing MATLAB s real-time toolbox. Table 2.2 gives the parameters used in the controller as implemented in the following experiments. These parameters give a look ahead time of 3.91 to 4.11 (s) depending on the present value of t s,corr. These parameters define an optimization problem for each tube that can be solved on a single core of an i7 processor in less than 5 (ms). The number of optimization problems to be solved depends on the number of tubes needed to represent the environmental envelope. In the worst-case, the number of tubes needed to represent the environmental envelope is 2 n, with n being the number of obstacles. This occurs in scenarios where each obstacle presents the controller with a choice of avoidance on the left or on the right. However, as the number of tubes grows, more of them can be ignored without affecting the performance of the controller because only the tube with the lowest optimal solution to optimization (2.25) determines the steering input applied to the vehicle. As illustrated in Figure 2.7, many of the tubes in these worst-case environments unnecessarily weave between obstacles resulting in large optimal objective values to optimization (2.25) due to objective (2.25b). Therefore, heuristics

58 CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL 39 could be employed to reduce the number of tubes to a representative handful, which could be evaluated in real time. However, to illustrate the effectiveness of this shared control approach without this additional complexity, the environments used in the experimental validation can all be represented using a maximum of two tubes. This allows for time to evaluate both tubes in less than 1 (ms), which is the execution time step length of the controller, without having to implement the controller on a parallel processing system.

59 CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL 4 Obstacle Lane boundary Tube to evaluate Tube to ignore Figure 2.7: An environment with 3 obstacles arranged in a manner that results in the worst-case number of possible tubes, 2 n = 8. A single trajectory from each tube is illustrated. Many of the tubes unnecessarily weave around obstacles and can be ignored without affecting the optimal steering command of the controller. Figure 2.8: Stanford s X1, an all electric, throttle- and steer-by-wire research testbed with automatic brakes and haptic force feedback steering system

60 CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL Experimental Validation Experiments, using an instrumented test vehicle on a low friction surface, demonstrate the shared control scheme presented. In these experiments, the controller works with a human driver to negotiate obstacles along the reference line. The test vehicle is an all electric, drive-, brake-, and steer-by-wire vehicle called X1 which is shown in Figure 2.8. The parameters for this vehicle are specified in Table 2.3 and were obtained using similar techniques as described by Laws et al. [49]. X1 is equipped with an integrated GPS/INS system that provides real-time estimates of the vehicle states. In these experiments, obstacles and road boundary locations are assumed to be known. All of the following experiments took place on a gravel surface with variable friction. Although real-time friction estimation has been demonstrated on vehicles equipped with steer-by-wire [39], a fixed estimate of friction is used for simplicity. During these experiments, the driver maintains full control of the vehicle s acceleration and braking, and the hand wheel in the vehicle s steer-by-wire system does not incorporate any artificial steering feel. Figure 2.9 illustrates a scenario in which the controller safely avoids a collision when the human driver takes no action to do so. In this scenario, a single obstacle lies in the middle of the road with a larger obstacle further down and on the left side of the road. As illustrated in the bottom of Figure 2.9, the vehicle s speed, which is dictated by the driver, varies throughout the test. The simplifying assumption that the vehicle is maintaining a constant speed throughout the prediction horizon, Table 2.3: X1 Vehicle Parameters Parameter Symbol Value Units Mass m 1973 kg Yaw moment of inertia I zz 2 kg m 2 Distance from front axle to CG a 1.53 m Distance from rear axle to CG b 1.23 m Width d 1.87 m Front cornering stiffness C αf 1 kn/rad Rear cornering stiffness C αr 14 kn/rad

61 CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL 42 Distance (m) Vehicle Path Vehicle Width Obstacles Distance (m) 4 Yaw Rate, r (deg/s) Stable Handling Envelope Actual Side Slip, β (deg) Steer Angle, δ (deg) Actual Driver 18 Speed, U x (m/s) Time (s) Figure 2.9: Experiment on a low friction surface with a single obstacle in the middle of the road with a larger obstacle further down and on the left side of the road

62 CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL 43 which was addressed in Section 2.3.2, does not negatively affect the controller s performance as the controller avoids collision with the environment while maintaining vehicle stability. Initially, the controller identically matches the driver s command because doing so will not lead to a violation of either safe driving envelope. However, at instance 1, the driver s steering command threatens to violate the stable handling envelope, and the controller deviates from the driver s command to safely stabilize the vehicle. The controller continues to counter-steer the vehicle to avoid the approaching obstacle, and, at instance 2, the controller makes an additional steering adjustment to enforce both safe envelopes. The controller begins the process of again matching the driver s command. The rate of this transition is determined by γ near. At instance 3, the controller makes a quick steering correction to maintain stability of the vehicle before identically matching the driver once again. This illustrates the importance of considering vehicle stability when transitioning between an automated system and a human driver. Despite the approaching parked car, the controller still identically matches the driver s command because his command safely avoids the obstacle. This illustrates the minimally invasive approach of envelope control. After two minor steering augmentations around instance 4 in response to the stable handling envelope, the vehicle safely continues down the road unscathed. Figure 2.1 illustrates the controller s performance in a scenario where vehicle stability and obstacle avoidance directly compete. This is illustrated in a double lane change maneuver which conforms to ISO standard [1] on a low friction surface at speeds greater than 6 kph. In the beginning of the lane change, the controller identically matches the driver s steering command because doing so will not lead to a violation of either safe driving envelope. At instance 1, the driver prematurely initiates the first lane change and the controller makes a slight augmentation to prevent collision with the environment. The driver s command successfully completes the first lane change with only a slight correction made by the controller; however, at instance 2, the driver again prematurely initiates the lane change forcing the controller to modify his command. Once the obstacle has safely passed, the controller attempts to again match the driver s command at instance 3 ; however, the driver s command

63 CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL 44 Distance (m) Vehicle Path Vehicle Width Obstacles Distance (m) Stable Handling Envelope Actual Yaw Rate, r (deg/s) Side Slip, β (deg) Steer Angle, δ (deg) Actual Driver 18 Speed, U x (m/s) Time (s) Figure 2.1: Double lane change (ISO ) on a low friction surface

64 CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL 45 is too aggressive and threatens to violate both safe driving envelopes. The controller responds with a large augmentation of the driver s command to avoid colliding with the right road boundary. At instance 4, the controller allows the vehicle to operate briefly outside the stable handling envelope to avoid collision with the road boundary. This illustrates the capability of this control framework to evaluate the trade-off between vehicle stability and obstacle avoidance in real time which is determined by the penalty functions and relative weights on the envelope slack variables. At instance 5, a large steering augmentation re-stabilizes the vehicle. Only.2 (s) separate instances 4 and 5, illustrating the importance of a fast controller execution rate in ensuring vehicle stability. The demanding maneuver presented in Figure 2.1 also illustrates a limitation of the linear rear tire model used in the long-term prediction horizon. Without considering the future saturation of the rear tires, the controller attempts to match the driver at instance 3. If the driver s command had been ignored, violation of the safe envelopes may have been avoided. This can be addressed using the successive linearizations technique for handling nonlinearities in real-time MPC as explored by the authors [23]. However, even with the simplifying linear rear tire model in the long-term horizon, the controller successfully stabilizes the vehicle while safely navigating the environment in this demanding maneuver at high speed on low friction. 2.6 Discussion The constraint-based approach of envelope control enables the controller to ensure vehicle safety while being minimally invasive to the driver. Experimental results demonstrate smooth integration of the controller s and driver s commands, showing promise of this approach as a shared control scheme without the need to model or interpret the driver s intentions. Dividing the environment into feasible tubes allows the use of fast convex optimization techniques resulting in a controller that can evaluate vehicle stability and obstacle avoidance at 1 (Hz) with a 4 (s) prediction horizon. In addition, dynamic maneuvers in an experimental testbed on a low friction

65 CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL 46 surface illustrate the combined stabilizing and obstacle avoidance capabilities of the controller even in the presence of unmodeled disturbances.

66 Chapter 3 Design Decisions, Justifications, and Extensions The safe driving envelope controller presented in the previous chapter achieves the desired objectives of collision avoidance and vehicle stability as demonstrated in experimental validation. However, additional complexities can be introduced to expand the capabilities and performance of the envelope controller. These additional complexities include a more detailed model of the shape of the vehicle and a modified penalty function to improve performance when the vehicle is operating near the environmental boundary in real-world experiments. A simple approach to modeling road curvature will be presented that allows for the use of the envelope controller on roads that are not straight. An additional terminal cost influences the nature of the planned vehicle trajectories and improves the performance of the controller. The last additions focus on improving cooperation between the controller and the human driver through the use of haptic feedback derived directly from the MPC optimal trajectory. This chapter presents these additional extensions and provides justification for the added complexity they introduce. All of the modifications described in this chapter can still be implemented using convex optimization techniques and therefore do not significantly increase the computational requirements of the controller. This preserves the fast execution rate that characterizes this control framework. 47

67 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS Considering Vehicle Shape In Chapter 2, the collision avoidance criteria used in the controller design required the distance from the vehicle s center of gravity (CG) to the environmental envelope to be greater than a fixed value, effectively modeling the vehicle as a circle. However, few vehicles are this shape. Therefore, the simple approach shown in Chapter 2 requires a conservative value for d buffer in environmental envelope constraint (2.24) to ensure collision avoidance regardless of the orientation of the vehicle for non-circular vehicles. For convenience, constraint (2.24) is repeated here: H env x (k) G (k) env with H env = G env = 1 1 e(k) max 1w d 2 buffer e (k) min 1w d 2 buffer This conservative approach may result in unnecessary interventions by the controller as the vehicle approaches the environmental envelope boundary. A simulation of a driver attempting to merge close to the road boundary illustrates this effect as shown in Figure 3.1. As a result of the conservative model of the vehicle s width, the controller unnecessarily augments the driver s steer command, limiting the driver s autonomy even though her steering commands would not have resulted in collision or loss of control. Instead of using a fixed value for the required distance from the vehicle s CG to the environmental envelope in constraint (2.24), the dependency on vehicle orientation can be directly modeled. Figure 3.2 illustrates the relationship between the required distance between the vehicle s CG and the environmental envelope as a function of

68 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 49 Environmental Envelope 1 [m] [m] 2 1 Steering Command Driver Cmd Actual Cmd δ [deg] [s] Figure 3.1: Simulation: Conservative behavior at road boundary as a result of the crude vehicle width model

69 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 5 d FL = d RL d FL d RL Figure 3.2: Schematic of required distance from vehicle s CG to environmental envelope for various vehicle orientations vehicle orientation relative to the reference line. When considering collision on the left side of the vehicle, both the front-left and rear-left corners of the vehicle need to be considered. The distance of these points from the vehicle s CG in the direction perpendicular to the road are nonlinear functions of the vehicle heading deviation, ψ, as given by: d FL = d RL = [ (w 2 [ (w 2 1 ) 2 2 [ ( + a 2] w sin ψ + arctan 2a)] 1 ) [ ( )] b 2] w sin ψ + arctan 2b (3.1) (3.2) where d FL and d RL correspond to the distances from the vehicle s CG to the front-left and rear-left corners of the vehicle, respectively. As illustrated in Figure 3.2, the minimum distance from the environmental envelope occurs when the vehicle is oriented with the reference line. In this orientation, d FL = d RL = 1 w. However, as the orientation of the vehicle changes in either direction, this required distance increases. To ensure the left side of the vehicle 2 does not collide with the environmental envelope for all vehicle orientations, the maximum distance of the two corner points must be considered. This maximum distance on the left side of the vehicle, d L, is given by: d L = max(d FL, d RL ) (3.3)

70 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS [m] d L d FL d RL a b w/ Heading deviation, ψ [deg] Figure 3.3: Plot of required distance from CG to environmental envelope for various vehicle orientations and is illustrated in Figure 3.3. With a small angle assumption for ψ, equations (3.1) and (3.2) can be approximated as linear over the range of expected heading deviations. Therefore, a piece-wise linear approximation of d L can now be expressed as: d L = max (λ FL ( ψ ψ ) + d,fl, λ RL ( ψ + ψ ) + d,rl ) (3.4) where ψ is the linearization point for this approximation and λ FL and λ RL are the slopes of this approximation for the front and rear of the vehicle, respectively. In general, ψ should be small since ψ will be near zero with the envelope controller

71 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS ψ = [deg] ψ = 15 [deg] ψ = 25 [deg] Approximation error [m] Heading deviation, ψ [deg] Figure 3.4: Plot of approximation error, ( d L d L ), vs vehicle orientation relative to the reference line safely guiding the vehicle down the road; however, ψ can serve as a tuning parameter to shape the approximation error as illustrated in Figure 3.4. Larger values of ψ improve the range over which d L provides a useful approximation for d L, and smaller values reduce the approximation error when the vehicle is oriented with the road, ψ =. Regardless of the choice of ψ, d L provides an upper-bound for d L for all vehicle orientations. Due to the symmetry of the vehicle, a similar approximation for the equivalent distance on the right side, d R, is given by: d R = max (λ FL ( ψ ψ ) + d,fl, λ RL ( ψ + ψ ) + d,rl ) (3.5)

72 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 53 These expressions for d L and d R can be used to augment constraint (2.24) to give an environmental envelope constraint which considers vehicle orientation: H env x (k) G (k) env max ( F FL x (k) + J FL, F RL x (k) + J RL ) (3.6) with F FL = J FL = F RL = J RL = λ FL λ FL λ FL ψ + d FL 1w 2 λ FL ψ + d FL 1w 2 λ RL λ RL λ RL ψ + d RL 1w 2 λ RL ψ + d RL 1w 2 and H env and G env are similarly defined as in (2.24). Constraint (3.6) consists of a linear term less than a convex expression. This results in a convex constraint which can be easily incorporated into optimal control problem (2.25), which was described in Chapter 2. Equipped with this new version of the environmental envelope constraint, a less conservative value for d buffer is required to ensure collision avoidance and the controller behaves less conservatively near the road boundary. This is illustrated in Figure 3.5 where this improved environmental envelope constraint is applied to the identical merge scenario presented in Figure 3.1. In addition, environmental constraint (3.6) enables the controller to appropriately orient the vehicle when confronted with narrowly spaced obstacles, as illustrated in Figure 3.6.

73 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 54 Environmental Envelope 1 [m] [m] Steering Command Driver Cmd Actual Cmd δ [deg] [s] Figure 3.5: Simulation: Less intrusive behavior at road boundary when using environmental constraint (3.6) with ψ = 8 [deg].

74 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 55 Environmental Envelope 5 [m] [m] Steering Command Driver Cmd Actual Cmd δ [deg] [s] Figure 3.6: Simulation: Controller properly orients the vehicle when confronted with narrowly spaced obstacles

75 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS Terminal Cost on Vehicle Heading Model predictive controllers typically include a terminal cost on the system state at the end of the horizon to drive the system to an invariant set. This provides system stability in addition to the benefits of infinite horizon optimal control in a finite horizon implementation as described by Mayne [53] and many others. The stable handling envelope (2.3.1) was designed to be an invariant set with regards to the velocity states of the vehicle to ensure stability and feasibility as described by Beal and Gerdes [1] with a proof appearing in Beal s PhD Thesis [11]. In theory, the stable handling envelope would only have to be enforced as a terminal constraint to achieve the stability guarantee. However, in practice, it is enforced throughout the prediction horizon to ensure yaw stability even in the presence of un-modeled disturbances and mismatch between the vehicle plant and the vehicle model as illustrated in the experimental results presented in Chapter 2. The environmental envelope does not define an invariant set. If the state of the vehicle at the final step of the prediction horizon adheres to the environmental envelope, no guarantees are made that future vehicle states beyond the prediction horizon will also do so. This is illustrated in Figure 3.7 where the solution to the optimal control problem is illustrated as predicted vehicle states along the reference line. Even though all points along the prediction horizon safely adhere to the environmental envelope, it is clear that if the vehicle were to follow this trajectory it would most certainly collide with the road boundary at some time just beyond the prediction horizon. Instead, this planned trajectory will have to be repeatedly modified during subsequent executions of the controller as the prediction horizon recedes further down the reference line. Enforcing a terminal cost on the heading deviation, ψ, of the vehicle over the final steps in the prediction horizon improves upon this situation. This additional cost term can be expressed as: ( q ) T ψ (j) 2 j (3.7)

76 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 57 Environmental Envelope 5 [m] Actual Traj Safe Short Term Traj Safe 4 Long 45 Term Traj [m] Steering Command Driver Cmd Actual Cmd δ [deg] [s] Figure 3.7: Simulation: Without a terminal cost, the planned trajectories arc into the road boundaries

77 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 58 where j = (T 3)...T and q T is chosen so that this terminal cost objective dominates the smooth steering and driver autonomy objectives discussed in Section of Chapter 2. Terminal cost (3.7) is enforced over the last three time steps. This ensures the terminal state of the vehicle is safely traveling tangent to the path. The condition ψ = only in the final step of the horizon does not necessarily ensure the terminal state of the vehicle is safely traveling tangent to the path because the vehicle could have a large lateral velocity, resulting in a similar situation as illustrated in Figure 3.7. A temping alternative is to impose a terminal cost on β; however, when operating on curved roads, this is not the desired terminal condition as the vehicle is expected to have a non-zero, steady-state side slip. Figure 3.8 illustrates the effect of terminal cost (3.7) on the optimal trajectory when the controller is confronted with the the same environment as Figure 3.7. If the vehicle were to follow this new trajectory, it would remain collision free even after reaching the end of the prediction horizon. In addition, the predicted trajectory takes the form of a lane change. Lane changes are a much more intuitive avoidance maneuver than the trajectory given in Figure 3.7 and appear extensively in vehicle control literature, as presented by Thrun et al. for the base maneuvers used in Stanley s path planner [69] and by Funke and Gerdes for autonomous vehicle control at the limits of handling [32].

78 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 59 Environmental Envelope 5 [m] Actual Traj Safe 4Short 45 Term Traj [m] Safe Long Term Traj Steering Command Driver Cmd Actual Cmd δ [deg] [s] Figure 3.8: Simulation: With the terminal cost, the planned trajectories take the form of lane changes. In this example, q T = 5

79 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS Curved Roads For simplicity, the environments presented in Chapter 2 were all set on straight roads using a reference line with no curvature. However, the optimal control problem can be adapted to consider curved roads by incorporating reference paths with nonzero curvature. Figure 3.9 illustrates a reference path with nonzero curvature. The curvature at a point that is located at a distance s along the reference path, K(s), is defined as the reciprocal of the radius R of curvature, which is radius of the osculating circle at that point. This curvature profile parameterizes the reference path. Given a starting position in global coordinates (E, N ) and an initial heading angle (ψ ), the position and heading angle along the path can be computed from this parameterized curvature: ψ(s) = E(s) = N(s) = s s s K(x)dx + ψ (3.8) cos (ψ(x)) dx + E (3.9) sin (ψ(x)) dx + N (3.1) where ψ, E, and N are also illustrated in Figure 3.9. The equations of motion of the position states relative to a reference path with nonzero curvature can be written as: ψ = r K(s)U x (3.11) ė = U x sin ( ψ) + U y cos ( ψ) (3.12) ( ) 1 ṡ = (U x cos ( ψ) U y sin ( ψ)) (3.13) 1 K(s)e Using small angle assumptions for ψ and β as was done in Chapter 2, these nonlinear equations of motion can be approximated as: ė U x ψ + U x β (3.14) ( ) 1 ṡ U x (3.15) 1 K(s)e

80 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 61 Reference Path e s N p ψ R E Figure 3.9: Curvature at point p that is located at a distance s along the reference path is K(s) = 1, where K > for left hand turns R(s) where, for small values of β and ψ, the product β ψ. When the vehicle travels along the path, i.e. e =, only state update equation (3.11) differs from the formulation presented in Chapter 2. This modified equation has an affine term that introduces road curvature as a known disturbance. This approach to modeling road curvature appears in other model predictive approaches to vehicle control as described by Turri et al. [71]. If the future speed profile of the vehicle is known apriori, as was assumed in Chapter 2, and the vehicle travels along the reference trajectory with e = throughout the prediction horizon, the curvature at each point along the prediction horizon is known prior to solving the optimal control problem. Therefore, state update equation (3.11) is readily incorporated into the affine time-varying vehicle model used in the optimal control problem formulation presented in Chapter 2. Incorporating road curvature in this way requires no modification to the environmental envelope definition or generation, but does require mapping obstacle and road boundary locations to e and s coordinates of a curved reference path. In the case of a zero curvature reference path, this mapping requires only a simple linear transformation. In the case of a curved reference path, this mapping is more complicated, but algorithms exist to do this in real-time as demonstrated by Rossetter et al. [64]. If the vehicle travels with nonzero lateral error, i.e. e, state update equation (3.15) differs from the zero curvature version presented in Chapter 2. Unfortunately,

81 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 62 equation (3.15) cannot be directly incorporated into the optimal control formulation because, as described in Section 2.3.2, generation of the environmental envelope requires knowledge of the vehicle s future location along the reference path prior to solving the optimal control problem. This is not possible because equation (3.15) is a function of the vehicle s future lateral offset which is not known prior to solving the optimal control problem. Therefore, an approximation of equation (3.15) is required. One approach is to assume e = throughout the prediction horizon, reducing equation (3.15) to the form presented in Chapter 2: ṡ approx U x (3.16) If the vehicle remains near the reference path, i.e. e, this assumption works well. Figure 3.1 illustrates the type of maneuvers the controller is capable of planning using curved reference paths and the assumption that e =. In this simulation, the controller plans an avoidance trajectory around an obstacle while negotiating a turn. This approach is validated with experimental results as presented by Funke et al. [33], who applied this modified framework to a fully autonomous vehicle operating on curved paths. If the vehicle deviates from the path, as would be expected when sharing control with a human driver, this approximation results in an error in the predicted distance of the vehicle along the reference path throughout the prediction horizon: ṡ err = ṡ approx ṡ (3.17) ( ) 1 U x U x (3.18) 1 K(s)e This error causes the controller to inaccurately predict the distance to upcoming obstacles. To quantify the magnitude of this error, simplifying assumptions that the road curvature is constant and the lateral offset is constant throughout the prediction horizon allow error term (3.18) to be easily integrated over the full prediction horizon.

82 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 63 9 Environmental Envelope [m] Actual Traj Safe 4 Short 5Term Traj [m] Safe Long Term Traj Steering Command Driver Cmd Actual Cmd δ [deg] Figure 3.1: Simulation: Controller augments driver s command to navigate around an obstacle while negotiating a turn

83 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 64 This gives the longitudinal position error at the end of the horizon: s err = tph tph tph (ṡ approx ṡ) dt (3.19) ) dt (3.2) ( 1 U x U x 1 Ke ( Ke U x 1 Ke U x ( Ke 1 Ke ) dt (3.21) ) t PH (3.22) where t PH is the time length of the prediction horizon and s err is the error in predicted vehicle position along the reference path at the end of the horizon. As seen in equation (3.22), this prediction error is a function of vehicle speed U x, reference path curvature K, lateral offset from the reference path e, and prediction horizon length t PH. Using the actual distance along the prediction horizon: s actual = ( ) 1 U x t PH 1 Ke (3.23) (3.24) a prediction error percentage can be computed: s err,% = 1(s err /s actual ) (3.25) 1(Ke) (3.26) To get a sense of the magnitude of this error in typical driving scenarios, Table 3.1 provides minimum radii of curvature recommended by the American Association of State Highway and Transportation Officials for the design of U.S. roadways for a range of vehicle speeds. Table 3.1 additionally provides the corresponding prediction error and error percentage at each speed/curvature combination assuming the same 4 (s) prediction horizon length used in Chapter 2 and a lateral offset of 3.6 (m), which is the typical width of a highway lane in the U.S. [4]. As seen in Table 3.1, at speeds greater than 14 (m/s) the recommended minimum

84 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 65 Table 3.1: Prediction errors for recommended minimum radius curves for a lateral offset of one lane, e = 3.6 (m) Vehicle Minimum Radius Prediction Prediction Error Speed U x of Curvature R Error s err Percentage s err,% (m/s) (m) (m) (%) radius of curvature of U.S. roadways is large enough to limit the error in predicted distance along the reference path to less than 6% for lateral offsets of a single lane. However, at slower speeds, this error grows significantly as the recommended minimum radius shrinks. This reiterates the trend that the models used in this dissertation are more accurate the faster the vehicle travels. Future work will focus on two methods to reduce this approximation error. The first approach is to assume the vehicle maintains a constant lateral offset from the path throughout the horizon. In this case, equation (3.15) is approximated as: ( ) 1 ṡ approx U x (3.27) 1 K(s)e () where e () is the vehicle s measured lateral offset from the reference path at the moment the optimal control problem is solved. This approximation ensures accurate near-term predictions of the distance s to upcoming obstacles even when the vehicle is not following the reference path. However, approximation errors of the order seen in Table 3.1 will still result at the end of the horizon for large planned deviations

85 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 66 from the vehicle s current path offset. The second method uses a more complicated approximation for state update equation (3.15) that relies on the predicted future lateral offsets from the previous MPC optimization to inform the current optimization: ( ) 1 ṡ approx U x (3.28) 1 K(s)ê (k) where ê (k) is the predicted lateral offset at time step k from the optimal trajectory computed in the previous controller execution. This approach is similar to the successive linearizations technique that will be explored in Chapter 4 to approximate the nonlinear behavior of tire saturation. When the optimal control problem solution does not vary significantly between executions of the controller, this provides an accurate approximation of state update equation (3.15) throughout the prediction horizon regardless of the vehicle s lateral offset from the reference path. The difficulty comes when the solution to the optimal control problem changes drastically between executions of the controller, as might occur when a new obstacle enters the prediction horizon. Future work will address these potential challenges. 3.4 Quadratic Environmental Slack Cost To ensure feasibility of the optimal control problem, state constraints in MPC implementations are often softened using slack variables as described by Maciejowski [5] and many others. As presented in Chapter 2, the safe driving envelope constraints are softened with slack variables to ensure a feasible solution to optimization problem (2.25) always exists. The l 1 norm provides an exact penalty function for the slack variables which preserves the optimal MPC behavior whenever the state constraints can be enforced as observed by Oliverira and Biegler [19] and further examined by Scokaert and Rawlings [66]; therefore, the l 1 norm was initially chosen for the penalty functions for the safe envelope slack variables. However, interactions with the environmental envelope boundary using the l 1 norm results in aggressive steering interventions as illustrated in Figure Human drivers reported these

86 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 67 Steer cmd (deg) Distance (m) Vehicle Speed (m/s) Distance (m) Time (s) Start Vehicle Path Obstacles Time (s) Actual Driver Figure 3.11: Experiment: Driver bouncing off environmental boundary; l 1 -norm slack penalty function leads to harsh interventions by controller harsh interventions felt uncomfortable. To address this issue, a quadratic penalty function is used instead for the environmental slack variables. Quadratic penalty functions are also commonly used in the literature as described by Scokaert and Rawlings [66] and more recently by Zeilinger et al. in the context of robust stability [76]. The controller s performance using the quadratic penalty functions is illustrated in Figure The steering interventions by the controller are much smoother when the vehicle is up against the environmental envelope. With this change, human drivers report a more pleasant experience when the controller intervenes to enforce the environmental envelope.

87 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 68 Steer cmd (deg) Distance (m) Vehicle Speed (m/s) Distance (m) Time (s) Start Vehicle Path Obstacles Time (s) Actual Driver Figure 3.12: Experiment: Driver bouncing off environmental boundary; quadratic slack penalty function leads to smooth interventions by controller

88 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS Biasing to Match the Driver A key design consideration for the envelope controller presented in Chapter 2 is the lack of assumptions about the future behavior of the driver. This avoided the need for driver models in the problem formulation. To this end, the controller only considers the driver s present (and previous) steering commands in the optimal control problem. However, due to inherent inertia in the steering system and the driver s self, driver commands in the near term horizon can be predicted and assumed without imposing significant limitation on the driver s autonomy. This section explores the use of these near term predicted driver commands in the problem formulation. Assuming a constant speed hand wheel motion is a simple approach to predicting near term driver commands. future driver commands can be approximated as: where δ (k) driver Given the driver s current and previous commands, ( ) δ (k) driver = δ() driver + k δ () driver δ( 1) driver (3.29) is the predicted driver command at the kth step into the prediction horizon and δ () driver and δ( 1) driver are the driver s current and previous steering commands, respectively. As was presented in Chapter 2, these driver steering commands can be converted to front tire force commands using (2.27). With these predicted driver front tire force commands, objective term (2.25a) can be adapted to bias the optimal trajectory to identically match the driver over several time steps into the prediction horizon: i ρ (i) F (i) yf,driver F (i) yf,opt (3.3) where ρ (i) is a tunable gain and F (i) yf,driver and F (i) yf,opt are the driver s commanded and the MPC optimal front tire forces at the ith step into the horizon. In (2.25a) presented

89 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 7 in Chapter 2, ρ (i) was implicitly defined as: 1 i = ρ (i) = otherwise (3.31) The following figures illustrate the effect of ρ (i) on the predicted trajectories and closed-loop controller behavior. In Figures 3.13 and 3.14, the magnitude of ρ () is varied while ρ (i) = for i >. As expected, increasing ρ () results in more time spent matching the driver at the expense of harsher steering commands and aggressive predicted trajectories up against the environmental envelope. Figure 3.15 illustrates the use of biasing to the driver further along the prediction horizon with ρ (i) for i >. As shown, this approach and tuning provide a good balance between smooth steering and driver autonomy. In addition, as is explored in the Section 3.6, this biasing further down the prediction horizon enables the development of haptic signals to improve cooperation between the driver and controller.

90 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 71 Environmental Envelope 1 [m] Actual Traj Safe Short Term Traj [m] Safe Long Term Traj Steering Command Driver Cmd Actual Cmd δ [deg] FFB [Nm] [s] Figure 3.13: Simulation: Avoidance scenario with the driver turning into the obstacle and road boundary, with ρ () = 1.2 and ρ (i) = for i >. The steering command and predicted trajectory are smooth but the steering command rarely tracks the driver.

91 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 72 Environmental Envelope 1 [m] Actual Traj Safe Short 7 Term 8Traj [m] Safe Long Term Traj Steering Command Driver Cmd Actual Cmd δ [deg] FFB [Nm] [s] Figure 3.14: Simulation: Avoidance scenario with the driver turning into the obstacle and road boundary, with ρ () = 4.8 and ρ (i) = for i >. Steering matches driver but at the expense of aggressive steering commands.

92 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 73 Environmental Envelope 1 [m] Actual Traj Safe Short 7 Term 8 Traj [m] Safe Long Term Traj Steering Command Driver Cmd Actual Cmd δ [deg] FFB [Nm] [s] Figure 3.15: Simulation: Avoidance scenario with the driver turning into the obstacle and road boundary, with ρ () = 1.2, ρ (1:3) =.5, and ρ (i) = for i > 3. This tuning provides a good balance between driver autonomy and smooth steering.

93 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS Cooperation Between Controller and Driver The envelope controller proposed may be required to significantly augment the driver s steering command to ensure the safe operation of the vehicle. During these interventions, the driver may feel disconnected from the vehicle especially if the driver is unaware of the need for the intervention. This may occur if the driver is distracted or unfamiliar with nonlinear vehicle dynamics in the case of low friction surfaces or excessive speeds. However, it is during these exact situations for which the proposed envelope controller is most valuable; therefore, improving the cooperation between controller and driver in these situations is important. The following presents a method of communicating the planned actions of the controller to the driver which is simple and intuitive. This approach leverages hardware commonly found on vehicles equipped with steer-by-wire: a force feedback (FFB) steering system. Due to the mechanical decoupling of the hand wheel and the road wheels in a steer-by-wire vehicle, the steering feel to the driver through the hand wheel is drastically altered. It has been shown in literature that drivers rely on this steering feel to perform vehicle maneuvers as demonstrated by Forsyth and MacLean [31]. To restore the feel of a conventional steering system in a steer-by-wire vehicle, an artificial steering feel can be created from torques applied to the hand wheel using the FFB steering system. The next section describes how a FFB steering system can be used to emulate the basic components of this artificial steering feel, focusing specifically on design considerations to appropriately handle situations where large steering augmentations may result from the envelope controller described in Chapter 2. The last section presents how little to no modification to the control problem presented in Chapter 2 is required to generate a signal suitable for haptic feedback to the driver to enable better cooperation between the driver and the controller. This cooperation is demonstrated in experimentation at end of this section.

94 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS Steering Feel Design for Active Steering The work presented in this section was done in collaboration with Avinash Balachandran and was originally presented at the ASME Dynamic Systems and Control Conference in September, 214 [8]. In a conventional steering vehicle, the torque at the hand wheel (τ hw ) is generated from a combination of the dynamics of the steering system and forces acting at the road wheels as illustrated in Figure Balachandran and Gerdes [9] present the following dynamic model of this system suitable for use in an artificial steering feel emulator: τ hw = b steer system δ + Jsteer system δ + Ktire moment (τ jack + τ align ) (3.32) where b steer system and J steer system are the steering system inertia and damping, respectively, K tire moment is the tire moment gain, τ jack is the jacking torque, and τ align is the aligning moment. The tire moment gain (K tire moment ) varies for different steering systems, suspension geometries, and steering ratios. The jacking torque (τ jack ) felt at the hand wheel is a function of the front normal tire forces (F zf ) acting on the vehicle. Jacking torque creates the centering feel for a driver at low speeds. The aligning moment (τ align ) is generated by the front lateral tire forces (F yf ) acting on the vehicle, and dominates the steering feel at higher speeds. The steering angle of the front road wheels (δ) directly influences both the jacking Figure 3.16: Conventional Steering System

95 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 76 Figure 3.17: Force Feedback (FFB) Steering System torque and the aligning moment felt at the hand wheel. In a steer-by-wire vehicle, the hand wheel does not have the feel of a conventional vehicle. Instead, this feel can be emulated using a FFB steering system. Figure 3.17 illustrates the FFB system hardware whose dynamics can be modeled as: τ hw,ffb = τ motor + J FFB system δhw + b FFB system δhw (3.33) where b FFB system and J FFB system are the FFB steering system inertia and damping, respectively, τ motor is the FFB motor torque, and δ hw is the hand wheel angle. It should be noted that δ hw = δ driver /K steer where K steer is the steering ratio and δ driver is the road wheel angle commanded by the driver via the hand wheel as previously described in Chapter 2 (2.2). Combining (3.32) and (3.33) provides an expression for τ motor which emulates the τ hw of the conventional steering system while canceling the dynamic effects of the FFB system to give an artificial steering feel on a steer-by-wire vehicle. When the envelope controller identically matches the driver s command, the hand wheel agrees with the road wheel steer angle, i.e. δ = δ driver, and the approach to emulating steering feel outlined above can be directly applied. However, when the envelope controller augments the driver s steering command as illustrated by Figure 3.18, it is unclear if feeding back the actual road wheel forces to the hand wheel still provides the driver with useful information. To evaluate this affect of steering augmentation on steering feel, experiments were conducted using X1, which is equipped with a FFB steering system that generates

96 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 77 Figure 3.18: Steering feel during augmentation artificial steering feel at the hand wheel. Precise encoders obtain the hand wheel angle and road wheel angle while the road wheel angle derivative is approximated by differencing the encoder signal. Figure 3.19 illustrates performance of the combined envelope controller and steering feel emulator using the conventional steering feel model. As is shown, feeding back the actual road wheel forces to the hand wheel generates a hand wheel torque that is in opposition to the action of the envelope controller, enhancing the disparity between the driver s and the controller s steering commands. Collision with the environment is still safely avoided as the envelope controller simply commands a larger augmentation to overcome the driver s conflicting command; however, the driver is receiving mixed signals from the vehicle and cooperation between driver and controller is not observed. To resolve these conflicting signals to the driver, the steering feel emulator should be designed to feedback the forces that would be generated by the driver s commanded steer angle (δ driver ) rather than the forces generated by the actual steer angle of the road wheels (δ). Figure 3.2 illustrates the affect of of this modification to the steering feel emulator during an similar scenario as Figure As is shown, generating the artificial steering feel based on the fictional forces associated with the driver s steering command encourages the driver to act in the save direction as the envelope controller, improving the agreement between driver and controller.

97 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 78 Vehicle Path 12 Start [m] 14 Front Tireforce [kn] Steering Feel Torque [Nm] Roadwheel Angle [deg] [m] 5 5 Driver Cmd Actual 2Cmd Time [s] Steer Feel Driver Cmd 1 Actual Cmd Figure 3.19: Experiment: Steering feel emulator using δ, the actual road wheel angle, to generate the artificial steering feel

98 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 79 Vehicle Path 12 Start [m] 14 Front Tireforce [kn] Steering Feel Torque [Nm] Roadwheel Angle [deg] [m] 5 Driver Cmd Actual Cmd Steer Feel 18 Time [s] Driver Cmd 1 Actual Cmd Figure 3.2: Experiment: Steering feel emulator using δ driver, the driver s commanded road wheel angle, to generate the artificial steering feel

99 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS Predictive Haptic Feedback As demonstrated in the previous section, haptic feedback from a FFB steering system can help improve the cooperation between a driver and controller during active steering interventions. In this section, it will be demonstrated that haptic feedback can additionally be used to improve driver and controller cooperation prior to augmentation of the driver s steering command, possibly to an extent where the steering augmentation no longer becomes necessary. The envelope controller s predictive nature and objective to identically match the driver s command in the initial steps of the prediction horizon are key properties that enable predictive haptic feedback. As is commonly done in MPC implementations, a planned input trajectory over the entire prediction horizon is computed at each execution of the controller, but only the input on the first time step is actually applied to the plant with the remaining planned inputs discarded. For the envelope controller presented in Chapter 2, this remaining input trajectory can be used to generate a haptic signal to inform the driver of the controller s planned course of action. Figures 3.21 and 3.22 illustrate the planned vehicle states and input sequences of the envelope controller in response to an approaching obstacle at two instances in time, t = 1 and t = 2. As seen in these figures, the optimal trajectory computed at t = 1 shows a planned, non-zero steer command 1 [s] into the prediction horizon, but the optimal steer command computed 1 [s] later still matches the driver s command of zero steer angle. This is expected. At t = 1, the controller makes no assumptions about the future behavior of the driver, but 1 [s] later new information about the driver s steering command becomes available, and this is considered in the generation of the new optimal trajectory. Even though at both time instances the optimal steering command applied to the vehicle is δ =, the severity of the optimal input trajectory computed at t = 2 suggests a pending steering augmentation by the controller. Therefore, it would be desirable for a predictive haptic signal to be stronger for the case illustrated in Figure 3.22 than Figure A simple approach is to compute the predictive

100 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS Environmental Envelope [m] 5 1 Actual Traj Safe Short 6 Term Traj [m] Safe Long Term Traj Steering Command 5 δ [deg] Driver Cmd Actual Cmd [s] Figure 3.21: Simulation: At t = 1 [s], controller matches driver s steer command but plans to augment the command thereafter with almost 2 [deg] of augmentation planned for the future t = 2 [s]

101 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS Environmental Envelope [m] 5 1 Actual Traj Safe Short 6 Term Traj [m] Safe Long Term Traj Steering Command 5 δ [deg] Driver Cmd Actual Cmd [s] Figure 3.22: Simulation: At t = 2 [s], controller still matches driver s steer command despite previously planning an augmentation. The planned trajectory has increased in severity relative to the planned trajectory at t = 1 [s]

102 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 83 haptic feedback torque as: τ haptic = K haptic ( δ (k haptic) opt ) δ (k haptic) driver (3.34) where τ haptic is the predictive haptic feedback torque, K haptic is a tunable gain, δ (k haptic) driver and δ (k haptic) opt are the anticipated driver s commanded steer angle and optimal planned steer angle at the k haptic time step into the prediction horizon, respectively. Equation (3.34) provides a directional haptic signal that nudges the driver to steer in agreement with the controller. In practice, this signal is also saturated in the interest of driver comfort as well as to reflect the limited torque capabilities of the haptic feed back steering system. Figure 3.23 illustrates the time series of the haptic signal generated using (3.34) with k haptic = 4 for the same obstacle avoidance scenario used in Figures 3.21 and As is shown, the haptic signal nudges the driver in the direction of the steering augmentation and grows in magnitude as the controller intervention time approaches. Saturation of the haptic signal is also illustrated. Choice of k haptic provides another tuning knob in shaping the haptic signal. Increasing k haptic provides a subtle distinction in the haptic signal as illustrated in Figure Comparing the haptic signals from Figures 3.23 and 3.24, it is seen that the larger k haptic produces a stronger and earlier haptic signal. Earlier and strong signals may be preferred to give advanced warning to the driver and encourage tighter cooperation between driver and controller. Alternatively, a delayed haptic signal is less intrusive to the driver. These are common haptic design considerations and the optimal control problem underlying the envelope controller provides flexibility in making these design decisions. However, this simulation does not include a model of the haptic signal s influence on the driver; therefore, to evaluate the performance with a driver-in-the-loop, experimentation on X1 is conducted with the results presented in Figure In this test, the driver works with the envelope controller and allows the FFB system to guide him through the environment. Even though augmentations of his steering commands are still required, they are small and always in the same direction as the driver s command. As a result, the envelope controller is effectively applying a positive scaling to

103 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 84 Environmental Envelope 1 [m] Actual Traj Safe 7 Short Term 8 Traj [m] Safe Long Term Traj Steering Command Driver Cmd Actual Cmd τ Haptic δ [deg] FFB [Nm] [s] Figure 3.23: Simulation: Haptic signal generated from (3.34) with K haptic = 15 [Nm/rad] and k haptic = 4 for single obstacle avoidance scenario using an open-loop driver model

104 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 85 Environmental Envelope 1 [m] δ [deg] FFB [Nm] Actual Traj Safe Short Term Traj [m] Safe Long Term Traj Steering Command Driver Cmd Actual Cmd τ Haptic [s] Figure 3.24: Simulation: Haptic signal generated from (3.34) with K haptic = 15 [Nm/rad] and k haptic = 6 for single obstacle avoidance scenario using an open-loop driver model. Note the start delay in the haptic signal relative to Figure 3.23

105 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 86 the steering ratio to avoid the obstacles in the environment and never has to reverse the steering direction relative to the driver. Initial simulations and experiments show promise for this technique of deriving haptic feedback signals directly from the MPC solution. Future work will focus on user studies to validate the effectiveness of this haptic feedback signal in promoting cooperation between driver and controller.

106 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 87 Distance (m) Start Vehicle Path Obstacles Distance (m) Steer cmd (deg) 5 5 Driver Actual Vehicle Speed (m/s) Time (s) Time (s) Figure 3.25: Experimentation: With predictive haptic feedback enabled, augmentation of the driver s steer command by the controller is reduced as the driver and controller cooperate to navigate the environment. Haptic signal was generated from (3.34) with K haptic = 15 [Nm/rad] and k haptic = 4

107 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS Discussion This chapter presented a number of additional complexities and extensions to the safe driving envelope controller. Many of these additions were motivated by simulation and experimental results. Among these are improved modeling of the interaction between the vehicle and environment when the vehicle is operating near the boundaries of the environmental envelope, as well as modified penalty functions to improve performance at this boundary in the presence of model/plant mismatch. The addition of a terminal state cost was motivated by model predictive control theory which improves the stability and leads to planned vehicle trajectories that are intuitive. The last two additions, curved roads and haptic feedback, required almost no additional changes to the controller structure, but expanded the types of environments in which the controller could be applied and improved the cooperation between controller and driver. Despite the added complexity, the underlying optimal control problem is still formulated as a set of quadratic programs which can be quickly and reliably evaluated in real-time.

108 Chapter 4 Predicting Rear Tire Saturation The proposed envelope controller has been demonstrated to successfully share control with a human driver, augmenting the driver s steering commands to avoid collisions and prevent loss of control. The extent to which this can be done is limited by the controller s ability to anticipate dangerous scenarios in order to appropriately intervene and steer the vehicle to safety. However, the nonlinear nature of tire dynamics poses a challenge in predicting and modifying vehicle behavior in real-time. This chapter describes another extension to the proposed envelope controller that uses an improved method for modeling the rear tire dynamics. Enabled by this modeling improvement, the envelope controller can more accurately predict, and possibly prevent, saturation of the rear tires. As referenced throughout this dissertation, the nonlinearity of the rear tires plays a central role in vehicle stability, and an improved model of this physical property improves the performance of the controller in highly dynamic situations. Like the extensions presented in the previous chapter, this improved modeling approach comes at the cost of complexity. However, simulation and experimentation demonstrate improved performance that justifies this additional complexity. In addition, the simulation and experimental results illustrate interesting interactions between the occasionally competing objectives of vehicle stability and collision avoidance. This improved modeling technique enables the controller to identify situations in which violation of the safe driving envelopes is unavoidable using only steering 89

109 CHAPTER 4. PREDICTING REAR TIRE SATURATION 9 actuation. As will be discussed in Chapter 5, this result establishes the foundation for a comprehensive envelope controller capable of ensuring vehicle safety through augmentation of the driver s steering, throttle, and brake commands. The majority of this chapter including the simulation results using P1 was originally presented at the American Control Conference in Portland, OR in June, 214 [23] with the balance of the chapter focusing on subsequent experimental validation using X Introduction As described in Chapter 2, the rear tire model used by the MPC controller in the near-term prediction horizon follows the approach presented by Beal and Gerdes [1]. This approach works well for short time scales where the state of the tire does not change significantly. However, over the course of a few seconds, which is the time scale required for obstacle avoidance, this assumption does not hold, and the tire state can change dramatically while maneuvering around obstacles. Therefore, a different approach is required if the nonlinearity of the rear tires is to be captured throughout the prediction horizon of the proposed envelope controller. One approach is to abandon the convexity requirement for the vehicle model and directly consider the nonlinear tire model in the optimization. This approach comes at the expense of run-time performance and solver reliability, limiting its use in a real-time, safety critical application. Another approach approximates nonlinear plant dynamics through a technique known as successive linearizations. In these schemes, the controller optimizes the plant s trajectory using a linear time-varying plant model derived from the optimized trajectory from the previous time step. This enables consideration of nonlinear dynamics while retaining the performance and reliability of convex optimization with affine models. Falcone et al. [25] use the technique of successive linearizations in a real-time trajectory tracking controller intended for a fully autonomous vehicle. Vehicle stability is enforced through an ad hoc constraint on maximum front tire slip angle. In their approach, the entire vehicle model is linearized, requiring a non-trivial amount of computational time to compute the linearizations at each time step. The resulting Linear

110 CHAPTER 4. PREDICTING REAR TIRE SATURATION 91 Time Varying (LTV) MPC scheme enables a 15x improvement in controller execution run-time over an equivalent implementation using the nonlinear vehicle model directly with a nonlinear optimization solver, a Sequential Quadratic Program (SCP) solver. Building upon this initial work, Falcone et al. [26] proposed a similar LTV MPC scheme based on successive linearizations that replaced the application specific, ad hoc stability criteria with a more general stability constraint. This general stability criteria involves an additional convex constraint on the states and inputs in the MPC formulation and is provably stabilizing for a wide range of nonlinear systems. Applications of the successive linearizations technique also extend beyond real-time control. For example, Timing and Cole [7] use successive linearizations as an efficient alternative to nonlinear optimization in an off-line algorithm for generating racing lines. In this chapter, the successive linearizations technique improves the rear-tire model used by the MPC controller, enabling the controller to explicitly consider rear tire saturation in predicting future vehicle behavior relative to the safe driving envelopes presented in Chapter 2. Simulation results demonstrate more accurate prediction of situations in which vehicle stability needs to be sacrificed in order to ensure collision avoidance. The chapter concludes with experimental results of aggressive maneuvers that demonstrate the usefulness of this approach in ensuring vehicle safety in highly dynamic maneuvers in constrained environments. 4.2 Modified MPC Plant Model The vehicle model used in the online MPC controller is the same constant speed, planar bicycle model presented in Chapter 2 and illustrated again in Figure 4.1 for reference. The model modification involves the treatment of the rear tires. Recall from Chapter 2, a linearization of the brush tire model (2.2) at a given rear tire slip angle (ᾱ r ) models rear tire force (F yr ) as an affine function of rear slip angle (α r ) as illustrated in 4.2. Following the lead of Beal and Gerdes [1], this rear tire model is linearized around the measured rear tire slip angle, α r, in the near term of the prediction horizon. Over

111 CHAPTER 4. PREDICTING REAR TIRE SATURATION 92 b U y a β α f δ F yr α r r e U x F yf s Reference Line Figure 4.1: Bicycle model schematic Brush tire model Affine approx Tire Lateral Force Fy F y α C α Tire slip angle α Figure 4.2: Brush tire model with affine approximation at ᾱ

112 CHAPTER 4. PREDICTING REAR TIRE SATURATION 93 this short horizon, the vehicle states do not change significantly, enabling accurate prediction of vehicle state propagation in the near term. The appropriate choice of ᾱ r in the remainder of the prediction horizon is the focus of this chapter. Two possibilities will be evaluated and compared. The first is simply choosing ᾱ (k) r =, which results in a linear tire model for time steps k in the latter portion of the horizon as was used in Chapter 2. The second approach is to choose ᾱ (k) r = ˆα r (k), where ˆα r (k) is the predicted rear slip angle at time step k from the optimal trajectory computed in the previous controller execution. This use of successive linearizations provides an approximation to the nonlinear tire behavior throughout the entire prediction horizon. For both of these approaches, the equations of motion of the velocity states can be expressed as affine functions of the states and input: β = ṙ = F yf + F yr Cᾱr (β br U x ᾱ r ) r (4.1) mu [ x ( )] af yf b Fyr Cᾱr β br U x ᾱ r (4.2) I zz The position states of the vehicle are defined as presented in Chapter 2 using small angle approximations: ψ = r (4.3) ė U x ψ + U x β (4.4) ṡ U x (4.5) giving a discrete, time-varying vehicle model which can be expressed as: x (k+1) = A (k) ᾱ r,t s x (k) + B (k) ᾱ r,t s F yf (k) + d (k) ᾱ r,t s (4.6) where x = [β r ψ s e] T, k is a time step index, subscript ᾱ r denotes linearization of the rear tire model around rear slip angle ᾱ r, and subscript t s denotes discretization of the vehicle model using time step length t s.

113 CHAPTER 4. PREDICTING REAR TIRE SATURATION Comparison to Other Approaches Comparing this approach to that of Falcone et al. discussed previously, some interesting differences emerge. In both [25] and [26], Falcone et al. linearize the full vehicle model, which requires a non-trivial amount of computational time to compute the linearizations at each time step. Simplifying assumptions were used in an approximate method of generating these linearizations to reduce the computational complexity. In contrast, the approach presented in this chapter linearizes only the rear tire model, avoiding this computational complexity and the need for simplifying assumptions. Another significant difference is the tracking of a reference trajectory, which plays a important role in the stability proof of the general stability criteria presented in [26]. Since the proposed envelope controller does not restrict the human driver to a predefined path, a tracked reference trajectory is not available and the assumptions of the more general stability criteria are not met. Therefore, an application specific stability criteria is required and is achieved through the invariant set defined by the stable handling envelope presented in Chapter Safe Driving Envelopes The safe driving envelopes used in this chapter are identical to those presented in Chapter 2 and are briefly described here for reference Stable Handling Envelope Originally presented by Beal and Gerdes, the stable handling envelope defines limits on the states describing the vehicle s motion as illustrated in Figure 4.3. This envelope reflects the maximum capabilities of the vehicle s tires so at any point within this envelope, there exists a steering command to safely remain inside, ensuring stability [1]. The maximum steady-state yaw rate defines bounds 2 and 4 in Figure 4.3. The final two bounds of the vehicle envelope serve to limit the rear slip angle to the angle at which lateral force saturates. Given this bound on rear slip angle, a maximum

114 CHAPTER 4. PREDICTING REAR TIRE SATURATION Yaw Rate r Sideslip β.1.2 Figure 4.3: Stable handling envelope sideslip can be defined and serves as the basis for bounds 1 and 3 in Figure 4.3. The stable handling envelope can be compactly expressed as the inequality: Hsh x (k) Gsh (4.7) where x k is the vehicle state at the kth time step into the prediction horizon and subscript sh indicates the stable handling envelope Environmental Envelope The environmental envelope consists of a set of collision free tubes along the nominal path like the two illustrated in Figure 4.4. To avoid collision with the environment, the vehicle s trajectory must be fully contained within any one of these tubes. Each tube defines time-varying constraints on the lateral deviation of the vehicle from the nominal path: e (k) e (k) max 1 2 d d buffer (4.8) e (k) e (k) min d + d buffer (4.9)

115 CHAPTER 4. PREDICTING REAR TIRE SATURATION 96 where e (k) max and e (k) min indicate the lateral deviation bounds for time step k, d is the vehicle s width, and d buffer specifies a preferred minimum distance between obstacles and the vehicle to ensure driver comfort. For any two trajectories generated using vehicle model (4.6) and contained within a tube, the linear combination of those trajectories will also be contained within that same tube. Therefore, the set of collision free trajectories corresponding to a single tube is a convex set, which enables the use of fast optimization techniques to quickly identify optimal trajectories. Inequalities (4.8) and (4.9) can be compactly expressed as: where subscript env indicates the environmental envelope. H env x (k) G (k) env (4.1) A vehicle trajectory is collision free throughout the prediction horizon if and only if it satisfies inequality (4.1) for all k for any one tube in the environmental envelope. For simplicity, the environments considered in this chapter can all be represented using only one tube; however, the proposed approach easily extends to the more complicated environments discussed in Chapter 2.

116 CHAPTER 4. PREDICTING REAR TIRE SATURATION 97 e s Nominal path Obstacle Lane boundary Tube boundary a) e (1) max e min (1) e max (4) e min (4) b) e (1) max e (4) max e (4) min e (1) min Figure 4.4: The environmental envelope is a representation of a) a collection of obstacles along the nominal path using b) tubes (two of them in this example) which define a maximum (e (k) max) and minimum (e (k) min ) lateral deviation from the nominal path at each time step, k.

117 CHAPTER 4. PREDICTING REAR TIRE SATURATION MPC Formulation The controller s objectives match those presented in Chapter 2: safe vehicle operation within the safe driving envelopes while being minimally invasive to the driver and avoiding harsh interventions. Therefore, these same overall objectives are expressed as the same optimal control problem using the modified plant model: minimize F yf,driver F () yf,opt (4.11a) + ( ) 2 γ (k) F (k) yf,opt F (k 1) yf,opt (4.11b) k [σ sh σ sh ] S (k) (4.11c) + k + k sh,opt ( 2 [σ env σ env ] S env,opt) (k) (4.11d) subject to x (k+1) = A (k) ᾱ r,t s x (k) + B (k) F (k) yf,opt F yf,max Hsh x (k+1) Gsh + S (k) sh,opt H env x (k+1) G env (k+1) + S (k) env,opt F (k) yf,opt F (k 1) yf,opt F (k) k = ᾱ r,t s F (k) yf,opt + d(k) ᾱ r,t s yf,max slew (4.11e) (4.11f) (4.11g) (4.11h) (4.11i) where the variables to be optimized are the optimal input trajectory (F yf,opt ) and the safe driving envelope slack variables (S sh,opt, S env,opt ). As is commonly done in MPC, only the optimal input for the first step into the prediction horizon, F () yf,opt, is applied to the vehicle, and optimization problem (4.11) is re-solved at the next time step. Tunable parameters in this optimization are again the slack variable costs (σ sh, σ env ) and γ, which establishes the trade-off between a smooth input trajectory (4.11b) and matching the driver s present steering command (4.11a). Constraint (4.11f) reflects the maximum force capabilities of the front tires and (4.11i) reflects the slew rate limit of the vehicle steering system. Constraints (4.11g) and (4.11h) enforce the

118 CHAPTER 4. PREDICTING REAR TIRE SATURATION 99 Table 4.1: Prediction Horizon Parameters Near Term Long Term Parameter Horizon Horizon k t (k) s.1 [s].2 [s] γ (k) F (k) yf,max slew.2 [kn] 5 [kn] σ sh 6 6 σ env (Linear Model) ᾱ r (k) α r or ˆα (k) r (Approx Nonlinear Model) stable handling and environmental envelopes, respectively. These constraints are softened with slack variables, S sh,opt and S env,opt, to ensure optimization (4.11) is always feasible. With the choice of sufficiently large σ sh and σ env, cost terms (4.11c) and (4.11d) encourage zero-valued slack variables, resulting in optimal vehicle trajectories that adhere to both safe driving envelopes whenever possible. In addition, the slack variable costs are chosen such that σ env σ sh, which establishes a hierarchy between the two safe driving envelopes, prioritizing collision avoidance over vehicle stability. This trade-off will be explored in the simulations and experiments that follow. The prediction horizon used in optimization (4.11) uses different length time steps in the near and long terms of the horizon as described in Chapter 2. Table 4.1 gives the values of parameters in these two portions of the prediction horizon, which are used in the simulations that follow. 4.6 Simulation Results Simulations of the controller using both approaches to rear tire modeling demonstrate the value of approximating nonlinear rear tire behavior using the technique of successive linearizations. This simulations use the vehicle parameters listed in Table 4.2 and represent Stanford s P1 research test bed. Illustrated in Figure 4.5, this test

119 CHAPTER 4. PREDICTING REAR TIRE SATURATION 1 Figure 4.5: Stanford s P1, an all electric, throttle- and steer-by-wire research testbed vehicle proved to be an invaluable platform for preliminary work on the proposed envelope controller. Although all of the experimental results obtained from P1 have been supplanted by the more modern X1 test vehicle, the following simulations were conducted using P1 s parameters and, for legacy and nostalgia reasons, have been retained in this dissertation. In the following simulations, the arrangement of obstacles, as illustrated in Figure 4.6, requires the vehicle to execute a double lane change to avoid collision with the environment. This double-lane change maneuver conforms to ISO standard [1]. For simplicity, the effects of vehicle roll and weight transfer are ignored in the following simulations. Table 4.2: P1 Vehicle Parameters Parameter Value Units m 1725 kg I z 13 kg m 2 a 1.35 m b 1.15 m d 1.6 m C αf 57.8 kn rad 1 C αr 11 kn rad 1