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 2015 © 2015 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 AttributionNoncommercial 3.0 United States License. http://creativecommons.org/licenses/by-nc/3.0/us/ This dissertation is online at: http://purl.stanford.edu/pr371mm0121 ii 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 This dissertation is dedicated to my family. iv 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 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 100 (Hz). This approach is validated on an experimental vehicle working with human drivers to negotiate obstacles in low friction environments. vi 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 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 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 Contents Abstract v Acknowledgment vii 1 Introduction 1 1.1 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1 1.2 Shared Control of Vehicles . . . . . . . . . . . . . . . . . . . . . . . . 3 1.3 Envelope Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7 1.4 Dissertation Contributions . . . . . . . . . . . . . . . . . . . . . . . . 12 1.4.1 Combined Obstacle Avoidance and Vehicle Stability . . . . . . 12 1.4.2 Convex Approach to Shared Human-Machine Control . . . . . 12 1.4.3 Convex Approximation of Tire Nonlinearity Over a Long Horizon 13 1.4.4 Braking in Response to Steering Infeasibility . . . . . . . . . . 13 Dissertation Outline . . . . . . . . . . . . . . . . . . . . . . . . . . . 14 1.5 2 Safe Envelopes for Shared Steering Control 16 2.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16 2.2 Vehicle Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 17 2.2.1 Velocity States . . . . . . . . . . . . . . . . . . . . . . . . . . 18 2.2.2 Position States . . . . . . . . . . . . . . . . . . . . . . . . . . 21 Envelope Definitions . . . . . . . . . . . . . . . . . . . . . . . . . . . 24 2.3.1 Stable Handling Envelope . . . . . . . . . . . . . . . . . . . . 24 2.3.2 Environmental Envelope . . . . . . . . . . . . . . . . . . . . . 26 MPC Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30 2.3 2.4 x 2.4.1 Two Time Scale Prediction Horizon . . . . . . . . . . . . . . . 30 2.4.2 Convex Optimization Problem(s) . . . . . . . . . . . . . . . . 31 2.4.3 Varying Time Steps in Prediction Horizon . . . . . . . . . . . 35 2.4.4 Matching the Driver’s Command . . . . . . . . . . . . . . . . 36 2.4.5 Implementation . . . . . . . . . . . . . . . . . . . . . . . . . . 36 2.5 Experimental Validation . . . . . . . . . . . . . . . . . . . . . . . . . 41 2.6 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 45 3 Design Decisions, Justifications, and Extensions 47 3.1 Considering Vehicle Shape . . . . . . . . . . . . . . . . . . . . . . . . 48 3.2 Terminal Cost on Vehicle Heading . . . . . . . . . . . . . . . . . . . . 56 3.3 Curved Roads . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60 3.4 Quadratic Environmental Slack Cost . . . . . . . . . . . . . . . . . . 66 3.5 Biasing to Match the Driver . . . . . . . . . . . . . . . . . . . . . . . 69 3.6 Cooperation Between Controller and Driver . . . . . . . . . . . . . . 74 3.6.1 Steering Feel Design for Active Steering . . . . . . . . . . . . 75 3.6.2 Predictive Haptic Feedback . . . . . . . . . . . . . . . . . . . 80 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 88 3.7 4 Predicting Rear Tire Saturation 89 4.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 90 4.2 Modified MPC Plant Model . . . . . . . . . . . . . . . . . . . . . . . 91 4.3 Comparison to Other Approaches . . . . . . . . . . . . . . . . . . . . 94 4.4 Safe Driving Envelopes . . . . . . . . . . . . . . . . . . . . . . . . . . 94 4.4.1 Stable Handling Envelope . . . . . . . . . . . . . . . . . . . . 94 4.4.2 Environmental Envelope . . . . . . . . . . . . . . . . . . . . . 95 4.5 MPC Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 98 4.6 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . 99 4.7 Experimental Results . . . . . . . . . . . . . . . . . . . . . . . . . . . 106 4.8 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 111 xi 5 Envelope Control using Braking and Steering 112 5.1 Challenge of Longitudinal and Lateral Control . . . . . . . . . . . . . 112 5.2 Braking as Steering Feasibility Problem . . . . . . . . . . . . . . . . . 114 5.3 Braking to Prevent Envelope Violations . . . . . . . . . . . . . . . . . 118 5.4 5.5 5.3.1 Braking Policies . . . . . . . . . . . . . . . . . . . . . . . . . . 119 5.3.2 Constant Speed Assumption . . . . . . . . . . . . . . . . . . . 121 5.3.3 Ensuring the Horizon Always Recedes . . . . . . . . . . . . . . 122 5.3.4 Tire Force Coupling . . . . . . . . . . . . . . . . . . . . . . . 125 MPC Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 128 5.4.1 Friction Circle Constraint . . . . . . . . . . . . . . . . . . . . 128 5.4.2 Considering Front Longitudinal Forces Only . . . . . . . . . . 129 5.4.3 Optimal Control Problem . . . . . . . . . . . . . . . . . . . . 131 5.4.4 Feasibility of the Constant Speed Trajectory . . . . . . . . . . 134 Experimental Results . . . . . . . . . . . . . . . . . . . . . . . . . . . 135 5.5.1 Proportional Braking Policy . . . . . . . . . . . . . . . . . . . 135 5.5.2 Fixed Braking Policy . . . . . . . . . . . . . . . . . . . . . . . 140 5.6 Extension to Brake- and Throttle-By-Wire . . . . . . . . . . . . . . . 143 5.7 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 148 5.8 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 149 6 Conclusion 6.1 6.2 150 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 151 6.1.1 Fully Autonomous Vehicles . . . . . . . . . . . . . . . . . . . . 151 6.1.2 Haptic Feedback User Studies . . . . . . . . . . . . . . . . . . 152 6.1.3 Application to Racing . . . . . . . . . . . . . . . . . . . . . . 152 6.1.4 Implementable Ethics . . . . . . . . . . . . . . . . . . . . . . . 153 6.1.5 Leveraging Advances in Parallel Computing . . . . . . . . . . 153 Outlook . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 153 Bibliography 155 xii List of Tables 2.1 Vehicle Model Notation . . . . . . . . . . . . . . . . . . . . . . . . . . 23 2.2 Steering Controller Parameters . . . . . . . . . . . . . . . . . . . . . 38 2.3 X1 Vehicle Parameters . . . . . . . . . . . . . . . . . . . . . . . . . . 41 3.1 Prediction errors for recommended minimum radius curves for a lateral offset of one lane, e = 3.6 (m) . . . . . . . . . . . . . . . . . . . . . . 65 4.1 Prediction Horizon Parameters . . . . . . . . . . . . . . . . . . . . . . 99 4.2 P1 Vehicle Parameters . . . . . . . . . . . . . . . . . . . . . . . . . . 100 4.3 Maximum Speed without Collision . . . . . . . . . . . . . . . . . . . 106 5.1 Braking and Steering Controller Parameters . . . . . . . . . . . . . . 133 5.2 Proportional Braking Parameters . . . . . . . . . . . . . . . . . . . . 139 5.3 Fixed Braking Parameters . . . . . . . . . . . . . . . . . . . . . . . . 140 xiii List of Figures 1.1 Vehicle states and axes . . . . . . . . . . . . . . . . . . . . . . . . . . 8 2.1 Bike model schematic . . . . . . . . . . . . . . . . . . . . . . . . . . . 18 2.2 Brush tire model with affine approximation at α ¯ . . . . . . . . . . . . 23 2.3 Stable handling envelope . . . . . . . . . . . . . . . . . . . . . . . . . 24 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 (k) tubes (two of them in this example) which define a maximum (emax ) (k) and minimum (emin ) lateral deviation from the reference line at each time step, k. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.5 29 Representation of the environment re-evaluated a short time later (a) without a correction time step and (b) with a correction time step . . 36 2.6 Comparison of the l1 and l2 norms illustrating the different behavior (0) of the norms at small values of Fyf,driver − Fyf,opt versus larger values 37 2.7 An environment with 3 obstacles arranged in a manner that results in the worst-case number of possible tubes, 2n = 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. . . . . . . . . . . . . . . . . . . . 2.8 40 Stanford’s X1, an all electric, throttle- and steer-by-wire research testbed with automatic brakes and haptic force feedback steering system . . . xiv 40 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 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42 2.10 Double lane change (ISO 3888-1) on a low friction surface . . . . . . . 44 3.1 Simulation: Conservative behavior at road boundary as a result of the crude vehicle width model . . . . . . . . . . . . . . . . . . . . . . . . 3.2 49 Schematic of required distance from vehicle’s CG to environmental envelope for various vehicle orientations . . . . . . . . . . . . . . . . . 50 3.3 Plot of required distance from CG to environmental envelope for vari51 3.4 ous vehicle orientations . . . . . . . . . . . . . . . . . . . . . . . . . . Plot of approximation error, (d˜L − dL ), vs vehicle orientation relative to the reference line . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52 3.5 Simulation: Less intrusive behavior at road boundary when using environmental constraint (3.6) with ∆ψ0 = 8 [deg]. . . . . . . . . . . . . 3.6 Simulation: Controller properly orients the vehicle when confronted with narrowly spaced obstacles . . . . . . . . . . . . . . . . . . . . . 3.7 57 Simulation: With the terminal cost, the planned trajectories take the form of lane changes. In this example, qT = 50 . . . . . . . . . . . . . 3.9 55 Simulation: Without a terminal cost, the planned trajectories arc into the road boundaries . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.8 54 59 Curvature at point p that is located at a distance s along the reference path is K(s) = 1 , R(s) where K > 0 for left hand turns . . . . . . . . . 61 3.10 Simulation: Controller augments driver’s command to navigate around an obstacle while negotiating a turn . . . . . . . . . . . . . . . . . . . 63 3.11 Experiment: Driver bouncing off environmental boundary; l1 -norm slack penalty function leads to harsh interventions by controller . . . 67 3.12 Experiment: Driver bouncing off environmental boundary; quadratic slack penalty function leads to smooth interventions by controller . . xv 68 3.13 Simulation: Avoidance scenario with the driver turning into the obstacle and road boundary, with ρ(0) = 1.2 and ρ(i) = 0 for i > 0. The steering command and predicted trajectory are smooth but the steering command rarely tracks the driver. . . . . . . . . . . . . . . . 71 3.14 Simulation: Avoidance scenario with the driver turning into the obstacle and road boundary, with ρ(0) = 4.8 and ρ(i) = 0 for i > 0. Steering matches driver but at the expense of aggressive steering commands. . 72 3.15 Simulation: Avoidance scenario with the driver turning into the obstacle and road boundary, with ρ(0) = 1.2, ρ(1:3) = 0.5, and ρ(i) = 0 for i > 3. This tuning provides a good balance between driver autonomy and smooth steering. . . . . . . . . . . . . . . . . . . . . . . . . . . . 73 3.16 Conventional Steering System . . . . . . . . . . . . . . . . . . . . . . 75 3.17 Force Feedback (FFB) Steering System . . . . . . . . . . . . . . . . . 76 3.18 Steering feel during augmentation . . . . . . . . . . . . . . . . . . . . 77 3.19 Experiment: Steering feel emulator using δ, the actual road wheel angle, to generate the artificial steering feel . . . . . . . . . . . . . . . 78 3.20 Experiment: Steering feel emulator using δdriver , the driver’s commanded road wheel angle, to generate the artificial steering feel . . . 79 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] . . . . . . . . . . . . . 81 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] 82 3.23 Simulation: Haptic signal generated from (3.34) with Khaptic = 150 [Nm/rad] and khaptic = 4 for single obstacle avoidance scenario using an open-loop driver model . . . . . . . . . . . . . . . . . . . . . . . . 84 3.24 Simulation: Haptic signal generated from (3.34) with Khaptic = 150 [Nm/rad] and khaptic = 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 . . . . . . . . . . . . . . . . . . . . . . . . . . xvi 85 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 Khaptic = 150 [Nm/rad] and khaptic = 4 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 87 4.1 Bicycle model schematic . . . . . . . . . . . . . . . . . . . . . . . . . 92 4.2 Brush tire model with affine approximation at α ¯ . . . . . . . . . . . . 92 4.3 Stable handling envelope . . . . . . . . . . . . . . . . . . . . . . . . . 95 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 (k) (k) example) which define a maximum (emax ) and minimum (emin ) lateral deviation from the nominal path at each time step, k. . . . . . . . . . 97 4.5 Stanford’s P1, an all electric, throttle- and steer-by-wire research testbed100 4.6 Arrangement of obstacles and road boundaries used in the following simulations. Vehicle travels in the direction indicated by the arrow. . 101 4.7 Double lane change maneuver on low friction surface at 12 [m/s] . . . 102 4.8 Double lane change maneuver on low friction surface at 16 (m/s) . . . 103 4.9 Double lane change maneuver on low friction surface at 18 (m/s) . . . 104 4.10 Comparison of the planned safe trajectory midway through the double lane change manuever on low friction surface at 18 [m/s] . . . . . . . 105 4.11 Stanford’s X1, an all electric, throttle- and steer-by-wire research testbed with automatic brakes and haptic force feedback steering system . . . 107 4.12 Experiment using X1 in double lane change on low friction µ = 0.55 with no environmental envelope violation . . . . . . . . . . . . . . . . 109 4.13 Experiment using X1 in double lane change on low friction µ = 0.55 with slight violation of the environmental envelope at instance 3 . . . 110 5.1 Controller plans a single lane change in response to blocked lane . . . 115 5.2 Predicted envelope violations for same maneuver at three different vehicle speeds . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 117 xvii 5.3 Maximum predicted envelope violation for the same maneuver as a function of vehicle speed . . . . . . . . . . . . . . . . . . . . . . . . . 118 5.4 Block diagram of steering only envelope controller presented in previous chapters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 119 5.5 Block diagram of braking and steering envelope controller . . . . . . . 120 5.6 Obstacle blocking the lane enters the prediction horizon as the vehicle travels at 8 (m/s) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 122 5.7 Braking in excess of (5.4) moves the end of the prediction horizon to toward the vehicle causing the obstacle to leave the horizon . . . . . . 123 5.8 Brake acceleration required to fix the end of the prediction horizon in space . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 124 5.9 Friction circle concept illustrated using a g-g diagram . . . . . . . . . 126 5.10 Coupled tire force model . . . . . . . . . . . . . . . . . . . . . . . . . 127 5.11 Friction circle approximated as the intersection of n = 12 half-spaces 130 5.12 An experimental ISO 3888-2 double lane change on a low friction surface (µ ≈ 0.5) without braking . . . . . . . . . . . . . . . . . . . . . . 136 5.13 An experimental ISO 3888-2 double lane change on a low friction surface (µ ≈ 0.5) with braking in proportion to predicted envelope violation137 5.14 Predicted envelope violation and corresponding predicted rear tire lateral forces at instance 1 from the experiment illustrated in Figure 5.13 138 5.15 Predicted envelope violation and corresponding predicted rear tire lateral forces at instance 2 from the experiment illustrated in Figure 5.13 138 5.16 Predicted envelope violation and corresponding predicted rear tire lateral forces at instance 3 from the experiment illustrated in Figure 5.13 139 5.17 An experimental ISO 3888-2 double lane change on a low friction surface (µ ≈ 0.5) with a fixed brake amount in response to predicted envelope violation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 142 5.18 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 5.19 Friction circle for the front axle during the simulated panic brake maneuver on a low friction surface (µ ≈ 0.5) with Tbrake = 9 . . . . . . . 146 5.20 Friction circle for the front axle during the simulated panic brake maneuver on a low friction surface (µ ≈ 0.5) with Tbrake = 0 . . . . . . . 147 xix 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 [60]. This mobility comes at a cost: in 2012, 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 [30] [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,510 in 2005 to 32,367 in 2011, 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 70 percent [18], and these systems are estimated to have saved 1,144 lives in 2012 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 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 2001 to 2007 texting alone led to 16,000 additional driving fatalities. From 2005 to 2008, 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 2013, Nissan Motor Co. introduced the first production vehicle, the Inifiniti Q50, 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 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 2012, the average number of vehicle miles traveled per fatality was just shy of 90 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, 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 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. 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. CHAPTER 1. INTRODUCTION 1.3 7 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. [40] 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 [10] 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 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 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 CHAPTER 1. INTRODUCTION 10 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 [10], 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. 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. CHAPTER 1. INTRODUCTION 1.4 12 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. 1.4.1 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. 1.4.2 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 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. 1.4.3 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 [10] 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. 1.4.4 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. CHAPTER 1. INTRODUCTION 1.5 14 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 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. 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 2014 [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 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 CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL αf a b Uy αr Fyr β r 18 δ Fyf Ux e ∆ Reference Line s 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. 2.2.1 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 Ux Uy Ux (2.1) (2.2) where Uy and Ux are the lateral and longitudinal velocities in the body fixed frame, respectively, and the assumption that Ux Uy gives the simplified expression. Assuming Ux is constant, the vehicle’s velocity states have the following equations of motion: CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL Fyf + Fyr β˙ = −r mUx aFyf − bFyr r˙ = Izz 19 (2.3) (2.4) where Fy[f,r] is the lateral tire force of the [front, rear] axle, m is the vehicle mass, Izz 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 αr ar = arctan β + −δ Ux ar ≈ β+ −δ Ux br = arctan β − Ux br ≈ β− Ux (2.5) (2.6) where small angle approximations give linear expressions. This approximation is validated by the stability constraints used by the controller as discussed in Section 2.3.1. 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 Fy[f,r] and α[f,r] : Fy 2 Cα −Cα tan α + 3µF | tan α| tan α z 3 Cα 3µFz 3 = − 27µ2 F 2 tan α, |α| < arctan Cα z −µF sgn α, otherwise z = ftire (α) (2.7) where µ is the surface coefficient of friction, Fz[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 CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL 20 describes the vehicle’s behavior by using tire forces and not steer angles. Front tire force (Fyf ) serves as the input to the model and is mapped to δ using (2.5) and (2.7): δ = β+ ar −1 − ftire (Fyf ) Ux (2.8) −1 where real-time estimates of β and r are assumed to be available and ftire is computed numerically and implemented as a 2D lookup table with inputs of rear slip angle and surface friction estimate. Use of Fyf 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 (Fyr ) as an affine function of αr : Fyr = F¯yr − C¯α¯ r (αr − α ¯r) (2.9) ¯ r . This approxwhere F¯yr = ftire (¯ αr ) and C¯α¯ r is the equivalent cornering stiffness at α imation 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 [10]. 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 [10]. The equations of motion of the velocity states can now be expressed as affine functions of the states and input, Fyf : β˙ = r˙ = Fyf + F¯yr − C¯α¯ r β − br Ux −α ¯r mUx h aFyf − b F¯yr − C¯α¯ r β − Izz br Ux −r i −α ¯r (2.10) (2.11) CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL 2.2.2 21 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) e˙ = Ux sin (∆ψ) + Uy cos (∆ψ) (2.13) s˙ = Ux cos (∆ψ) − Uy sin (∆ψ) (2.14) Using small angle assumptions for ∆ψ and β, the above nonlinear equations can be approximated as linear functions of the vehicle states: e˙ ≈ Ux ∆ψ + Ux β (2.15) s˙ ≈ Ux − Ux β∆ψ ≈ Ux (2.16) where, for small values of β and ∆ψ, the product β∆ψ ≈ 0. 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.10), (2.11), (2.12), (2.15), and (2.16), a continuous state-space representation of the vehicle model can be expressed as: x˙ = Ac (¯ αr ) x + Bc Fyf + dc (¯ αr ) (2.17) CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL 22 with h iT x = β r ∆ψ s e ¯α bC ¯r mUx 2 C¯ − mUα¯ rx bC¯α¯ r Izz Ac (¯ αr ) = 0 0 Ux Bc = h dc (¯ αr ) = h 1 mUx 2 a Izz ¯α F¯yr +α ¯r C ¯r mUx −1 ¯ 0 − bIzzCUα¯xr 0 1 0 0 0 0 Ux 0 0 0 0 0 0 0 0 0 0 iT 0 0 0 ¯α ¯r C b(F¯yr +α ¯r ) − Izz 0 Ux iT 0 where subscript c denotes a continuous time model and Ac (¯ αr ) indicates matrix Ac is linearized around α ¯r. CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL 4 1 x 10 Brush tire model Affine approx Tire Lateral Force Fy 0.5 α 0 −0.5 Fy -Cα −1 Tire slip0angle α −0.2 −0.15 −0.1 −0.05 0.05 0.1 0.15 0.2 Figure 2.2: Brush tire model with affine approximation at α ¯ Table 2.1: Vehicle Model Notation Description Symbol Steer angle δ Longitudinal Speed Ux Lateral Speed Uy Sideslip β Yaw rate r Heading deviation from path ∆ψ Lateral deviation from path e Distance along the path s Lateral tire force on [front,rear] axle Fy[f,r] Tire slip angle on [front,rear] axle α[f,r] Units rad m/s m/s rad rad/s rad m m N rad 23 CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL 2.3 24 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. 2.3.1 Stable Handling Envelope The stable handling envelope was originally presented by Beal and Gerdes [10]. 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. 0.6 4 0.4 Yaw Rate r 0.2 3 0 1 −0.2 −0.4 −0.6 2 −0.2 −0.1 Sideslip 0 β 0.1 0.2 Figure 2.3: Stable handling envelope A steady-state analysis can be used to determine an appropriate bound on yaw 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: rss = Fyf + Fyr mUx (2.18) Neglecting the effects of weight transfer and assuming zero longitudinal tire forces, the following relationship holds: Fyf,max + Fyr,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: rss,max = gµ Ux (2.20) 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 mgµ a = arctan 3 C αr 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 Ux (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 Ux are available, the vehicle envelope 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: Hsh x(k) ≤ Gsh (2.23) with − Ubx 0 0 0 0 1 0 0 0 Hsh = −1 b 0 0 0 Ux 0 −1 0 0 0 αr,sat rss,max Gsh = α r,sat rss,max 1 where subscript sh denotes the stable handling envelope and x(k) indicates the vehicle state at the kth time step into the prediction horizon. 2.3.2 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. 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: Henv x(k) ≤ G(k) env (2.24) CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL 28 with 0 0 0 0 1 Henv = Genv 0 0 0 0 −1 (k) emax − 21 d − dbuffer = (k) 1 −emin − 2 d − dbuffer (k) (k) where the subscript env denotes the environmental envelope, emax and emin indicate the lateral deviation bounds for time step k, d is the vehicle width, and dbuffer specifies a preferred minimum distance between obstacles and the vehicle to ensure driver comfort. The additional distance of dbuffer 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. CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL e Reference line Obstacle Lane boundary s 29 Feasible gap Tube a) k = 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 b) c) (1) e max (1) e min d) (1) e max (4) e max (4) e min (4) e max (4) e min (1) e 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 (k) (k) define a maximum (emax ) and minimum (emin ) lateral deviation from the reference line at each time step, k. CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL 2.4 30 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. 2.4.1 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, ts,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, 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 Tsplit small time steps of size ts,MPC = 0.01 (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 ts,long = 0.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]. 2.4.2 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 (0) input for the first step into the prediction horizon, Fyf,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: CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL minimize (0) Fyf,driver − Fyf,opt 2 X (k) (k−1) + γ (k) Fyf,opt − Fyf,opt 32 (2.25a) (2.25b) k + X + X (k+1) [σsh σsh ] Ssh,opt (2.25c) 2 (l) [σenv σenv ] Senv,opt (2.25d) k l (k) (k) (k) (k) subject to x(k+1) = Ad x(k) + Bd Fyf,opt + dd (k) Fyf,opt ≤ Fyf,max (k+1) Hsh x(k+1) ≤ Gsh + Ssh,opt (2.25e) (2.25f) (2.25g) k = 0 . . . (T − 1) (l) (l) Henv x(l) ≤ Genv + Senv,opt l = (Tsplit + 1) . . . T Fyf,opt (i) − Fyf,opt (i−1) ≤ Fyf,max i = 0 . . . Tsplit (2.25h) slew (2.25i) where the variables to be optimized are the optimal input trajectory (Fyf,opt ) and the safe driving envelope slack variables (Ssh,opt , Senv,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 CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL 33 given by: γ (k) = γnear 0 ≤ k < Tsplit γ otherwise long (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 Fyf,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 Fyf,driver : Fyf,driver ar = ftire β + − δdriver Ux (2.27) Constraints (2.25g) and (2.25h) enforce the stable handling and environmental envelopes, respectively. These constraints are softened with slack variables, Ssh,opt and Senv,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, 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: (k) (k) (k) (k) (k) (k) Ad Bd dd Ac Bc dc = exp t(k) s ? ? ? 0 0 0 with (k) (k) Ac dc = i h Ac (αr(0) ) dc (αr(0) ) [Ac (0) dc (0)] (2.28) 0 ≤ k < Tsplit Tsplit ≤ k ≤ (T − 1) (0) 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 (0) (α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. CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL 2.4.3 35 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. 2.4.1. 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 tcorr is computed on-line by the controller using: ts,corr = t− ∆s Ux t− ∆s Ux s,corr − s,corr − t− s,corr − + ts,long ∆s Ux > ts,MPC (2.29) otherwise 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, ts , 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 = ts,MPC ts,corr t s,long 0 ≤ k < Tsplit k = Tsplit (2.30) Tsplit < k ≤ (T − 1) where ts,MPC is the controller execution time step, ts,long is the longer time step used in the later portion of the horizon, and tcorr is the corrective, variable length time step. CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL Obstacle Lane boundary 36 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 2.4.4 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 l1 norm for two reasons. First, the desire to identically match the driver’s command is better captured by the larger values of the l1 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 l1 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]. 2.4.5 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 CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL 37 4 l norm 2 3.5 l1 norm − F (0) yf,opt ( 2 norm (F 2.5 yf,driver 3 1.5 1 0.5 0 −2 −1.5 −1 −0.5 0 0.5 Fyf,driver − F (0) yf,opt 1 1.5 2 Figure 2.6: Comparison of the l1 and l2 normsillustrating the different behavior of (0) the norms at small values of Fyf,driver − Fyf,opt versus larger values CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL 38 Table 2.2: Steering Controller Parameters Parameter Surface friction coefficient Vehicle longitudinal speed Driver comfort distance Prediction horizon length Near-term horizon length Controller time step size Correction time step size Later horizon time step size Near-term smooth input weight Long-term smooth input weight Stable handing slack weight Environmental slack weight Driver’s commanded steer angle Symbol Value Units µ 0.55 (none) m Ux (measured) s dbuffer 0.4 m T 30 (none) Tsplit 10 (none) ts,MPC 0.01 s ts,corr (computed) s ts,long 0.2 s 1 γnear 5 kN2 1 γlong 2 1 kN2 s σsh [60 60] rad rad 1 σenv 1000 2 m δdriver (measured) 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 ts,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 2n , 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 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 10 (ms), which is the execution time step length of the controller, without having to implement the controller on a parallel processing system. CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL Obstacle Lane boundary 40 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, 2n = 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 CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL 2.5 41 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 Mass m Yaw moment of inertia Izz Distance from front axle to CG a Distance from rear axle to CG b Width d Front cornering stiffness Cαf Rear cornering stiffness Cαr Value Units 1973 kg 2000 kg · m2 1.53 m 1.23 m 1.87 m 100 kN/rad 140 kN/rad CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL Vehicle Path Vehicle Width Obstacles 10 8 Distance (m) 42 6 4 4 2 3 2 1 0 0 20 40 60 Distance (m) Yaw Rate, r (deg/s) 20 100 Stable Handling Envelope Actual 3 1 80 10 0 −10 4 2 −20 −10 −5 0 Side Slip, β (deg) 5 10 Steer Angle, δ (deg) 10 Actual Driver 1 5 3 0 4 2 −5 0 1 2 1 2 3 4 5 6 4 5 6 (m/s) Speed, U x 18 16 14 0 3 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 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.10 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 3888-1 [1] on a low friction surface at speeds greater than 60 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 CHAPTER 2. SAFE ENVELOPES FOR SHARED STEERING CONTROL Vehicle Path Vehicle Width Obstacles Distance (m) 6 2 4 3 2 1 0 0 20 40 60 80 100 Distance (m) 120 140 Stable Handling Envelope Actual 5 40 30 20 Yaw Rate, r (deg/s) 5 4 4 10 2 0 1 −10 3 −20 −10 −5 0 Side Slip, β (deg) Steer Angle, δ (deg) 10 4 1 2 0 −10 0 2 0 2 5 10 Actual Driver 5 3 4 6 8 6 8 (m/s) Speed, U x 18 17 16 4 Time (s) Figure 2.10: Double lane change (ISO 3888-1) on a low friction surface 44 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 0.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.10 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 100 (Hz) with a 4 (s) prediction horizon. In addition, dynamic maneuvers in an experimental testbed on a low friction 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. 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 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 48 3.1 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 dbuffer 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: Henv x(k) ≤ G(k) env with Henv Genv 0 0 0 0 1 = 0 0 0 0 −1 (k) emax − 12 w − dbuffer = (k) 1 −emin − 2 w − dbuffer 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 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 49 Environmental Envelope [m] 10 5 0 −5 5 10 15 20 25 [m] 30 35 40 45 Steering Command 20 Driver Cmd Actual Cmd δ [deg] 10 0 −10 −20 0 0.5 1 1.5 2 [s] 2.5 3 3.5 4 Figure 3.1: Simulation: Conservative behavior at road boundary as a result of the crude vehicle width model CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 50 dFL = dRL dFL dRL 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: 12 h w i sin ∆ψ + arctan 2 2a 12 w w 2 2 sin ∆ψ + arctan = +b 2 −2b dFL = dRL w 2 +a 2 (3.1) (3.2) where dFL and dRL 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, dFL = dRL = 21 w. However, as the orientation of the vehicle changes in either direction, this required distance increases. To ensure the left side of the vehicle 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, dL , is given by: dL = max(dFL , dRL ) (3.3) CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 51 2 1.8 1.6 1.4 [m] 1.2 dL 1 dFL 0.8 d RL 0.6 a b w/2 0.4 0.2 0 −100 −80 −60 −40 −20 0 20 40 Heading deviation, ∆ψ [deg] 60 80 100 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 dL can now be expressed as: d˜L = max (λFL (∆ψ − ∆ψ0 ) + d0,FL , λRL (∆ψ + ∆ψ0 ) + d0,RL ) (3.4) where ∆ψ0 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, ∆ψ0 should be small since ∆ψ will be near zero with the envelope controller CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 52 1.8 ∆ψ = 0 [deg] 1.6 ∆ψ0 = 15 [deg] 0 ∆ψ0 = 25 [deg] Approximation error [m] 1.4 1.2 1 0.8 0.6 0.4 0.2 0 −100 −50 0 Heading deviation, ∆ψ [deg] 50 100 Figure 3.4: Plot of approximation error, (d˜L − dL ), vs vehicle orientation relative to the reference line safely guiding the vehicle down the road; however, ∆ψ0 can serve as a tuning parameter to shape the approximation error as illustrated in Figure 3.4. Larger values of ∆ψ0 improve the range over which d˜L provides a useful approximation for dL , and smaller values reduce the approximation error when the vehicle is oriented with the road, ∆ψ = 0. Regardless of the choice of ∆ψ0 , d˜L provides an upper-bound for dL for all vehicle orientations. Due to the symmetry of the vehicle, a similar approximation for the equivalent distance on the right side, dR , is given by: d˜R = max (λFL (−∆ψ − ∆ψ0 ) + d0,FL , λRL (−∆ψ + ∆ψ0 ) + d0,RL ) (3.5) 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: (k) Henv x(k) ≤ G(k) + JFL , FRL x(k) + JRL env − max FFL x (3.6) with FFL = JFL = FRL JRL 0 0 λFL 0 0 0 0 −λFL 0 0 −λFL ∆ψ0 + d0 FL − 21 w 1 w 2 −λFL ∆ψ0 + d0 FL − 0 0 λRL 0 0 = 0 0 −λRL 0 0 λRL ∆ψ0 + d0 RL − 21 w = 1 λRL ∆ψ0 + d0 RL − 2 w and Henv and Genv 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 dbuffer 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. CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 54 Environmental Envelope [m] 10 5 0 5 10 15 20 25 [m] 30 35 40 45 Steering Command 20 Driver Cmd Actual Cmd δ [deg] 10 0 −10 −20 0 0.5 1 1.5 2 [s] 2.5 3 3.5 4 Figure 3.5: Simulation: Less intrusive behavior at road boundary when using environmental constraint (3.6) with ∆ψ0 = 8 [deg]. CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 55 Environmental Envelope [m] 5 0 −5 5 10 15 20 25 30 35 [m] Steering Command 20 Driver Cmd Actual Cmd δ [deg] 10 0 −10 −20 0 0.5 1 1.5 2 [s] 2.5 3 3.5 4 Figure 3.6: Simulation: Controller properly orients the vehicle when confronted with narrowly spaced obstacles CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 56 3.2 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 [10] 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: X j qT ∆ψ (j) 2 (3.7) CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 57 Environmental Envelope 5 [m] 0 −5 Actual Traj Safe Short Term Traj 40 Long 45 Term Traj Safe −10 5 10 15 20 25 [m] 30 35 Steering Command 5 δ [deg] Driver Cmd Actual Cmd 0 −5 0 1 2 3 4 [s] 5 6 7 8 Figure 3.7: Simulation: Without a terminal cost, the planned trajectories arc into the road boundaries CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 58 where j = (T − 3)...T and qT is chosen so that this terminal cost objective dominates the smooth steering and driver autonomy objectives discussed in Section 2.4.2 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 ∆ψ = 0 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]. CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 59 Environmental Envelope [m] 5 0 −5 −10 5 10 15 20 25 [m] 30 Actual Traj Term Traj 35 Safe 40Short45 Safe Long Term Traj Steering Command 20 Driver Cmd Actual Cmd δ [deg] 10 0 −10 −20 0 1 2 3 4 [s] 5 6 7 8 Figure 3.8: Simulation: With the terminal cost, the planned trajectories take the form of lane changes. In this example, qT = 50 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 60 3.3 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 (E0 , N0 ) and an initial heading angle (ψ0 ), the position and heading angle along the path can be computed from this parameterized curvature: Z s ψ(s) = K(x)dx + ψ0 (3.8) cos (ψ(x)) dx + E0 (3.9) sin (ψ(x)) dx + N0 (3.10) 0 Z s E(s) = 0 Z N (s) = s 0 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)Ux ∆ψ (3.11) e˙ = Ux sin (∆ψ) + Uy cos (∆ψ) (3.12) s˙ = (Ux cos (∆ψ) − Uy sin (∆ψ)) 1 1 − K(s)e (3.13) Using small angle assumptions for ∆ψ and β as was done in Chapter 2, these nonlinear equations of motion can be approximated as: e˙ ≈ Ux ∆ψ + Ux β 1 s˙ ≈ Ux 1 − K(s)e (3.14) (3.15) CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 61 Reference Path s e p N ψ R E Figure 3.9: Curvature at point p that is located at a distance s along the reference 1 , where K > 0 for left hand turns path is K(s) = R(s) where, for small values of β and ∆ψ, the product β∆ψ ≈ 0. When the vehicle travels along the path, i.e. e = 0, 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 = 0 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 6= 0, state update equation (3.15) differs from the zero curvature version presented in Chapter 2. Unfortunately, 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 = 0 throughout the prediction horizon, reducing equation (3.15) to the form presented in Chapter 2: s˙ approx ≈ Ux (3.16) If the vehicle remains near the reference path, i.e. e ≈ 0, this assumption works well. Figure 3.10 illustrates the type of maneuvers the controller is capable of planning using curved reference paths and the assumption that e = 0. 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: s˙ err = s˙ approx − s˙ ≈ Ux − Ux (3.17) 1 1 − K(s)e (3.18) 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. CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 63 Environmental Envelope 90 [m] 80 70 60 −20 −10 0 10 20 30 [m] Actual Traj Safe 40 Short 50Term Traj Safe Long Term Traj Steering Command 20 Driver Cmd Actual Cmd δ [deg] 10 0 −10 −20 0 0.5 1 1.5 2 2.5 3 Figure 3.10: Simulation: Controller augments driver’s command to navigate around an obstacle while negotiating a turn CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 64 This gives the longitudinal position error at the end of the horizon: Z tPH (s˙ approx − s) ˙ dt Z tPH 1 dt ≈ Ux − Ux 1 − Ke 0 Z tPH −Ke ≈ Ux dt 1 − Ke 0 −Ke tPH ≈ Ux 1 − Ke serr = (3.19) 0 (3.20) (3.21) (3.22) where tPH is the time length of the prediction horizon and serr 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 Ux , reference path curvature K, lateral offset from the reference path e, and prediction horizon length tPH . Using the actual distance along the prediction horizon: sactual = Ux 1 1 − Ke tPH (3.23) (3.24) a prediction error percentage can be computed: serr,% = |100(serr /sactual )| (3.25) ≈ |100(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 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 Speed Ux (m/s) 6 8 11 14 17 19 22 25 28 31 33 36 Minimum Radius of Curvature R (m) 9 26 56 106 167 257 366 510 700 953 1296 1774 Prediction Error serr (m) 11.4 6.7 4.4 3.1 2.4 2.0 1.6 1.4 1.2 1.1 0.9 0.8 Prediction Error Percentage serr,% (%) 51.4 20.0 10.0 5.6 3.7 2.5 1.9 1.4 1.1 0.9 0.7 0.5 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: s˙ approx ≈ Ux 1 1 − K(s)e(0) (3.27) where e(0) 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 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: s˙ approx ≈ Ux 1 1 − K(s)ˆ e(k) (3.28) where eˆ(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 [50] 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 l1 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 l1 norm was initially chosen for the penalty functions for the safe envelope slack variables. However, interactions with the environmental envelope boundary using the l1 norm results in aggressive steering interventions as illustrated in Figure 3.11. Human drivers reported these Distance (m) CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 67 Start Vehicle Path Obstacles −105 −110 −115 −120 −125 −300 −280 −260 Distance (m) −240 −220 Actual Driver 20 0 −20 0 Vehicle Speed (m/s) Steer cmd (deg) −320 2 4 6 8 Time (s) 10 12 14 2 4 6 8 Time (s) 10 12 14 12 10 8 6 4 0 Figure 3.11: Experiment: Driver bouncing off environmental boundary; l1 -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 3.12. 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. Distance (m) CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 68 −120 −125 −130 −135 −300 Steer cmd (deg) Start Vehicle Path Obstacles −290 −280 −270 −260 −250 −240 Distance (m) −230 −220 −210 Actual Driver 5 0 −5 −10 0 1 2 3 4 5 6 7 4 5 6 7 Vehicle Speed (m/s) Time (s) 12 10 8 6 4 0 1 2 3 Time (s) Figure 3.12: Experiment: Driver bouncing off environmental boundary; quadratic slack penalty function leads to smooth interventions by controller CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 69 3.5 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. Given the driver’s current and previous commands, future driver commands can be approximated as: (k) δdriver = (0) δdriver +k (0) δdriver − (−1) δdriver (3.29) (k) where δdriver is the predicted driver command at the kth step into the prediction (0) (−1) horizon and δdriver and δ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: X (i) ρ (i) (i) Fyf,driver − Fyf,opt (3.30) i (i) (i) where ρ(i) is a tunable gain and Fyf,driver and Fyf,opt are the driver’s commanded and the MPC optimal front tire forces at the ith step into the horizon. In (2.25a) presented CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 70 in Chapter 2, ρ(i) was implicitly defined as: ρ(i) = 1 i=0 0 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 ρ(0) is varied while ρ(i) = 0 for i > 0. As expected, increasing ρ(0) 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) 6= 0 for i > 0. 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. CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 71 Environmental Envelope [m] 10 0 −10 0 10 20 30 40 [m] Actual Traj Safe Short Term Traj 60 70 80 Safe Long Term Traj 50 Steering Command 20 Driver Cmd Actual Cmd δ [deg] FFB [Nm] 10 0 −10 −20 0 1 2 3 4 [s] 5 6 7 8 Figure 3.13: Simulation: Avoidance scenario with the driver turning into the obstacle and road boundary, with ρ(0) = 1.2 and ρ(i) = 0 for i > 0. The steering command and predicted trajectory are smooth but the steering command rarely tracks the driver. CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 72 Environmental Envelope [m] 10 0 −10 0 10 20 30 40 [m] 50 Actual Traj 60Safe Short 70 Term 80Traj Safe Long Term Traj Steering Command 20 Driver Cmd Actual Cmd δ [deg] FFB [Nm] 10 0 −10 −20 0 1 2 3 4 [s] 5 6 7 8 Figure 3.14: Simulation: Avoidance scenario with the driver turning into the obstacle and road boundary, with ρ(0) = 4.8 and ρ(i) = 0 for i > 0. Steering matches driver but at the expense of aggressive steering commands. CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 73 Environmental Envelope [m] 10 0 −10 0 10 20 30 40 [m] 50 Actual Traj 60 Safe Short 70 Term 80 Traj Safe Long Term Traj Steering Command 20 Driver Cmd Actual Cmd δ [deg] FFB [Nm] 10 0 −10 −20 0 1 2 3 4 [s] 5 6 7 8 Figure 3.15: Simulation: Avoidance scenario with the driver turning into the obstacle and road boundary, with ρ(0) = 1.2, ρ(1:3) = 0.5, and ρ(i) = 0 for i > 3. This tuning provides a good balance between driver autonomy and smooth steering. CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 74 3.6 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. CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 75 3.6.1 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, 2014 [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 3.16. Balachandran and Gerdes [9] present the following dynamic model of this system suitable for use in an artificial steering feel emulator: τhw = bsteer where bsteer tively, Ktire system and Jsteer moment ˙ + Jsteer system δ system ¨ + Ktire system δ moment (τjack + τalign ) (3.32) are the steering system inertia and damping, respec- is the tire moment gain, τjack is the jacking torque, and τalign is the aligning moment. The tire moment gain (Ktire 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 (Fzf ) 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 (Fyf ) 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 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 + JFFB where bFFB system and JFFB system ¨ system δhw + bFFB ˙ system δhw (3.33) 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 /Ksteer where Ksteer 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 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.20 illustrates the affect of of this modification to the steering feel emulator during an similar scenario as Figure 3.19. 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. CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 78 Vehicle Path [m] −120 Start −140 Roadwheel Angle [deg] Front Tireforce [kN] Steering Feel Torque [Nm] −160 −310 −300 −290 −280 −270 −260 [m] −250 −240 −230 5 0 −5 6 10 8 10 12 14 Time [s] 16 18 Driver Cmd Actual 20 Cmd Steer Feel 12 16 18 20 Driver Cmd Actual Cmd 0 −10 6 8 10 14 Figure 3.19: Experiment: Steering feel emulator using δ, the actual road wheel angle, to generate the artificial steering feel CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 79 Vehicle Path Start [m] −120 −140 Roadwheel Angle [deg] Front Tireforce [kN] Steering Feel Torque [Nm] −160 −310 −300 −290 −280 −270 −260 −250 −240 −230 −220 [m] 5 0 8 10 Time [s] 12 14 Driver Cmd Actual Cmd 16Steer Feel 18 8 10 12 14 16 −5 2 10 4 6 Driver Cmd Actual Cmd 0 −10 2 4 6 18 Figure 3.20: Experiment: Steering feel emulator using δdriver , the driver’s commanded road wheel angle, to generate the artificial steering feel CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 80 3.6.2 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 δ = 0, 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 3.21. A simple approach is to compute the predictive CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 81 Environmental Envelope 10 [m] 5 0 −5 −10 0 10 20 30 [m] Actual Traj Safe Short 60 Term Traj 50 Safe Long Term Traj 40 Steering Command δ [deg] 5 0 Driver Cmd Actual Cmd −5 0 1 2 3 [s] 4 5 6 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] CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 82 Environmental Envelope 10 [m] 5 0 −5 −10 0 10 20 30 [m] Actual Traj Safe Short 60 Term Traj 50 Safe Long Term Traj 40 Steering Command δ [deg] 5 0 Driver Cmd Actual Cmd −5 0 1 2 3 [s] 4 5 6 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] CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 83 haptic feedback torque as: (k ) (khaptic ) τhaptic = Khaptic δopthaptic − δdriver (3.34) (k haptic where τhaptic is the predictive haptic feedback torque, Khaptic is a tunable gain, δdriver (k ) ) and δopthaptic are the anticipated driver’s commanded steer angle and optimal planned steer angle at the khaptic 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 khaptic = 4 for the same obstacle avoidance scenario used in Figures 3.21 and 3.22. 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 khaptic provides another tuning knob in shaping the haptic signal. Increasing khaptic provides a subtle distinction in the haptic signal as illustrated in Figure 3.24. Comparing the haptic signals from Figures 3.23 and 3.24, it is seen that the larger khaptic 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 3.25. 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 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 84 Environmental Envelope [m] 10 0 −10 0 10 20 30 40 [m] 50 Actual Traj Safe 70Short Term 80 Traj Safe Long Term Traj 60 Driver Cmd Actual Cmd τHaptic Steering Command 10 δ [deg] FFB [Nm] 5 0 −5 −10 1 2 3 4 [s] 5 6 7 8 Figure 3.23: Simulation: Haptic signal generated from (3.34) with Khaptic = 150 [Nm/rad] and khaptic = 4 for single obstacle avoidance scenario using an open-loop driver model CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 85 Environmental Envelope [m] 10 0 −10 0 10 20 30 40 [m] Actual Traj Safe Short Term Traj 60 70 80 Safe Long Term Traj 50 Driver Cmd Actual Cmd τHaptic Steering Command 10 δ [deg] FFB [Nm] 5 0 −5 −10 1 2 3 4 [s] 5 6 7 8 Figure 3.24: Simulation: Haptic signal generated from (3.34) with Khaptic = 150 [Nm/rad] and khaptic = 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 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. Distance (m) CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 87 Start Vehicle Path Obstacles −115 −120 −125 Steer cmd (deg) −130 −330 −310 −300 −290 −280 Distance (m) −270 −260 −250 Driver Actual 5 0 −5 0 Vehicle Speed (m/s) −320 1 2 3 4 Time (s) 5 6 7 8 1 2 3 4 Time (s) 5 6 7 8 12 10 8 6 4 0 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 Khaptic = 150 [Nm/rad] and khaptic = 4 CHAPTER 3. DESIGN DECISIONS, JUSTIFICATIONS, AND EXTENSIONS 88 3.7 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. 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 CHAPTER 4. PREDICTING REAR TIRE SATURATION 90 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, 2014 [23] with the balance of the chapter focusing on subsequent experimental validation using X1. 4.1 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 [10]. 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 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 [70] 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 (Fyr ) as an affine function of rear slip angle (αr ) as illustrated in 4.2. Following the lead of Beal and Gerdes [10], this rear tire model is linearized around the measured rear tire slip angle, αr , in the near term of the prediction horizon. Over CHAPTER 4. PREDICTING REAR TIRE SATURATION αf a b Uy αr Fyr β r δ Fyf Ux e ∆ Reference Line s Figure 4.1: Bicycle model schematic Tire Lateral Force Fy Brush tire model Affine approx α Fy Cα Tire slip angle α Figure 4.2: Brush tire model with affine approximation at α ¯ 92 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 (k) is simply choosing α ¯r = 0, 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 (k) is to choose α ¯r (k) (k) = α ˆ r , where α ˆr 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: β˙ = Fyf + F¯yr − C¯α¯ r β − br Ux −α ¯r mUx h aFyf − b F¯yr − C¯α¯ r β − r˙ = br Ux −r i −α ¯r (4.1) (4.2) Izz The position states of the vehicle are defined as presented in Chapter 2 using small angle approximations: ˙ = r ∆ψ (4.3) e˙ ≈ Ux ∆ψ + Ux β (4.4) s˙ ≈ Ux (4.5) giving a discrete, time-varying vehicle model which can be expressed as: (k) (k) (k) x(k+1) = Aα¯ r ,ts x(k) + Bα¯ r ,ts Fyf (k) + dα¯ r ,ts where x = [β r ∆ψ (4.6) s e]T , k is a time step index, subscript α ¯ r denotes lin- earization of the rear tire model around rear slip angle α ¯ r , and subscript ts denotes discretization of the vehicle model using time step length ts . CHAPTER 4. PREDICTING REAR TIRE SATURATION 4.3 94 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 2. 4.4 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. 4.4.1 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 [10]. 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 CHAPTER 4. PREDICTING REAR TIRE SATURATION 95 0.6 4 0.4 Yaw Rate r 0.2 3 0 1 −0.2 −0.4 −0.6 2 −0.2 −0.1 Sideslip 0 β 0.1 0.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 xk is the vehicle state at the kth time step into the prediction horizon and subscript sh indicates the stable handling envelope. 4.4.2 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: 1 e(k) ≤ e(k) max − d − dbuffer 2 1 (k) (k) e ≥ emin + d + dbuffer 2 (4.8) (4.9) CHAPTER 4. PREDICTING REAR TIRE SATURATION (k) 96 (k) where emax and emin indicate the lateral deviation bounds for time step k, d is the vehicle’s width, and dbuffer 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: Henv x(k) ≤ G(k) env (4.10) where subscript env indicates the environmental envelope. A vehicle trajectory is collision free throughout the prediction horizon if and only if it satisfies inequality (4.10) 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. CHAPTER 4. PREDICTING REAR TIRE SATURATION e s 97 Nominal path Lane boundary Obstacle Tube boundary a) (1) e max (1) e min b) (1) e max (4) e max (4) e min (4) e max (4) e min (1) e 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 (k) (k) a maximum (emax ) and minimum (emin ) lateral deviation from the nominal path at each time step, k. CHAPTER 4. PREDICTING REAR TIRE SATURATION 4.5 98 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 (0) F − F yf,driver yf,opt 2 X (k) (k−1) + γ (k) Fyf,opt − Fyf,opt (4.11a) (4.11b) k + X (k) σsh ] Ssh,opt (4.11c) 2 (k) [σenv σenv ] Senv,opt (4.11d) [σsh k + X k (k+1) (k) (k) (k) (k) subject to x = Aα¯ r ,ts x(k) + Bα¯ r ,ts Fyf,opt + dα¯ r ,ts (k) Fyf,opt ≤ Fyf,max Hsh x(k+1) ≤ Gsh + S (k) sh,opt (k+1) Genv (k) Senv,opt Henv x(k+1) ≤ + (k) (k−1) (k) Fyf,opt − Fyf,opt ≤ Fyf,max (4.11e) (4.11f) (4.11g) (4.11h) (4.11i) slew k = 0 . . . 29 where the variables to be optimized are the optimal input trajectory (Fyf,opt ) and the safe driving envelope slack variables (Ssh,opt , Senv,opt ). As is commonly done in MPC, (0) only the optimal input for the first step into the prediction horizon, Fyf,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 CHAPTER 4. PREDICTING REAR TIRE SATURATION 99 Table 4.1: Prediction Horizon Parameters Parameter k (k) ts γ (k) (k) Fyf,max slew σsh σenv (k) α ¯r Near Term Horizon 0...9 0.01 [s] 30 0.2 [kN] 60 1500 αr (k) α ˆr Long Term Horizon 10...29 0.2 [s] 1.5 5 [kN] 60 1500 0 (Linear Model) or (Approx Nonlinear Model) stable handling and environmental envelopes, respectively. These constraints are softened with slack variables, Ssh,opt and Senv,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 CHAPTER 4. PREDICTING REAR TIRE SATURATION 100 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 3888-1 [1]. For simplicity, the effects of vehicle roll and weight transfer are ignored in the following simulations. Table 4.2: P1 Vehicle Parameters Parameter m Iz a b d Cαf Cαr Value 1725 1300 1.35 1.15 1.60 57.8 110 Units kg kg · m2 m m m kN · rad−1 kN · rad−1 CHAPTER 4. PREDICTING REAR TIRE SATURATION 101 20 [m] 10 0 −10 0 20 40 [m] 60 80 100 Figure 4.6: Arrangement of obstacles and road boundaries used in the following simulations. Vehicle travels in the direction indicated by the arrow. Figure 4.7 illustrates the shared control capabilities of the controller at a moderate speed of 12 (m/s) on a low friction surface (µ = 0.55). Using either tire model, the controller successfully augments the human driver’s steering command to negotiate the double lane change. The driver is modeled using an open-loop steering command which, at this speed, almost navigates the obstacles without collision or loss of control. However, as illustrated, two slight augmentations of the driver’s command are required to avoid collision, and the controller identically matches the driver’s command otherwise. Figure 4.8 provides a comparison of the controller’s performance at a more aggressive speed of 16 (m/s) on the same low friction surface. At this speed, the performance improvement using the successive linearizations technique becomes apparent. At the instance t = 6.25 (s), the controller using the improved rear tire model deviates from the driver’s command to set the vehicle up for a smooth exit through the final lane change. Uninformed of the saturation of the rear tires, the controller using the linear rear tire model mistakenly matches the driver at this point in time and leads the vehicle into a state without a feasible trajectory that adheres to both envelopes. This results in aggressive attempts to adhere to both safe envelopes, which fail on both accounts. This simulation highlights the importance of planning with accurate models to ensure vehicle safety. Subtle inputs at one point in time can drastically influence the feasibility at a future time. As the vehicle moves from the linear to the nonlinear regions, the sucessive linearization technique serves as a powerful tool CHAPTER 4. PREDICTING REAR TIRE SATURATION 102 to capture these nonlinear dynamics, enabling the controller to anticipate feasibility challenges and appropriately act to avoid them. With only the ability to steer, situations arise when adherence to both safe driving envelopes is impossible, as illustrated in Figure 4.9 where the speed is 18 (m/s). The combination of low friction and high speed forces the controller to violate one of the envelopes. According to the envelope hierarchy dictated by the relative weights discussed in Section 4.5, the controller should prioritize collision avoidance over vehicle Driver Linear Approx Non−Linear [m] 20 10 0 steering angle [deg] 0 20 40 60 [m] 80 100 120 5 0 −5 0 2 4 6 8 10 time [s] yaw rate [rad/s] 0.5 0 −0.5 −0.2 −0.15 −0.1 −0.05 0 0.05 side slip [rad] 0.1 0.15 0.2 Figure 4.7: Double lane change maneuver on low friction surface at 12 [m/s] CHAPTER 4. PREDICTING REAR TIRE SATURATION Driver Linear w/Collision Approx Non−Linear 20 [m] 103 10 0 steering angle [deg] 0 20 40 60 [m] 80 100 120 10 5 0 −5 0 1 2 3 4 5 time [s] 6 7 8 yaw rate [rad/s] 0.5 0 −0.5 −0.15 −0.1 −0.05 0 0.05 side slip [rad] 0.1 0.15 Figure 4.8: Double lane change maneuver on low friction surface at 16 (m/s) CHAPTER 4. PREDICTING REAR TIRE SATURATION 104 stability. However, only the controller equipped with the approximate nonlinear tire model can anticipate the need for this trade-off and avoid collision at the expense of stability. The controller not informed of the rear tire nonlinearity ultimately fails to satisfy either constraint and eventually stabilizes the vehicle only after allowing it to get significantly sideways and collide with the road boundary. Driver Linear w/Collision Approx Non−Linear [m] 20 10 0 steering angle [deg] 0 20 40 60 [m] 80 100 120 10 0 −10 −20 0 1 2 3 4 5 time [s] 6 7 −0.2 −0.1 side slip [rad] 0 0.1 8 yaw rate [rad/s] 1 0.5 0 −0.5 −0.5 −0.4 −0.3 0.2 Figure 4.9: Double lane change maneuver on low friction surface at 18 (m/s) Figure 4.10 displays a comparison of the internal predictions by the controller of the future vehicle states using each of the rear tire models. As expected, in both CHAPTER 4. PREDICTING REAR TIRE SATURATION Linear Approx Non−Linear 0.4 0.4 Initial State Near Term Prediction 0.3 Long Term Prediction Actual State Trajectory 0.2 0.3 0.1 yaw rate [rad/s] yaw rate [rad/s] 0.2 0 −0.1 0.1 0 −0.1 −0.2 −0.2 −0.3 −0.3 −0.4 −0.4 −0.2 105 −0.1 0 side slip [rad] 0.1 0.2 −0.2 −0.1 0 side slip [rad] 0.1 0.2 Figure 4.10: Comparison of the planned safe trajectory midway through the double lane change manuever on low friction surface at 18 [m/s] cases the controller accurately predicts the pending stable handling envelope violation because the current rear slip angle is used to predict near term rear tire behavior. However, as illustrated on the left of Figure 4.10, use of a linear model in the remainder of the prediction horizon underestimates the extent of this envelope violation. This leads the controller to make steering decisions that result in significant violation of the safe handling envelope followed by a collision with the road boundary. The approximate nonlinear rear tire model better predicts future vehicle states, and, as seen in the right of Figure 4.10, the controller plans a slight violation of the stable handling envelope in order to prevent significant violations in the future and avoid collision with the environment. CHAPTER 4. PREDICTING REAR TIRE SATURATION 106 Table 4.3: Maximum Speed without Collision Surface Friction, µ 0.90 0.55 Linear Rear Tire Model 18 [m/s] 14 [m/s] Approx. Nonlinear Rear Tire Model 22 [m/s] 19 [m/s] Additional simulations reveal a similar trend on high friction surfaces as summarized in Table 4.3. These results highlight the importance of considering future rear tire saturation for both low and high values of surface friction. 4.7 Experimental Results Experiments using Stanford’s X1 test bed, which is illustrated in Figure 4.11, support the results of the simulations presented previously and enable a direct comparison with the experimental results of Chapter 2. As in the simulations, the environment used in these experiments conforms to ISO standard 3888-1 [1] and the surface was gravel on asphalt with a surface coefficient of µ ≈ 0.5. Figure 4.12 illustrates the first of two experiments with a human driver and the proposed envelope controller sharing control of the vehicle as it negotiates the double lane change. As shown, a number of steering augmentations by the controller are required in order to safely navigate the double lane change without collision with the environment. A few of those augmentations are highlighted. At instance 1 , the controller reduces the magnitude of the driver’s steer command in response to a pending stable handling envelope violation. Even with this reduction in steer command, the vehicle safely completes the first lane change without collision with the environment. At instance 2 , the driver prematurely begins the second lane change, forcing the steering controller to delay the steer command by about 0.5 [s] in order to avoid collision with the environment. At time instances 3 and 4 , two quick steering interventions by the controller minimize violations of the stable handling envelope as the vehicle safely completes the second lane change. Overall, the benefit of the improved rear tire modeling that was demonstrated CHAPTER 4. PREDICTING REAR TIRE SATURATION 107 Figure 4.11: Stanford’s X1, an all electric, throttle- and steer-by-wire research testbed with automatic brakes and haptic force feedback steering system in simulation is made evident in experimentation by comparing Figure 4.12 with Figure 2.10 from Chapter 2. Both of these experiments used the same vehicle, same set of gains, and same environment. However, it should be noted that with the driver in the loop, the experimental runs are not identical, and the driver’s command differs between the two runs. The only difference not related to the driver was the rear tire modeling approach. Although the controller performed quite well using only the linear tire model in the long term portion of the prediction horizon as shown in Figure 2.10, a large violation of the stable handling envelope and a small violation of the environmental envelope were observed. In addition, the controller using the linear model made large steering interventions in reaction to unanticipated challenges to vehicle stability and obstacle avoidance. Informed of future rear tire saturation, the controller using the approximate nonlinear rear tire model better adheres to both safe envelopes while avoiding large steering commands. Figure 4.13 illustrates another iteration of the same test and demonstrates how control at the limits remains challenging even with the approximate nonlinear rear tire model. At instances 1 and 2 , the controller makes attempts to adhere to the stable handing envelope but the vehicle slides more than the controller predicted leading to a slight violation of the environmental envelope. The violation of the stable handling envelope is reduced relative to Figure 2.10 illustrating better stability with CHAPTER 4. PREDICTING REAR TIRE SATURATION the approximate nonlinear rear tire model. 108 CHAPTER 4. PREDICTING REAR TIRE SATURATION 109 Start Vehicle Path Obstacles Distance (m) 40 30 2 20 3 1 4 10 80 100 120 140 Distance (m) 160 180 Steer cmd (deg) 5 Actual Driver 2 0 3 1 −5 0 1 2 20 Yaw Rate (deg/s) 200 3 4 4 Time (s) 4 5 6 7 Envelope Actual 3 10 0 2 −10 1 −20 Vehicle Speed (m/s) −10 −5 0 Side Slip (deg) 5 10 18 16 14 0 1 2 3 4 Time (s) 5 6 7 Figure 4.12: Experiment using X1 in double lane change on low friction µ = 0.55 with no environmental envelope violation CHAPTER 4. PREDICTING REAR TIRE SATURATION 110 Start Vehicle Path Obstacles Distance (m) 40 30 20 1 3 10 80 100 120 140 Distance (m) 10 Steer cmd (deg) 2 160 1 180 2 200 Actual Driver 5 0 3 −5 −10 0 1 2 3 4 Time (s) 5 6 7 Envelope Actual Yaw Rate (deg/s) 20 2 3 10 1 0 −10 −20 Vehicle Speed (m/s) −10 −5 0 Side Slip (deg) 5 10 18 16 14 0 1 2 3 4 Time (s) 5 6 7 Figure 4.13: Experiment using X1 in double lane change on low friction µ = 0.55 with slight violation of the environmental envelope at instance 3 CHAPTER 4. PREDICTING REAR TIRE SATURATION 4.8 111 Discussion With only the ability to steer, situations may require violation of the stable handling envelope to avoid a collision with the environment. In these situations, use of the successive linearization technique enables the controller to consider future rear tire saturation to appropriately steer the vehicle safely without additional computational burden. In addition, the prediction of future vehicle states throughout the prediction horizon is improved, giving advanced warning on pending challenges to the combined objectives of vehicle stability and collision avoidance. As described in the next chapter, this advanced warning allows the controller time to safely reduce the vehicle’s speed using brake actuation, ensuring the vehicle’s future trajectory will always adhere to both safe envelopes. This predictive capability is the key to determining when velocity changes are necessary and is the foundation for a comprehensive envelope controller capable of ensuring vehicle safety through augmentation of the driver’s steering, throttle, and brake commands. Chapter 5 Envelope Control using Braking and Steering As illustrated in Chapter 4, situations may arise in which steering alone cannot ensure adherence to both safe driving envelopes. Capturing the saturation of the rear tires using successive linearizations improves the controller’s ability to safely negotiate these types of situations, enabling the controller to trade-off vehicle stability to ensure collision avoidance. However, avoiding violation of the safe envelopes in these situations requires additional actuation. This chapter explores the use of brake actuation as a way of extending the steering only envelope controller presented thus far, expanding the number of situations in which the envelope controller can ensure vehicle safety. 5.1 Challenge of Longitudinal and Lateral Control Adding the ability to brake opens up many new opportunities to the envelope controller. Unfortunately, this expanded set of possibilities makes searching over these possible trajectories in real-time a challenging problem. A number of approaches have been purposed to overcome this challenge and enable the use of both steering and braking in the control of automated vehicles. Many approaches address this challenge of combined braking and steering control 112 CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 113 by reducing the problem to path tracking, which relies on a desired path generated externally to the controller. Falcone et al. present a nonlinear MPC (NMPC) approach to path tracking using both steering and braking [28]. This approach achieves good tracking performance using a higher order vehicle model that considers individual wheel braking; however, use of this high-fidelity model is computationally prohibitive for real-time implementation. A simplified version of their controller that models differential braking as a single moment applied to the vehicle allows for realtime implementation but at a reduced level of performance. Logic external to the optimal controller converts the optimal brake moment into individual wheel brake commands. Falcone et al. also present a linear time-varying MPC extension to the previous NMPC approach that addresses the performance and computational limitations [27]. More recently, Katriniok et al. present an optimal approach to combined lateral and longitudinal control using steering angle and longitudinal acceleration as control inputs to track a predefined evasive trajectory [42]. These approaches illustrate the ability to perform combined lateral and longitudinal control in real-time when a predetermined desired path is available. Moving away from path tracking, another way to reduce the search space of possible trajectories is to consider only a finite set of speed profiles. Turri et al. present this approach using multiple pre-processed speed profiles in a series of on-line optimizations for a fully autonomous vehicle [71]. In considering only a finite number of braking or throttling profiles, the search space of possible trajectories is greatly reduced and the lateral and longitudinal dynamics of the vehicle can be decoupled. For each speed profile, the optimal steering command is computed by solving a simple QP, thereby reducing the problem to a set of QPs that can be solved efficiently at each time step. Turri et al. assume prior knowledge of which side of the obstacles it is best to pass. The complexity would increase if multiple routes, or tubes, around obstacles are additionally considered. Experiments in test vehicles on icy roads validate this approach. Approaches which do not restrict the vehicle to a pre-defined path or finite set of speed profiles in general require the use of nonlinear optimization techniques. Gao et al. present a NMPC path planner combined with a linear MPC path tracker for CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 114 combined steering and braking control. The nonlinear path planner uses a spatially discretized model with a 22.5 (m) look ahead distance that executes at 5 (Hz). Although this shows promise of NMPC in real-time, its limited look-ahead distance requires heuristic augmentation of the obstacles to ensure smooth avoidance trajectories and is limited to single tube environments [34]. Bevan et al. also presents a nonlinear path planner for combined braking and steering using a multiple pass optimization scheme that solves approximations to the full control problem at each pass. Although the approach leverages fast convex optimization techniques, it requires a simplified point-mass vehicle model, single tube environment, and the convergence of the multi-stage solver is not guaranteed [12]. Other examples in the literature combine path tracking and path following into a single NMPC controller which explores the full space of feasible trajectories; however, all of these have a computational complexity that prevents real-time implementation [56] [74] [17]. These examples underline the computational challenge in optimal longitudinal and lateral vehicle control. To address this challenge, the approach presented in this chapter formulates braking with envelope control as a steering feasibility problem. The controller reduces the vehicle’s speed in response to anticipated envelope violations, ensuring a feasible trajectory exists that adheres to both safe envelopes. This approach does not restrict the driver to a predetermined path or speed profile and therefore continues to ensure driver autonomy whenever possible. In addition, this feasibility problem is solved using the same QP presented in Chapter 4 with an additional convex constraint and sparsity seeking objective; therefore, nonlinear optimization techniques are not required and the envelope controller maintains its speed and reliability. 5.2 Braking as Steering Feasibility Problem Using the approach to rear tire modeling presented in the previous chapter, the controller can determine in real-time when the current vehicle speed is too fast to safely adhere to the safe envelopes. Figure 5.1 illustrates the envelope controller’s predicted CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 115 20 15 [m] 10 5 0 −5 −10 −15 −20 −10 0 10 [m] 20 30 40 50 Figure 5.1: Controller plans a single lane change in response to blocked lane vehicle trajectory when confronted with a blocked lane at Ux = 16 (m/s) on a surface with µ = 0.5. As illustrated in the top plot of Figure 5.2, the optimal vehicle trajectory at this speed requires violation of the stable handling envelope. This indicates that the vehicle is traveling too fast for the combination of road conditions and approaching environmental hazards. The remaining plots in Figure 5.2 illustrate the predicted envelope violations for the optimal trajectory evaluated at different vehicle speeds. As shown, the maximum speed at which adherence to the stable handling envelope is possible can be framed as a convex feasibility problem. The maximum violation of the stable handling envelope is an increasing function of vehicle speed as illustrated in Figure 5.3. As the vehicle speed decreases, the stable handling envelope increases in size and the max acceleration and yaw rate required to complete the lane change decrease. These two factors combine to give the generally monotonic trend observed in Figure 5.3: faster speeds result in larger required envelope violations. This trend is not CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 116 strictly monotonic due to the variation in discretization and sampling of the environment which, as described in Chapter 2, is speed dependent. However, this trend indicates that reducing the vehicle’s speed via brake actuation is a key mechanism for reducing envelope violations. When the vehicle speed is too fast for conditions, the MPC optimal trajectory reveals that steering alone is not sufficient to ensure vehicle safety. CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 117 Ux = 16 [m/s] yaw rate [rad/s] 0.6 0.4 0.2 0 −0.2 −0.4 −0.2 −0.1 0 0.1 0.2 0.1 0.2 0.1 0.2 Ux = 10.9 [m/s] yaw rate [rad/s] 0.6 0.4 0.2 0 −0.2 −0.4 −0.2 −0.1 0 Ux = 10 [m/s] yaw rate [rad/s] 0.6 0.4 0.2 0 −0.2 −0.4 −0.2 −0.1 0 side slip [rad] Figure 5.2: Predicted envelope violations for same maneuver at three different vehicle speeds CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 118 normalized envelope violation [−] 0.7 0.6 0.5 0.4 0.3 0.2 0.1 0 6 8 10 12 vehicle speed [m/s] 14 16 Figure 5.3: Maximum predicted envelope violation for the same maneuver as a function of vehicle speed 5.3 Braking to Prevent Envelope Violations Figure 5.4 provides a block diagram of the proposed envelope controller presented thus far. As shown, the envelope controller has the capability to modify only the driver’s steering command, δdriver , and the driver’s brake and acceleration commands, represented here as a lumped longitudinal force command, Fx,driver , are unregulated. This leaves the driver in full control of the vehicle’s speed. Measurements of this speed, the vehicle states, x, and the driver’s steering command serve as inputs to the MPC optimization as described in previous chapters. Figure 5.5 illustrates the block diagram for the proposed braking and steering envelope controller introduced in Section 5.2. As shown, the final longitudinal force command, Fx , is no longer dictated solely by the driver. Instead, the envelope controller now has the capability to override the driver’s longitudinal force command in addition to the steering command. A new input to the MPC optimization, Fx,des , is computed as the minimum of the driver’s command and a braking force that seeks to reduce envelope violations, Fx,envViolation . This heuristically determined brake force, Fx,envViolation , is a function of the maximum predicted envelope violation, dmaxEV , CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 119 Figure 5.4: Block diagram of steering only envelope controller presented in previous chapters which is derived directly from the MPC optimal solution. When the current vehicle speed results in an MPC optimal trajectory that violates the safe envelopes, dmaxEV > 0 and the brake policy computes an Fx,envViolation that reduces the vehicle’s speed. As described in Section 5.2, this speed reduction will reduce the anticipated envelope violation. If instead the current vehicle speed results in an MPC optimal trajectory that does not violate either safe envelope, dmaxEV <= 0 and a reduction in speed is not required. In this situation, Fx,envViolation is set arbitrarily large so that Fx,des = Fx,driver . As will be described in Section 5.4, an MPC optimization will ultimately determine the amount of braking and steering applied to the vehicle to ensure vehicle safety. The desired longitudinal force, Fx,des , is determined externally from the MPC optimization, leaving the optimal controller with the simpler decision of determining how much of this desired longitudinal force can be safely commanded at the current time step. 5.3.1 Braking Policies There is much freedom in determining a policy for Fx,envViolation , so long as Fx,envViolation is chosen to adequately reduce the vehicle’s speed when envelope violations are predicted. This section describes two possible braking policies, creatively called the fixed braking and proportional braking policies. Both of these policies command braking CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 120 Figure 5.5: Block diagram of braking and steering envelope controller in response to predicted envelope violations represented as the scalar dmaxEV , which is a normalized, maximum violation of the stable handling envelope from the MPC optimal solution. This can be computed directly from the optimal slack variables: dmaxEV = maximize max k (k) Ssh,opt ./Gsh (5.1) where ./ indicates element-wise division, max gives the maximum value from a given vector, k is a time step index into the prediction horizon, Gsh is the stable handling (k) envelope constraint, and Ssh,opt is the optimal slack variables for the stable handling (k) envelope at time step k. Both Gsh and Ssh,opt were previously defined in Sections 2.3.1 and 2.4.2, respectively. For the fixed braking policy, the controller commands a constant braking amount whenever there is a predicted envelope violation: Fx,envViolation ∞ = F x,fixed dmaxEV ≤ 0 (5.2) otherwise where Fx,fixed is a predetermined braking value. This policy is simple and, as shown later in experimental validation, effective in reducing the vehicle’s speed to adhere to CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 121 the safe envelopes. For the proportional braking policy, the commanded braking is in proportion to predicted envelope violations: Fx,envViolation ∞ = Kbrake dmaxEV F x,maxComfort dmaxEV ≤ 0 0 < dmaxEV < Fx,maxComfort Kbrake (5.3) otherwise where Kbrake is a proportional gain and Fx,maxComfort serves to bound the commanded brake force to ensure driver comfort. This policy seeks to more smoothly introduce braking in response to anticipated envelope violations. 5.3.2 Constant Speed Assumption Throughout this dissertation, the vehicle model used in the MPC optimization assumes the vehicle is traveling at a constant speed. With the introduction of braking policies into the envelope controller, it is reasonable to question whether this assumption is still valid. Given the controller’s fast execution rate of 100 (Hz), braking can, at most, alter the vehicle’s speed by ≈ 0.1 (m/s) in between controller executions assuming a full 1 (g) brake event. Therefore, the vehicle’s speed remains nearly constant between executions of the controller even during extreme braking events, validating the constant speed assumption that is used in the MPC vehicle model. If a variable speed trajectory is known a priori, this constant speed vehicle model could be replaced with an linear time varying (LTV) model linearized at these known speeds along the prediction horizon. Future work will apply this braking and steering envelope controller to vehicle applications with known speed trajectories. One of these applications is Shelley, an autonomous race car jointly developed by Stanford and Audi [45]. Shelley maintains a planned speed profile generated from the racing line, and this profile could serve as the known speed profile for the MPC vehicle model. However, for simplicity in illustrating the basic concept of braking in response to envelope violations, the braking and steering envelope controller proposed in this chapter continues to utilize the constant speed vehicle model presented previously. CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 122 12 10 8 6 [m] 4 2 0 −2 −4 −6 15 20 25 [m] 30 35 40 45 Figure 5.6: Obstacle blocking the lane enters the prediction horizon as the vehicle travels at 8 (m/s) 5.3.3 Ensuring the Horizon Always Recedes In the presence of braking, a time based, constant speed prediction horizon could encounter a “loss of information” with the end of the prediction horizon advancing toward the vehicle instead of receding. This situation is illustrated in Figures 5.6 and 5.7, and occurs when the brake acceleration exceeds: −Ux tPH (5.4) where Ux is the vehicle longitudinal speed and tPH is the length of time of the prediction horizon. Acceleration (5.4) can be derived by first defining the distance along the reference line of the end of the prediction horizon: send,PH (t) = tPH Ux (t) + s(t) (5.5) CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 123 12 10 8 6 [m] 4 2 0 −2 −4 −6 15 20 25 [m] 30 35 40 45 Figure 5.7: Braking in excess of (5.4) moves the end of the prediction horizon to toward the vehicle causing the obstacle to leave the horizon and setting its derivative to zero: s˙ end,PH (t) ≈ tPH ax (t) + Ux (t) (5.6) 0 = tPH ax (t) + Ux (t) −Ux (t) ax (t) = tPH (5.7) (5.8) where it is assumed s˙ ≈ Ux and U˙x ≈ ax . If the vehicle brakes at the acceleration specified by (5.8), the end of the prediction horizon will remain fixed in space and the vehicle will safely come to a stop at this point. If the vehicle is braking in response to a threat that exists at the end of the prediction horizon and the vehicle is capable of braking at the amount specified by (5.8), then there is no reason to brake in excess of this amount. Figure 5.8 illustrates the magnitude of this acceleration as a function of vehicle speed. As can be seen at fast speeds and short horizon times, this acceleration value can easily exceed the brake capabilities of typical passenger vehicles; however, using longer horizon times, like the tPH = 4 (s) used in this work, this acceleration is easier to achieve. Therefore, as expected, longer prediction horizons require less CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 124 tPH = 1 (s) 0 tPH = 4 (s) −2 tPH = 8 (s) −4 (m/s2) Brake Acceleration, ax = −Ux / tPH tPH = 2 (s) −6 −8 −10 −12 0 5 10 15 20 25 Vehicle Speed, Ux (m/s) 30 35 40 Figure 5.8: Brake acceleration required to fix the end of the prediction horizon in space CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 125 braking to ensure vehicle safety. 5.3.4 Tire Force Coupling Braking to prevent future violations of the safe driving envelopes should not create new near term envelope violations. This is a possibility because the braking and steering of a vehicle are coupled. This coupling derives from a physical limitation of the tires referred to as the friction circle, which dictates that the total force generated by the tire in any direction cannot exceed the total force available from friction. A common way to visualize this limitation with respect to the acceleration of the vehicle is the g-g diagram as described by Rice [63] and Milliken and Milliken [54] and illustrated in Figure 5.9. As shown, when the vehicle is engaged in full braking, it has no ability to generate lateral force. Likewise, when it is engaged in full lateral acceleration, it has no ability to generate longitudinal force. Kritayakirana and Gerdes [45] further describe the vehicle capabilities conceptualized with the friction circle (g-g diagram) as applied to path planning and control at the limits of handling. This limitation of the tires can be expressed as the following physical constraint on the tire forces: Fx2 + Fy2 ≤ (µFz )2 (5.9) where Fx , Fy , and Fz are the lateral, longitudinal, and normal forces, respectively, acting on the tire and µ is the tire-road surface friction coefficient. The brush tire model (2.7) introduced in Chapter 2 does not capture the effect of this force coupling. Therefore, the proposed braking and steering envelope controller uses a modification to the brush tire model to approximate this coupling. Initially presented by Hindiyeh and Gerdes [37] for use in the control of drifting, this modified brush tire model serves as a simple model of tire force coupling at the limits of friction. This simple model ignores wheel speed dynamics and therefore does not represent extremely fast dynamics. However, the model does capture two key properties: the derated peak lateral force in the presence of a non-zero longitudinal force and the linear characterization of lateral tire force at low slip angles, as shown in Figure 5.10. CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 126 ax Maximum steering ay Friction Limit Maximum braking Figure 5.9: Friction circle concept illustrated using a g-g diagram This modified brush model is the original brush tire model with a derated surface friction coefficient, ξµ: Fy 2 Cα −Cα tan α + 3ξµF | tan α| tan α z 3 Cα = − tan3 α, 27ξµ2 Fz2 −ξµF sgn α, z = fcoupled tire (α, Fx ) |α| < arctan 3ξµFz Cα otherwise (5.10) where ξ is a derating factor (0 ≤ ξ ≤ 1) that accounts for the reduction in lateral force capability in the presence of longitudinal forces and the remaining terms are similarly defined as in the tire model presented in Section 2.2.1. This derating factor CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 127 Tire Lateral Force, Fy Fx = 0% Fx = 25% Fx = 50% Fx = 75% Tire Slip Angle, α Figure 5.10: Coupled tire force model is computed using a rearrangement of constraint (5.9): ξ = q (µFz )2 − Fx2 µFz (5.11) The reverse lookup of the coupled tire model serves to map optimal lateral forces to desired steer angles and is expressed as: −1 α = fcoupled tire (Fy , Fx ) (5.12) Although this modified tire model has the additional dimension of Fx , it can be implemented using the same 2D lookup table described in Chapter 2. This is possible because ξµ appears in (5.10) as a derated surface friction; therefore, a given Fx serves only to augment the surface friction coefficient used in the original brush tire model (2.7). CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 128 5.4 MPC Formulation The tire force coupling described in the previous section needs to be considered when computing the steering and braking commands to be applied to the vehicle. Without this consideration, attempts to prevent future violations of the safe driving envelopes though the use of brake actuation could result in near term envelope violations, doing more harm than good. As shown in Figure 5.5 and described previously, a desired longitudinal force, Fx,des , is determined externally from the MPC optimization, leaving the optimal controller with the simpler decision of determining how much of this desired longitudinal force can be safely commanded at the current time step. An additional convex constraint, which represents the friction circle, and an additional sparsity seeking objective enable the MPC optimization to determine an optimal steering and braking/accelerating command with explicit consideration of the coupling between the two. 5.4.1 Friction Circle Constraint Constraint (5.9) is convex and could be directly incorporated into the convex optimal control problem defined in Chapter 4. Directly incorporating constraint (5.9) into the previously presented QP (4.11) creates a Second Order Cone Program (SOCP), which could be solved exactly using embedded solvers like ECOS [20]. An alternative to this exact representation is to approximate constraint (5.9) as the intersection of a number of half-spaces. This approximation is beneficial for a number of reasons. First, the solver used in this work, CVXGEN, supports QPrepresentable problems only; therefore, the half-space approximation of (5.9) allows for the continued use of this reliable and efficient solver. Second, the half-spaces approximation provides modeling flexibility to capture secondary effects that alter the shape of the friction circle. The circular shape of the friction circle is an idealized model that ignores the effects of weight transfer. When these effects are included, the shape of the friction “circle” is more diamond-like as described in Kritayakirana’s PhD Thesis [46] and the intersection of half-spaces provides the flexibility to approximate this shape better than an ellipsoid. Future work will explore these more complex CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 129 representations of the friction circle and modeling the friction circle constraint directly in a SOCP formulation; however, for simplicity, in this work the friction circle is assumed to be circular and modeled using the half-space approximation in a QP formulation. The half-space approximation of (5.9) can be expressed as: π sin (1) 2π −n n sin (2) 2π − π n n Ly = .. . π 2π sin (n) n − n Ly Fy + Lx Fx ≤ M π cos (1) 2π − n n cos (2) 2π − π n n Lx = .. . π 2π cos (n) n − n (5.13) λµFz λµF z M = .. . λµFz where n is the number of half-spaces and λ is a scale factor. This half-space representation of the friction circle is illustrated in Figure 5.11 using n = 12. The scalar factor, λ, ensures the approximation always lies within the friction circle, and can be expressed as: λ = sin 5.4.2 (n − 2)π 2n (5.14) Considering Front Longitudinal Forces Only As described in Section 2.2.1, mapping of the optimal front tire force, Fyf,opt , to the steer command, δ, allows for direct consideration of the tire force nonlinearity for the front tires. Coupled tire force model (5.10) captures this nonlinearity and the derating of the lateral tire force in the presence of a longitudinal tire force. Therefore, this coupled tire force model provides a mapping from Fyf,opt to δ in the presence of longitudinal forces. This mapping, combined with the friction circle constraint presented in the previous section, enables optimization of both braking and steering of the front tires. However, as also discussed in Section 2.2.1, the inability to steer the rear wheels CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 130 Fx Friction Circle Half−Space Approx. Fy Figure 5.11: Friction circle approximated as the intersection of n = 12 half-spaces CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 131 prevents the use of this nonlinear mapping to model the rear tire forces. Instead, an affine approximation to the nonlinear tire curve provides a model for the rear tire forces linearized around a given operating point, as was extensively investigated in Chapter 4. As presented by Beal and Gerdes [10], this nominal operating point can additionally include longitudinal forces present at the tire to capture the effects of tire force coupling in the short term. Unfortunately, this requires a priori knowledge of the these longitudinal forces, thereby preventing the simple optimization of combined braking and steering that is possible with the front tires. An approximate method for enabling consideration of rear braking is described in Section 5.7, and future work focuses on addressing this challenge. However, to illustrate the basic concept of braking in response to envelope violations, the proposed braking and steering envelope controller considers longitudinal forces on the front tires only because this can be directly incorporated into the MPC optimization. 5.4.3 Optimal Control Problem The optimal control problem presented in Chapter 4 can now be expanded to include this combined braking and steering of the front tires along with the extensions described in Chapter 3 to give Quadratic Program (5.15). An additional optimization variable, Fxf,opt , represents the optimal amount of front axle longitudinal force command that adheres to the friction limits of the front tires. This value is biased to match the desired longitudinal force, Fx,des , which, as described previously, is a minimum of the driver’s command and braking in response to predicted envelope violations. The resulting combined longitudinal and lateral control problem is expressed as: CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 132 minimize ν |Fxf,opt − Fx,des | X (k) (k) (k) + ρ Fyf,opt − Fyf,driver (5.15a) (5.15b) k + X γ (k) 2 (k) (k−1) Fyf,opt − Fyf,opt (5.15c) k + X [σsh (k+1) σsh ] Ssh,opt (5.15d) k + X [σenv σenv ] (l) Senv,opt 2 (5.15e) l + X qT ∆ψ (h) 2 (5.15f) h h = Tterm . . . T (k) (k) (k) (k) subject to x(k+1) = Ad x(k) + Bd Fyf,opt + dd (k) Fyf,opt ≤ Fyf,max (k+1) Hsh x(k+1) ≤ Gsh + Ssh,opt (k) (k−1) (k) Fyf,opt − Fyf,opt ≤ Fyf,max (5.15g) (5.15h) (5.15i) slew (5.15j) k = 0 . . . (T − 1) (l) Henv x(l) ≤ Genv − max FFL x(l) + JFL , FRL x(l) + JRL l = (Tsplit + 1) . . . T (j) Ly Fyf,opt + Lx Fxf,opt ≤ M (5.15k) (5.15l) j = 0 . . . Tbrake where the optimization variables are the input trajectory (Fyf,opt ), the safe driving envelope slack variables (Ssh,opt , Senv,opt ), and the front longitudinal force scalar (Fxf,opt ). Tunable parameters in this optimization problem include ν, ρ, and γ, which establish the trade-off between matching the desired longitudinal force (Fx,des ), matching CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 133 Table 5.1: Braking and Steering Controller Parameters Parameter T Tterm Tsplit Tbrake ν ρ(0) ρ(1:3) ρ(4:T ) γ (0:Tsplit ) γ (Tsplit +1:T −1) (0:Tsplit ) Fyf,max slew (Tsplit +1:T −1) Fyf,max slew qT σsh σenv dbuffer Value 30 28 10 10 8 1.2 0.5 0 5 2 0.79 5 50 [60 60] 1200 0.3 Units (none) (none) (none) (none) 1 kN 1 kN 1 kN 1 kN 1 kN2 1 kN2 kN kN 1 rad2 1 rad 1 m2 s rad m the driver’s present lateral force command (Fyf,driver ), and a smooth steering trajectory. These gains are chosen to prioritize matching the desired longitudinal force, Fx,des , over the driver’s desired lateral force, i.e. ν > ρ, because this desired longitudinal force may be important for the safety of the vehicle. Other tunable parameters include the slack variable costs (σsh , σenv ) and the terminal heading deviation cost (qT ). Table 5.1 gives the values for the controller parameters that will be used in all of the simulations and experiments presented in this chapter unless otherwise specified. (0) The optimal inputs to be applied to the vehicle are Fxf,opt and Fyf,opt . The optimal front brake force (Fxf,opt ) is mapped to an actuator command using: Pf,brake = Kbrake actuator Fxf,opt where Pf,brake is the hydraulic pressure applied to the front brakes and Kbrake (5.16) actuator is a experimentally determined brake actuator gain. The optimal front lateral force (0) (Fyf,opt ) is mapped to a front steering angle, δ, using the inverse lookup of the coupled CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 134 tire model (5.12): δ = β+ ar −1 − fcoupled Ux tire (0) Fyf,opt , Fxf,opt (5.17) Softened constraint (5.15k) enforces the environmental envelope with explicit consideration of the vehicle’s shape as presented in Chapter 3. Terminal cost (5.15f) encourages lane change trajectories as also presented in Chapter 3. Constraint (5.15l) enforces the friction circle on the optimal front longitudinal and lateral forces over the first Tbrake time steps into the prediction horizon. The parameter Tbrake provides a simple method for considering delays in the braking system. Setting Tbrake large ensures the optimal brake command will adhere to the friction circle over a large number of planned steering commands, ensuring adherence to the friction circle even if these brake commands are delayed in appearing at the tires. This enables conservatism in modeling the time response of the brake actuator as illustrated in simulations later in this chapter. 5.4.4 Feasibility of the Constant Speed Trajectory As described in Section 5.3.2, the constant speed assumption is still valid as a result of the fast execution rate of the controller, despite the new capability of the controller to actively brake the vehicle. With this new capability, it is also reasonable to question whether the planned constant speed trajectories are feasible. The constant speed planned trajectories are always feasible because, at any point, a viable option available to the controller is to set Fxf,opt = 0, effectively ceasing to alter the vehicle’s speed. Therefore, at any given time step, a planned constant speed trajectory is a feasible trajectory. In this way, even though the actions of the controller can now directly change the vehicle’s speed, the constant speed assumption is still valid and the planned constant speed trajectories are feasible. CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 135 5.5 Experimental Results Experiments conducted in a limited friction environment using the X1 test bed illustrate the improved capability of the controller to safely navigate the vehicle using combined steering and braking. X1 is equipped with an automatic brake system that operates in parallel with the manual brake system. These manual brakes are controlled exclusively by the driver. As a base for comparison, Figure 5.12 illustrates the performance of the controller navigating a double lane change without any braking. Specifically, the maneuver is an ISO-3888-2 [2] double lane change on low friction at 14 (m/s). Due to the limited testing space available, these experiments use ISO-38882 double lane change specification because it is an aggressive maneuver that reaches the limits of the vehicle at lower speeds than ISO-3888-1. Although the controller performs well at this demanding task without braking, violations of both safe envelopes are evident, indicating that this speed is too fast to safely navigate this environment. Three collisions with the environment occur at the three instances indicated. At instance 1 , the vehicle clips the environment as the controller attempts to minimize future collisions by prematurely initiating the double lane change. The steering commands leading up to instance 2 correspond to circling around the lower bound of the stable handling envelope as shown in the second plot. The vehicle comes into contact with the environment at instance 3 before the controller safely matches the driver’s command again. 5.5.1 Proportional Braking Policy Figure 5.13 illustrates the same maneuver with the same initial speed, but equipped with the proportional braking policy (5.3) and coupled tire force optimal control problem (5.15). As shown, the controller safely slows the vehicle and completes the maneuver without violating either safe envelope. Table 5.2 provides the braking policy parameters used in this experiment. At instance 1 , the desired braking saturates to the maximum brake force for driver comfort, Fx,maxComfort , in response to the large predicted envelope violation. This predicted violation is illustrated in Figure 5.14 along with the corresponding rear CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 136 Vehicle Path Vehicle Width Obstacles Distance (m) 6 2 4 2 1 3 0 0 10 20 30 40 Distance (m) 60 20 1 3 0 −20 2 −10 −5 0 5 Side Slip, β (deg) 10 Actual Driver Steer Angle, δ (deg) 20 1 3 0 2 (m/s) −20 Speed, Ux 70 Stable Handling Envelope Actual 40 Yaw Rate, r (deg/s) 50 14 12 10 8 6 0 1 2 0 1 2 Time (s) 3 4 5 3 4 5 Figure 5.12: An experimental ISO 3888-2 double lane change on a low friction surface (µ ≈ 0.5) without braking CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 137 Vehicle Path Vehicle Width Obstacles Distance (m) 6 4 2 1 3 2 0 20 0 40 60 Distance (m) 80 2 3 −2 Actual Desired 1 −3 −4 0 2 4 6 8 Steer Angle, δ (deg) 20 0 1 10 3 Actual Driver 10 2 −10 (m/s) −20 Speed, U x 100 −1 (kN) Front Longitudinal Force, F xf 0 14 12 10 8 6 0 2 4 0 2 4 6 6 Time (s) 8 10 8 10 Figure 5.13: An experimental ISO 3888-2 double lane change on a low friction surface (µ ≈ 0.5) with braking in proportion to predicted envelope violation CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 138 Current State Near Pred Long Pred Actual 40 30 6 Nonlinear Tire Curve 4 10 0 −10 2 (kN) Rear Lateral Force, Fyr Yaw Rate, r (deg/s) 20 0 −2 −20 −4 −30 −6 −40 −15 −10 −5 0 5 Side Slip, β (deg) 10 15 −5 0 5 Rear Slip Angle, αr (deg) Figure 5.14: Predicted envelope violation and corresponding predicted rear tire lateral forces at instance 1 from the experiment illustrated in Figure 5.13 Current State Near Pred Long Pred Actual 40 30 6 Nonlinear Tire Curve 4 10 0 −10 2 (kN) Rear Lateral Force, Fyr Yaw Rate, r (deg/s) 20 0 −2 −20 −4 −30 −6 −40 −15 −10 −5 0 5 Side Slip, β (deg) 10 15 −5 0 5 Rear Slip Angle, αr (deg) Figure 5.15: Predicted envelope violation and corresponding predicted rear tire lateral forces at instance 2 from the experiment illustrated in Figure 5.13 CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 139 Current State Near Pred Long Pred Actual 40 30 Nonlinear Tire Curve 6 4 10 0 −10 2 (kN) Rear Lateral Force, Fyr Yaw Rate, r (deg/s) 20 0 −2 −20 −4 −30 −6 −40 −15 −10 −5 0 5 Side Slip, β (deg) 10 15 −5 0 5 Rear Slip Angle, αr (deg) Figure 5.16: Predicted envelope violation and corresponding predicted rear tire lateral forces at instance 3 from the experiment illustrated in Figure 5.13 tire forces. At instance 1 , the optimal vehicle states do not safely return within the stable handling envelope as a result of the prioritization of collision avoidance over stability. Using the approximate nonlinear rear tire model, the controller correctly identifies the current speed as too fast for upcoming conditions. At instance 2 , the vehicle’s speed has greatly reduced and the predicted envelope violations, which are shown in Figure 5.15, are much smaller resulting in a smaller braking command. At instance 3 , the vehicle emerges from the double lane change unscathed with the vehicle states just reaching the stable handling envelope bound as illustrated in Figure 5.16. Throughout this experiment, the friction circle constraint largely remains inactive, Table 5.2: Proportional Braking Parameters Parameter Kbrake Fxf,maxComfort Value -12 -3.6 Units kN kN CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 140 Table 5.3: Fixed Braking Parameters Parameter Fxf,fixed Value -3 Units kN and the desired front longitudinal force, Fxf,des , identically matches the commanded front tire force, Fxf,opt . This will not be the case for more aggressive braking policies. 5.5.2 Fixed Braking Policy Figure 5.17 illustrates the controller implementing the fixed braking policy (5.2) for the same double lane change maneuver at the same initial speed. For this experiment, the brake parameters used are specified in Table 5.3. Despite the change in braking policy, the vehicle’s speed, as expected, is reduced to a similar level as the previous braking policy, indicating both policies converge to the same safe maximum speed. And, like the previous example, the controller successfully navigates the maneuver without violating either safe envelope. At time instance 1 , the second lane change comes into view of the prediction horizon, which causes a slight predicted envelope violation. Following braking policy (5.2), a fixed brake value is commanded in response to this small predicted violation. However, the optimal brake force applied to the vehicle is much less than this desired fixed amount because the full friction capabilities of the front tires are currently required to steer the vehicle, and the controller is explicitly considering this tire force coupling. The steering command at this instant is rapidly decreasing as the controller completes the first lane change. Even though the steering angle is not saturated, the lateral force from the front tires is saturated. This is a result of the extreme maneuver being executed and the dependency of front lateral force on both steer angle and vehicle states, as previously described by equation (5.17). This is something that may not be intuitive to the average driver but is explicitly considered by the controller. The extreme steering of the maneuver at instance 1 naturally causes a slight drop in vehicle speed without the use of brake actuation. This drop in vehicle speed is enough to eliminate the predicted envelope violation, and, in CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 141 accordance with the braking policy (5.2), results in no commanded braking shortly after instance 1 . This experiment illustrates the ability of the controller to handle the coupling between steering and braking in real-time as it avoids collisions and stabilizes the vehicle. This ability is also important during emergency evasive maneuvers as described in the next section. CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 142 Vehicle Path Vehicle Width Obstacles Distance (m) 6 1 4 2 0 20 40 60 Distance (m) 80 0 −1 (kN) Front Longitudinal Force, F xf 0 Actual Desired −2 −3 1 −4 0 2 4 6 8 10 8 10 8 10 Steer Angle, δ (deg) 20 0 1 −10 (m/s) x −20 Speed, U Actual Driver 10 14 12 10 8 6 0 2 4 0 2 4 6 6 Time (s) Figure 5.17: An experimental ISO 3888-2 double lane change on a low friction surface (µ ≈ 0.5) with a fixed brake amount in response to predicted envelope violation CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 143 5.6 Extension to Brake- and Throttle-By-Wire Up to this point, Fxf,des has been largely dictated in response to predicted envelope violations. However, as shown in the block diagram in Figure 5.5, this desired longitudinal force could additionally come from the driver. Although X1 is equipped with automatic brakes, the driver always maintains control over a set of manual brakes; therefore, the driver’s brake command cannot be overridden as would be possible in a true brake-by-wire system. For this reason, simulation results illustrate the controller’s application using brake-by-wire. Figure 5.18 illustrates a scenario where a driver overreacts and panic brakes in response to a pop-up obstacle with the controller intervening to bring the vehicle safely to a stop. At instance 1 , a pop-up obstacle appears 30 (m) in front of the vehicle. The driver reacts immediately with a step steer command. The controller augments this step command to adhere to the stable handling envelope. This is followed by an aggressive brake command by the driver a short time later at instance 2 . The controller augments this brake command to ensure the combined steering and braking of the front wheels adheres to the friction limits as illustrated by the friction circle plotted in Figure 5.19. Transitioning from instance 2 to 3 , the controller commands optimal lateral and longitudinal forces near the friction limits of the front tires providing near maximum braking while allowing for the steering force necessary to avoid collision and loss of control. At instance 4 , the full brake command of the driver is safely applied to the vehicle. From Figure 5.19, it is seen that the controller commands forces near, but not always exactly on, the friction circle during the panic brake event. This is a result of the conservatism introduced by using Tbrake = 9. Since the optimal longitudinal force scalar is constrained to be within the friction circle for all Tbrake time steps, the optimal longitudinal force is decreased as the controller plans to increase the lateral force in the near term. This hedges against delays in the brake system leading to saturation of the front tires on subsequent time steps. If instead Tbrake = 0, the controller would brake up to the friction limits at each time step during the panic brake event as indicated in Figure 5.20; however, this assumes a more aggressive CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 144 model of the time response of the brake actuator. The extension to throttle-by-wire is straight forward on front-wheel-drive vehicles. In this case, throttle commands would result in positive values of Fxf,des with brake commands equating to negative values of Fxf,des . This would allow the controller to override the driver’s acceleration commands in the exact same manner as brakes to safely avoid collisions and loss of control. CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 145 8 Vehicle Path Vehicle Width Obstacles Distance (m) 6 4 4 1 2 2 0 0 10 20 30 40 Distance (m) 50 60 0 (kN) Front Longitudinal Force, Fxf 3 Actual Driver −2 1 2 3 4 −4 −6 0 2 4 6 1 10 8 Actual Driver 20 Steer Angle, δ (deg) 70 0 2 4 3 −10 −20 0 2 4 6 8 0 2 4 Time (s) 6 8 (m/s) Speed, U x 15 10 5 0 Figure 5.18: 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 collision CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 146 Friction Circle Front Tire Forces 5 4 3 2 Fxf [kN] 1 0 1 −1 −2 −3 3 2 −4 4 −5 −5 −4 −3 −2 −1 0 Fyf [kN] 1 2 3 4 5 Figure 5.19: Friction circle for the front axle during the simulated panic brake maneuver on a low friction surface (µ ≈ 0.5) with Tbrake = 9 CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 147 Friction Circle Front Tire Forces 5 4 3 2 Fxf [kN] 1 0 −1 −2 −3 −4 −5 −5 −4 −3 −2 −1 0 1 Fyf [kN] 2 3 4 5 Figure 5.20: Friction circle for the front axle during the simulated panic brake maneuver on a low friction surface (µ ≈ 0.5) with Tbrake = 0 CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 148 5.7 Future Work Considering braking as a steering feasibility problem provides a nice simplification to the challenging control problem of combined braking and steering. The implementation of this idea presented in this chapter works well for many driving situations; however, there are some limitations left to future work. The first limitation is the use of front longitudinal forces only, as mentioned previously in Section 5.4.1. Since passenger cars are predominately front-wheel-drive and brake forces come predominately from the front brakes, due to weight transfer effects during braking, the front longitudinal force only approach presented in this chapter is well suited to typical passenger vehicles as is. However, considering only front longitudinal forces does limit the peak brake force available to the vehicle and is therefore not acceptable for a production safety system. To enable the envelope controller access to the full braking capability of the vehicle, future work will seek to incorporate the coupled tire model for the rear wheels as well as the front. A starting approach could be to introduce another optimization variable representing an optimal rear longitudinal force, Fxr,opt , and simply impose the friction circle constraint (5.9) on this force and the rear lateral force, which is modeled using the affine approximation of the nonlinear tire curve. This would ensure the rear tire brake force, Fxr,opt , adheres to the friction circle. The challenge comes with respect to the derating of the rear lateral force in the presence of this non-zero longitudinal force. As a result of the rear tire affine model, the lateral rear force computed by the MPC optimization will not reflect this derating and therefore this approach will only be an approximation. Future work will determine how accurate or useful this approximation can be. Another limitation arises from the constant speed assumption. Assuming a constant speed throughout the prediction horizon limits the trajectories evaluated by the controller to only a subset of the feasible trajectories. In most cases, this is not a problem. In more extreme maneuvers, a safe variable speed trajectory may exist where a safe constant speed trajectory does not, resulting in unnecessary intervention by the controller. Although this may be irksome to the driver, the controller still ensures the safe operation of the vehicle. However, the constant speed assumption CHAPTER 5. ENVELOPE CONTROL USING BRAKING AND STEERING 149 does result in the controller choosing to always steer to avoid collision, and, in some rare cases, steering alone or combined steering and braking could lead to a collision when there may exist a collision-free trajectory using braking only. Due to the long 4 (s) prediction horizon, these situations would only arise with pop-up obstacle over a small range of vehicle speeds [55]. Fortunately in these situations, the controller would identify the pending collision. Therefore, external logic could be used to force the controller to consider non-steering trajectories in these scenarios. This could be accomplished through augmentation of the road bounds to force the controller to remain within its present lane or the generation of a future speed profile that varies. Therefore, if an appropriate future speed profile could be computed externally from the optimal controller, these situations could be avoided and the approach presented in this chapter could be more broadly applied with Fxf,des coming partially from this planned speed profile. 5.8 Discussion Introducing a desired longitudinal force into the optimal control formulation avoids the computationally challenging task of generating an optimal brake and steer trajectory in real-time. Instead, the controller simply determines the amount of longitudinal force that can be safely commanded at the current time step. This desired longitudinal force is generated to slow the vehicle in response to predicted envelope violations, creating a convex feasibility problem whose solution is the maximum safe vehicle speed for the given combination of road conditions and environmental hazards. In addition, this desired front longitudinal force can also 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. Chapter 6 Conclusion Future driver assistance systems will continue to move beyond vehicle stabilization and additionally consider environmental threats to vehicle safety. Enabled by significant improvements in sensing and actuation capabilities, automated systems can assume more responsibility in ensuring vehicle safety, freeing human drivers of the burden of perfection in the safe control of their vehicles. This sharing of control permits a synergy that combines the unique cognitive and reasoning abilities of humans with the vigilance and precision of machines. This dissertation presents a simple and effective approach to shared control between a human driver and a highly capable automated system, implementing a form of envelope control to ensure vehicle safety. Inspired by envelope control approaches in both aircraft and vehicle control, the design and development of the proposed framework makes a number of contributions of its own. The main contributions involve modeling and control formulations that make possible real-time trajectory optimization with explicit consideration of driver autonomy, obstacle avoidance, and vehicle stability. A simple representation of the environment allows for the fast generation of safe and feasible trajectories in real-time. A sparsity seeking objective biases the MPC optimal solution to identically match the driver’s command whenever possible. As a result of this strong bias to match the driver, the optimal steering solution lends itself to the generation of a directional haptic feedback signal to intuitively communicate the controller’s intentions to the driver. As demonstrated in experimental validation, 150 CHAPTER 6. CONCLUSION 151 this feedback encourages cooperation between driver and machine. The challenging task of combined lateral and longitudinal control is cast as a feasibility problem whose solution is the maximum safe vehicle speed for the given combination of road conditions and environmental hazards. Applying successive linearization to a pathless MPC approach enables real-time identification of vehicle speeds that are too fast for the safe operation of the vehicle. This allows for a comprehensive envelope controller capable of ensuring vehicle safety in a wide range of driving scenarios through augmentation of the driver’s steering, throttle, and brake commands. Extensive validation and testing with experimental vehicles provides support to the analysis and design of this control framework. The resulting control framework is a powerful alternative to the path tracking and following paradigm commonly employed in automated vehicle control. 6.1 Future Work The impact and contribution of a research project is demonstrated by the new and exciting research directions that it inspires. The envelope control framework presented in this dissertation serves as the foundation for a number of new research projects addressing various vehicle control problems. 6.1.1 Fully Autonomous Vehicles The design of the proposed control framework focuses on cooperation with a human driver. However, this framework could be used in the control of a fully automated vehicle. Removing the objective to match the driver’s steer command results in an autonomous steering controller capable of both obstacle avoidance and stability. Extending the heading deviation cost through the prediction horizon and adding a cost on deviations from a desired lateral offset, a path tracking objective is easily incorporated, which, if prioritized much lower than the obstacle avoidance and stability objectives, provides guidance to the controller in the absence of threats to vehicle safety. Implementing a fully autonomous vehicle is a major undertaking, but this CHAPTER 6. CONCLUSION 152 modified envelope controller is well suited to be the coordinator of the steering, braking, and throttle commands to achieve higher level objectives in a fully autonomous vehicle. This transforms the complex problem of generating safe and feasible trajectories through the environment into higher level objectives like desired path and desired longitudinal force, simultaneously simplifying the control problem while ensuring vehicle safety. Experimental results using X1 have already provided a proof-of-concept of this approach to fully automated driving, and future work seeks to build upon this initial success. 6.1.2 Haptic Feedback User Studies To the author’s knowledge, using an MPC optimal trajectory as the basis for a directional haptic feedback signal has not been previously explored in the literature. The results presented in Chapter 3 show promise that this haptic signal can provide a useful and intuitive method of communication between the control system and the human driver. Future research focuses on exploring the subjective user experience and overall effectiveness of this approach to haptic feedback for shared vehicle control. In particular, a user study is underway with the goal of evaluating drivers’ responses to pop-up obstacles and of understanding how the proposed haptic signal influences these responses. 6.1.3 Application to Racing As discussed briefly in Chapter 5, situations exist where considering variable speed trajectories would improve the performance of the controller. The challenge arose in how to compute these speed trajectories in real-time. In certain driving situations, like racing, these variable speed profiles could be generated off-line and used in a modified form of the proposed envelope controller. Research efforts are underway to apply this approach to Shelley, an autonomous race car jointly developed by Stanford and Audi [45]. CHAPTER 6. CONCLUSION 6.1.4 153 Implementable Ethics The proposed envelope controller uses multiple objectives to ensure safe vehicle operation. As illustrated a number of times, these objectives sometime compete and the tiered costs establish an objective hierarchy that guides the controller in these situations. In doing this, the envelope controller implements a type of ethical framework, and current research explores how the design of these types of control systems might change if viewed through the lens of various established, ethical frameworks. This interdisciplinary research attempts to bridge the gap between philosophy and engineering. 6.1.5 Leveraging Advances in Parallel Computing Modern processing units increasingly incorporate multiple processing cores to boost computational performance. The proposed envelope controller is well suited to leverage these advancements in parallel processing hardware. The method of dividing the non-convex obstacle avoidance problem into a number of convex sub-problems enables a parallel processing unit to evaluate all of the sub-problems very quickly. Future research efforts are underway to port the current implementation of the envelope controller to parallel processing hardware, enabling the real-time consideration of environments with many obstacles. 6.2 Outlook Utilizing recent innovations in vehicle actuation and sensing, the proposed envelope control framework shows great promise for ensuring vehicle safety. This work highlights the capabilities and opportunities of next generation driver assistant systems in protecting the driver against environmental hazards in addition to stabilizing the vehicle. These systems will play a more active role in driving, ensuring vehicle safety in situation where, currently, the driver shoulders much of the burden. Off-loading some of this responsibility to an envelope controller like the one presented would leverage CHAPTER 6. CONCLUSION 154 the precision and vigilance of machines while still retaining the critical thinking abilities of humans. These next generation driver assistance systems will radically improve the nature and safety of driving, while still providing drivers with the mobility and flexibility of the automobile. Bibliography [1] Passenger cars - test track for a severe lane-change manoeuvre - part 1: Double lane-change, 1999. [2] Passenger cars - test track for a severe lane-change manoeuvre - part 2: Obstacle avoidance, 2011. [3] Volvo collision avoidance features: initial results. Technical report, Highway Loss Data Institute, April 2012. Vol. 29 (5). [4] AASHTO. Aashto green book: A policy on geometric design of highway and streets, 6th ed. Technical report, American Association of State Highway and Transportation Officials, Washington D.C., USA, 2011. [5] Robert C. Allen and Harry G. Kwatny. Maneuverability and envelope protection in the prevention of aircraft loss of control. In 8th Asian Control Conference (ASCC), pages 381–386, 2011. [6] S.J. Anderson, S.B. Karumanchi, and K. Iagnemma. Constraint-based planning and control for safe, semi-autonomous operation of vehicles. In IEEE Intelligent Vehicles Symposium, pages 383–388, June 2012. [7] Rachid Attia, Jeremie Daniel, Jean-Philippe Lauffenburger, Rodolfo Orjuela, and Michel Basset. Reference generation and control strategy for automated vehicle guidance. In IEEE Intelligent Vehicles Symposium (IV), pages 389–394, June 2012. 155 BIBLIOGRAPHY 156 [8] Avinash Balachandran, Stephen M. Erlien, and J. Christian Gerdes. The virtual wheel concept for supportive steering feel during large active steering interventions. Proceedings of the ASME Dynamic Systems and Control Conference, September 2014. [9] Avinash Balachandran and J. Christian Gerdes. Designing steering feel for steerby-wire vehicles using objective measures. IEEE/ASME Transactions on Mechatronics, PP(99):1–11, 2014. [10] C. E. Beal and J. C. Gerdes. Model predictive control for vehicle stabilization at the limits of handling. IEEE Transactions on Control Systems Technology, 2013. [11] Craig E. Beal. Applications of Model Predictive Control to Vehicle Dynamics for Active SaSafe and Stability. PhD thesis, Stanford University, 2011. [12] G P Bevan, H Gollee, and J OReilly. Trajectory generation for road vehicle obstacle avoidance using convex optimization. Journal of Automobile Engineering, 224:455–473, 2010. [13] Carrie G. Bobier and J. Christian Gerdes. Staying within the nullcline boundary for vehicle envelope control using a sliding surface. Vehicle System Dynamics: International Journal of Vehicle Mechanics and Mobility, 51(2):199–217, 2013. [14] S. Boyd and L. Vandenberghe. Convex Optimization. Cambridge University Press, Cambridge, UK, 2004. [15] Marc Starnes & Marilouise Burgess. Passenger vehicle occupant fatalities: The decline for six years in a row from 2005 to 2011. Technical Report DOT HS 812 034, NHTSA of the U.S. Department of Transportation, June 2014. [16] Robert N. Charette. Nissan moves to steer-by-wire for select infiniti models. IEEE Spectrum, October 2012. [17] Chulho Choi, Yeonsik Kang, and Seangwock Lee. Emergency collision avoidance maneuver based on nonlinear model predictive control. In In proceedings of the BIBLIOGRAPHY 157 IEEE International Conference on Vehicular Electronics and Safety, pages 393– 398, July 2012. [18] Jennifer N. Dang. Statistical analysis of the effectiveness of electronic stability control systems - final report. Technical Report DOT HS 810 794, NHTSA of the U.S. Department of Transportation, July 2007. [19] Nuno M. C. de Oliveira and Lorenz T. Biegler. Constraint handling and stability properties of model-predictive control. American Institute of Chemical Engineers, 40:1138–115, July 1994. [20] A. Domahidi, E. Chu, and S. Boyd. ECOS: An SOCP solver for embedded systems. In European Control Conference (ECC), pages 3071–3076, 2013. [21] Nicoleta Minoiu Enache, Sad Mammar, Mariana Netto, and Benoit Lusetti. Driver steering assistance for lane-departure avoidance based on hybrid automata and composite lyapunov function. IEEE Transactions on Intelligent Transportation Systems, 11(1), March 2010. [22] Stephen M. Erlien, Susumu Fujita, and J. Christian Gerdes. Shared steering control using safe envelopes for obstacle avoidance and vehicle stability. IEEE Transactions on Intelligent Transportation Systems, 2014. (submitted, under review). [23] Stephen M. Erlien, Joesph Funke, and J. Christian Gerdes. Incorporating nonlinear tire dynamics into a convex approach to shared steering control. In American Control Conference, pages 3468 – 3473, June 2014. [24] A. Muller F. von Hundelshausen, M. Himmelsbach and H.-J.Wunsche. Diriving with tentacles - integrated structures of sensing and motion. International Journal of Field Robotics Research, 25(9):640–673, 2008. [25] P. Falcone, F. Borrelli, J. Asgari, H.E. Tseng, and D. Hrovat. A model predictive control approach for combined braking and steering in autonomous vehicles. In Mediterranean Conference on Control Automation, pages 1–6, June 2007. BIBLIOGRAPHY 158 [26] P. Falcone, F. Borrelli, E. Tseng, J.Asgari, and D. Hrovat. Linear time-varying model predictive control and its application to active steering systems: Stability analysis and experimental validation. International Journal of Robust and Nonlinear Control, 18:862–875, 2007. [27] Paolo Falcone, Francesco Borrelli, H. Eric Tseng, Jahan Asgari, and Davor Hrovat. Low complexity mpc schemes for integrated vehicle dynamics control problems. In Proceedings of the 9th International Symposium on Advanced Vehicle Control (AVEC), 2008. [28] Paolo Falcone, H. Eric Tseng, Francesco Borrelli, Jahan Asgari, and Davor Hrovat. Mpc-based yaw and lateral stabilization via active front steering and braking. Vehicle System Dynamics: International Journal of Vehicle Mechanics and Mobility, 46:611–628, Sept 2008. [29] E. Fiala. Lateral forces on rolling pneumatic tires. In Zeitschrit V.D.I, volume 96, 1954. [30] Center for Disease Control and Prevention. Injury prevention & control: Data & statistics, September 2014. [31] B.A.C. Forsyth and K.E. MacLean. Predictive haptic guidance: intelligent user assistance for the control of dynamic tasks. IEEE Transactions on Visualization and Computer Graphics, 12(1):103–113, Jan 2006. [32] Joesph Funke and J. Christian Gerdes. Simple clothoid paths for autonomous vehicle lane change at the limits of handling. In proceedings of the ASME Dynamic Systems and Control Conference, October 2013. [33] Joseph Funke, Mathew Brown, Stephen M. Erlien, and J. Christian Gerdes. Discretionary stability control integrated with obstacle avoidance and path tracking for autonomous vehicles. In proceedings of the IEEE Intelligent Vehicles Symposium, 2015. (submitted, under review). BIBLIOGRAPHY 159 [34] Yiqi Gao, Andrew Gray, J. V. Frasch, Theresa Lin, H. Eric Tseng, J. Karl Hedrick, and Francesco Borrelli. Spatial predictive control for agile semi- autonomous ground vehicles. In International Symposium on Advanced Vehicle Control, 2012. [35] Yiqi Gao, Theresa Lin, Francesco Borrelli, Eric Tseng, and Davor Hrovat. Predictive control of autonomous ground vehicles with obstacle avoidance on slippery roads. In Dynamic Systems and Control Conference, 2010. [36] Andrew Gray, Yiqi Gao, Theresa Lin, J. Karl Hedrick, H. Eric Tseng, and Francesco Borrelli. Predictive control for agile semi-autonomous ground vehicles using motion primitives. In American Control Conference (ACC), 2012, pages 4239 –4244, june 2012. [37] Rami Y. Hindiyeh and J. Christian Gerdes. A controller framework for autonomous drifting: Design, stability, and experimental validation. Journal of Dynamic Systems Measurements and Control, 136(5), July 2014. [38] Y H Judy Hsu and J. Christian Gerdes. Envelope control: Keeping the vehicle within its handling limits using front steering. In 21st International Symposium on Dynamics of Vehicles on Roads and Tracks, 2009. [39] Y.-H.J. Hsu, S.M. Laws, and J.C. Gerdes. Estimation of tire slip angle and friction limits using steering torque. IEEE Transactions on Control Systems Technology, 18(4):896–907, July 2010. [40] S. Inagaki, I. Kshiro, and M. Yamamoto. Analysis of vehicle stability in critical cornering using phase-plane method. In Proceedings of the International Symposium on Advanced Vehicle Control, 1994. [41] Rolf Isermann, Roman Mannale, and Ken Schmitt. Collision-avoidance systems proreta: Situation analysis and intervention control. Control Engineering Practice, 20(11):1236–1246, Nov 2012. BIBLIOGRAPHY 160 [42] Alexander Katriniok, Jan P. Maschuw, Frederic Christen, Lutz Eckstein, and Dirk Abel. Optimal vehicle dynamics control for combined longitudinal and lateral autonomous vehicle guidance. In In Proceedings of the European Control Conference, number 978-3-033-03962-9, pages 974–979, July 2013. [43] Taketoshi Kawabe, Hikaru Nishira, and Toshiyuki Ohtsuka. An optimal path generator using a receding horizon control scheme for intelligent automobiles. In IEEE International Conference on Control Applications, 2004. [44] Mykel J. Kochenderfer, Jessica E. Holland, and James P. Chryssanthacopoulos. Next generation airborne collision avoidance system. LINCOLN LABORATORY JOURNAL, 19:55–71, 2012. [45] Krisada Kritayakirana and J. Christian Gerdes. Autonomous vehicle control at the limits of handling. International Journal of Vehicle Autonomous Systems, 10(4):271–296, Jan 2012. [46] Krisada (Mick) Kritayakirana. Autonomous Vehicle Control at the Limits of Handling. PhD thesis, Stanford University, 2012. [47] James K. Kuchar and Ann C. Drumm. The traffic alert and collision avoidance system. Lincoln Laboratory Journal, 16(2):277–296, 2007. [48] Steven M LaValle. Planning Algorithms. Cambridge University Press, 2006. [49] Shad M. Laws, Christopher D. Gadda, and J. Christian Gerdes. Frequency characteristics of vehicle handling: Model and experimental validation of yaw, sideslip, and roll modes to 8 hz. In Proceedings of the AVEC 8th International Symposium on Advanced Vehicle Control, number 0211, August 2011. [50] Jan M Maciejowski. Predictive Control with Constraints. Prentice Hall, 2000. [51] J. Mattingley and S. Boyd. Cvxgen: a code generator for embedded convex optimization. Optimization and Engineering, 13(1):1–27, 2012. BIBLIOGRAPHY 161 [52] J. Mattingley, Yang Wang, and S. Boyd. Code generation for receding horizon control. In 2010 IEEE International Symposium on Computer-Aided Control System Design (CACSD), pages 985 –992, sept. 2010. [53] David Q. Mayne, James B. Rawlings, Christopher V. Rao, and Pierre O. M. Scokaert. Constrained model predictive control: Stability and optimality. Automatica, 36(6):789–814, 2000. [54] William F. Milliken and Douglas L. Milliken. Race Car Vehicle Dynamics, pages 345–359. SAE International, 1995. [55] Nikolai Moshchuk, Shih-Ken Chen, Chad Zagorski, and Amy Chatterjee. Optimal braking and steering control for active safety. In Proceedings of the 15th IEEE Conference on Intelligent Transportation Systems, pages 1741–1746, 2012. [56] M. Nanao and T. Ohtsuka. Nonlinear model predictive control for vehicle collision avoidance using c/gmres algorithm. In In proceedings of the IEEE International Conference on Control Applications, pages 1630–1635, Sept 2010. [57] NHTSA. Traffic safety facts: 2012 data. Technical Report DOT HS 812 016, U.S. Department of Transportation, May (Revised) 2014. [58] NHTSA. Traffic safety facts: Estimating lives saved by electronic stability control, 2008-212. Technical Report DOT HS 812 042, U.S. Department of Transportation, June 2014. [59] M. D. North. Finding common ground in envelope protection systems. Aviation Week and Space Technology, August 2008. [60] U.S. Department of Transportation. Transportation statistics annual report 2012. Technical report, Research and Innovative Technology Administration Bureau of Transportation Statistics, Washington, D.C., August 2013. [61] Meeko Oishi, Ian Mitchell, Claire Tomlin, and Patrick Saint-Pierre. Computing viable sets and reachable sets to design feedback linearizing control laws under BIBLIOGRAPHY 162 saturation. In 45th IEEE Conference on Decision and Control, pages 3801–3807, Dec 2006. [62] Hans B. Pacejka. Tire and Vehicle Dynamics. Butterworth-Heinemann, 3rd edition, 2012. [63] R. S. Rice. Measuring car-driver interaction with the g-g diagram. Society of Automotive Engineers, Warrendale, PA, (730018):1–19, 1973. [64] Eric J. Rossetter, Joshua P. Switkes, and J. Christian Gerdes. Experimental validation of the potential field lanekeeping system. International Journal of Automotive Technology, 5:98–108, 2004. [65] Louay Saleh, Philippe Chevrel, Fabien Claveau, Jean-Franois Lafay, and Franck Mars. Shared steering control between a driver and an automation: Stability in the presence of driver behavior uncertainty. IEEE Transactions on Intelligent Transportation Systems, 14(2):974–983, June 2013. [66] Pierre O. M. Scokaert and James B. Rawlings. Feasibility issues in linear model predictive control. American Institute of Chemical Engineers, 45:1649–1659, 1999. [67] T. B. Sheridan and R. Parasuram. Human-automation interaction. Rev. Human Factors Ergonomics, 1(1):89–129, 2005. [68] Suk-Hwan Suh and Albert B. Bishop. Collision-avoidance trajectory planning using tube concept: Analysis and simulation. Journal of Robotic Systems, 5:497– 525, 1988. [69] Sabastian Thrun. Stanley: The robot that won the darpa grand challenge. Journal of Field Robotics, 23(9):661–692, 2006. [70] J. P. Timings and D. J. Cole. Minimum maneuver time calculation using convex optimization. Journal of Dynamic Systems Measurements and Control, 135, March 2013. BIBLIOGRAPHY 163 [71] Valerio Turri, Ashwin Carvalho, Hongtei Eric Tseng, Karl Henrik lohansson, and Francesco Borrelli. Linear model predictive control for lane keeping and obstacle avoidance on low curvature roads. In IEEE International Conference on Intelligent Transportation Systems, Oct 2013. [72] Chris Urmson. Autonomous driving in urban environments: Boss and the urban challenge. Journal of Field Robotics, 25(8):425–466, 2008. [73] Klaus H Well. Aircraft control laws for envelope protection. In AIAA Guidance, Navigation, and Control Conference, number AIAA-2006-6055, 2006. [74] Mortiz Werling and Darren Liccardo. Automatic collision avoidance using modelpredictive online optimization. In Proceedings of the 51st IEEE Conference on Decision and Control, December 2012. [75] Fernando A. Wilson and Jim P. Stimpson. Trends in fatalities from distracted driving in the united states, 1999 to 2008. American Journal of Public Health, 100(11):2213–2219, 2012. [76] M.N. Zeilinger, M. Morari, and C.N. Jones. Soft constrained model predictive control with robust stability guarantees. IEEE Transactions on Automatic Control, 59(5):1190–1202, May 2014.
© Copyright 2024