Guidance of an O?-Road Tractor-Trailer System Using Model Predictive Control by James T. Salmon AthesissubmittedtotheGraduateFacultyof Auburn University in partial fulfillment of the requirements for the Degree of Master of Science Auburn, Alabama December 14, 2013 Keywords: MPC, Coupled Ground Vehicles, Mobile Robots Copyright 2013 by James T. Salmon Approved by: David M. Bevly, Chair, Professor of Mechanical Engineering John Y. Hung, Professor of Electrical and Computer Engineering Song-yul Choe, Professor of Mechanical Engineering Acknowledgments I would like to dedicate this thesis to all my friends and family who supported me during my academic career, starting with my immediate family ? my parents, Cyndy and Thad Salmon and my sister, Emily. Throughout the tough moments, you have always been there to keep me on the right track, you never gave up on me, and I don?t think I would have made it anywhere near this point without you. I also want to thank my extended family in Alabama. First, my grandparents, Cecil and Nell Prescott, as well as my grandmother, Mabel Salmon. Thank you for helping me in my move to Auburn. Thank you to my grandfather, Joseph T. Salmon Sr., who is no longer with us. You did some amazing things for our family, and you are greatly missed. Also, thank you to my aunts, uncles, and cousins out here who helped introduce me to Southern life. Watching football games with you have been great fun! Next, I want to thank my advisor, Dr. David Bevly, for o?ering me a research assis- tantship position in the GAVLAB. Also, thank you to Dr. John Hung and Dr. Song-Yul Choe whom I consider co-advisors, for their guidance. Graduate school was challenging, but IlearnedalotmoreaboutcontrolsystemsthanIeverknewexisted,andIamconfidentthat Iwouldbeabletocontributemuchasacontrolsengineerinmycareertocome. Also, thank you to my colleagues in the GAVLAB, to all of my friends in Auburn, and to all of my friends in California. You all have been a positive influence on me throughout the years, and I wish you all the best. Finally, I want to thank my great-great grandfather, Dr. Frederic C. Biggin, for his dedication to Auburn University. Your contributions in the Auburn School of Architecture set the stage for Auburn?s development into the great institution it has become. Auburn is in good hands, it has a bright future ahead, and I am glad to be a part of it. War Eagle! ii Table of Contents Acknowledgments . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ii List of Figures . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . v List of Tables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . viii 1Introduction......................................2 1.1 Background . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2 1.2 Previous Work at Auburn University . . . . . . . . . . . . . . . . . . . . . . 3 1.3 Proposing Model Predictive Control as a Potential Solution . . . . . . . . . . 6 1.4 Outline . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8 2 Kinematic and Dynamic Modeling of the System . . . . . . . . . . . . . . . . . 9 2.1 Kinematic Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9 2.1.1 List of Parameters & Velocity Diagram . . . . . . . . . . . . . . . . . 9 2.1.2 Kinematic Model Derivation . . . . . . . . . . . . . . . . . . . . . . . 11 2.1.3 Skid-Steer Vehicle Modification . . . . . . . . . . . . . . . . . . . . . 14 2.2 Dynamic Bicycle Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14 2.2.1 List of Parameters and Acceleration/Force Diagram . . . . . . . . . . 15 2.2.2 Force and Acceleration Analysis on Tractor and Trailer . . . . . . . . 17 2.2.3 Relative Forces and Accelerations . . . . . . . . . . . . . . . . . . . . 19 2.2.4 Tire Model Derivation . . . . . . . . . . . . . . . . . . . . . . . . . . 20 2.2.5 Equations Collected & Translation to Global Coordinates . . . . . . . 23 2.3 Discussion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24 2.4 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25 3 Applying MPC to a Tractor Trailer System . . . . . . . . . . . . . . . . . . . . 26 3.1 Set Point Calculations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26 iii 3.1.1 Initial Setup . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27 3.1.2 Finding Minimum Cost Using Newton?s Method . . . . . . . . . . . . 32 3.1.3 Finding Minimum Cost using Golden Section Search . . . . . . . . . 34 3.2 Control Calculations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38 3.2.1 Steering Motor Voltage Calculations for Front-Steering Tractor . . . . 38 3.3 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43 4 Computer Simulations and Live Application . . . . . . . . . . . . . . . . . . . . 45 4.1 Kubota RTV Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . 45 4.1.1 Kinematic Controller Model against Bicycle Plant Model . . . . . . . 45 4.1.2 Linearized Kinematic Controller Model against Nonlinear Kinematic Plant Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 48 4.2 Segway Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51 4.3 Segway Experimental Results . . . . . . . . . . . . . . . . . . . . . . . . . . 55 4.3.1 Controller Tuning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59 4.3.2 Turn Rate Analysis . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64 4.3.3 Applying the Scale Factor to the Input . . . . . . . . . . . . . . . . . 66 4.4 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 69 5 Summary and Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70 A Model Predictive Control - A General Case . . . . . . . . . . . . . . . . . . . . . 72 B Bicycle Model State Equations, Separated by State Variable . . . . . . . . . . . 75 Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79 iv List of Figures 1.1 Hitch Angle vs Time . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4 1.2 Path Following Using Linear State Feedback [9] . . . . . . . . . . . . . . . . 5 1.3 Path Following Using LQG [10] . . . . . . . . . . . . . . . . . . . . . . . . . 5 1.4 Path Following Using Hybrid Backstepping [11] . . . . . . . . . . . . . . . . 6 1.5 A Model Predictive Control Flowchart [8] . . . . . . . . . . . . . . . . . . . . 7 2.1 Schematic for a Tractor-Trailer System Kinematic Model . . . . . . . . . . . 10 2.2 Steering to Angular Velocity . . . . . . . . . . . . . . . . . . . . . . . . . . . 11 2.3 Tractor and Trailer Hitch Velocities . . . . . . . . . . . . . . . . . . . . . . . 12 2.4 Schematic for a Tractor-Trailer System Dynamic Model . . . . . . . . . . . . 16 2.5 Tractor Accelerations and Forces . . . . . . . . . . . . . . . . . . . . . . . . 17 2.6 Trailer Accelerations and Forces . . . . . . . . . . . . . . . . . . . . . . . . . 18 2.7 Tractor & Trailer Hitch Accelerations . . . . . . . . . . . . . . . . . . . . . . 19 2.8 Hitch Forces . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20 2.9 Tractor Motion from Tire Coordinate Frame . . . . . . . . . . . . . . . . . . 21 2.10 Dynamic Model vs Kinematic Model . . . . . . . . . . . . . . . . . . . . . . 24 2.11 Position Di?erences between the Kinematic and Dynamic Models . . . . . . 25 3.1 Simulation Path . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27 3.2 Set Point Calculations Flowchart . . . . . . . . . . . . . . . . . . . . . . . . 28 3.3 Test Points Relative to Desired Path . . . . . . . . . . . . . . . . . . . . . . 29 3.4 Newton?s Method Demonstration . . . . . . . . . . . . . . . . . . . . . . . . 33 v 3.5 MPC Set Point Simulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34 3.6 Newton?s Method Incompatibility . . . . . . . . . . . . . . . . . . . . . . . . 35 3.7 Golden Section Search . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37 3.8 Steering Angle Set-Points . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38 3.9 Armature Motor Model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39 3.10 Controlled Motor System . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39 3.11 Unwanted Oscillations . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40 3.12 Proportional Control . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41 3.13 Proportional-Integral Control . . . . . . . . . . . . . . . . . . . . . . . . . . 41 3.14 Steer Angle Controlled . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42 3.15 Voltage Across Motor vs Time (using PI control) . . . . . . . . . . . . . . . 42 3.16 Motor Response to Step Input with Varying Derivative Control Values . . . 43 3.17 Voltage Across Motor vs Time (using PID control) . . . . . . . . . . . . . . 44 4.1 MPC with Model Error . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 46 4.2 Deviation with High Model Error . . . . . . . . . . . . . . . . . . . . . . . . 46 4.3 Deviation with Low Model Error . . . . . . . . . . . . . . . . . . . . . . . . 47 4.4 Wide S-curve Path Following . . . . . . . . . . . . . . . . . . . . . . . . . . 48 4.5 Path Following Using Nonlinear Control Model . . . . . . . . . . . . . . . . 49 4.6 Path Following Using Linear Control Model . . . . . . . . . . . . . . . . . . 49 4.7 15m Radius S-curve . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 50 4.8 Path Following Using Nonlinear Control Model (15m radius) . . . . . . . . . 50 4.9 Path Following Using Linear Control Model (15m radius) . . . . . . . . . . . 51 4.10 Segway Simulator [18] . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52 4.11 Segway & Trailer Positions, Simulated . . . . . . . . . . . . . . . . . . . . . 53 4.12 Path Following Error, Simulated . . . . . . . . . . . . . . . . . . . . . . . . . 54 vi 4.13 Set Points, Simulated . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54 4.14 Segway Test Run . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55 4.15 Segway & Trailer Positions (first run) . . . . . . . . . . . . . . . . . . . . . . 56 4.16 Path Following Error (first run at 1 m/s) . . . . . . . . . . . . . . . . . . . . 57 4.17 Segway & Trailer Positions (third run at 0.5 m/s) . . . . . . . . . . . . . . . 57 4.18 Path Following Error (third run at 0.5 m/s) . . . . . . . . . . . . . . . . . . 58 4.19 Set Points (third run at 0.5 m/s) . . . . . . . . . . . . . . . . . . . . . . . . 58 4.20 Results with 4m prediction horizon, weighted . . . . . . . . . . . . . . . . . 59 4.21 Results with 4m prediction horizon, unweighted . . . . . . . . . . . . . . . . 60 4.22 Results with 5m prediction horizon, weighted . . . . . . . . . . . . . . . . . 60 4.23 Results with 5m prediction horizon, unweighted . . . . . . . . . . . . . . . . 61 4.24 Results with 5m prediction horizon, weighted inversely . . . . . . . . . . . . 62 4.25 Results with 4m prediction horizon, weighted inversely . . . . . . . . . . . . 62 4.26 Results with 5m prediction horizon, weighted inversely, one-to-one . . . . . . 63 4.27 Results with 4m prediction horizon, weighted inversely, one-to-one . . . . . . 63 4.28 Turn Rates at 0.5 m/s . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64 4.29 Turn Rates at 1.0 m/s . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65 4.30 Turn Rate Step Input Response . . . . . . . . . . . . . . . . . . . . . . . . . 65 4.31 Path Following at 0.5 m/s . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66 4.32 Path Following at 1.0 m/s . . . . . . . . . . . . . . . . . . . . . . . . . . . . 66 4.33 Path Following at 1.2 m/s . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67 4.34 Path Following at 1.4 m/s . . . . . . . . . . . . . . . . . . . . . . . . . . . . 67 4.35 Path Following at 1.6 m/s . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68 4.36 Path Following at 1.8 m/s . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68 4.37 Path Following at 2.0 m/s . . . . . . . . . . . . . . . . . . . . . . . . . . . . 68 vii List of Tables 2.1 List of Tractor-Trailer System Parameters (kinematic model) . . . . . . . . . 11 2.2 List of Tractor-Trailer System Parameters (dynamic model) . . . . . . . . . 15 4.1 List of Segway & Trailer Simulation Parameters (kinematic model) . . . . . 52 4.2 List of Segway & Trailer Live Parameters (kinematic model) . . . . . . . . . 55 4.3 Scale Factors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59 4.4 Inverted Scale Factors . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61 4.5 Inverted Scale Factors, One-to-One . . . . . . . . . . . . . . . . . . . . . . . 63 viii List of Symbols Angle Di?erence between Tractor and Trailer ( 1 2 ) Front Tire Steering Angle of Tractor ?x 1 Forward Velocity of Tractor ?x 2 , ?x 2 Forward Velocity and Acceleration of Trailer ?y 1 , ?y 1 Lateral Velocity and Acceleration of Tractor ?y 2 , ?y 2 Lateral Velocity and Acceleration of Trailer 1 , r 1 , ?r 1 Heading, Angular Velocity, and Angular Acceleration of Tractor 2 , r 2 , ?r 2 Heading, Angular Velocity, and Angular Acceleration of Trailer ? path j Angle between Trailer?s current heading and closest path point (when finding cost) ? pred j Angle between Trailer?s current heading and j-th Prediction Point (when finding cost) a Length from Tractor GPS (kinematic model) or CG (dynamic model) to front tire b Length from Tractor GPS (kinematic model) or CG (dynamic model) to back tire c Length from Tractor GPS (kinematic model) or CG (dynamic model) to hitch C 1 Cornering Coe?cient of the Front Tractor Tire C 2 Cornering Coe?cient of the Back Tractor Tire C 3 Cornering Coe?cient of the Trailer Tire d Length from Trailer GPS (kinematic model) or CG (dynamic model) to hitch ix e Length from Trailer GPS (kinematic model) or CG (dynamic model) to trailer tire H 0 x , H 0 y Forward and Lateral Hitch Forces (trailer side) H x , H y Forward and Lateral Hitch Forces (tractor side) I 1 Yaw Inertia of the Tractor I 2 Yaw Inertia of the Trailer J 0 i Derivative of the i-th Cost J i Cost of the i-th prediction l j Shortest Length between j-th Prediction Point and Path (when finding cost) m 1 Mass of the Tractor m 2 Mass of the Trailer w j Weight assigned to the j-th Prediction Point (when finding cost) X 1 , Y 1 Absolute X and Y Positions of Tractor X 2 , Y 2 Absolute X and Y Positions of Trailer X 1,ref , Y 1,ref Absolute X and Y Positions of Tractor (kinematic model) X 2,ref , Y 2,ref Absolute X and Y Positions of Trailer (kinematic model) X path j Y path j Closest Point on the Path to j-th Prediction Point (when finding cost) X trailer j Y trailer j j-th Predicted X and Y Positions of Trailer (when finding cost) x Abstract This thesis presents an e?ort to improve the path following reliability of a tractor-trailer system by using a non-linear Model Predictive Control (MPC) approach. The proposed method allows an autonomous mobile robot to make informed control decisions based on anticipating changes in the path conditions, rather than reacting to them, which could potentially reduce path following error on turns. Using a non-linear tractor-trailer model, the controller takes the tractor?s measured position and heading, as well as information about the path geometry in front of it, and it determines the optimal steer angle. Then, in the case of an Ackerman-steered vehicle, a secondary algorithm takes the desired steer angle and calculates the amount of voltage to apply to the steering wheel motor to achieve the steer angle. In comparison, a di?erential- steered, or skid-steered vehicle takes the set point (given as a turn rate in radians per second) and computes the voltages to the traction motors internally. In the MATLAB simulation study, the controller algorithm is capable of guiding a 2- 1/2 meter long trailer around a 5-meter radius turn, when towed by a four wheel drive o?-road utility vehicle, with a maximum error of 8.5 centimeters. These results are highly idealized, however. Adding sensor noise and process noise in simulation increases the error, and inherent sensor bias and latency during the live run increases the error substantially. From the experimental results, it is concluded that non-linear MPC has the potential to improve the reliability of the path following of a robot and trailer system. In order to fully reap the benefits of non-linear MPC however, the model has to be accurate, and the computer has to be fast enough to compute predictions from the model in real-time. 1 Chapter 1 Introduction Model Predictive Control (MPC) is a relatively new control method, first practiced by Shell Oil, which determines an optimal input, called a set point, and then guides the system to that set point. It is a control technique that is commonly used in the oil industry, as well as other applications in chemical engineering. With advancements in computing power, en- gineers have begun exploring the usage of MPC to other applications, including autonomous vehicle control. However, due to the computational expense of MPC, especially when a nonlinear model is used, most vehicular applications are limited to low-speed operation [1]. For many o?-road vehicle applications however, low-speed operation is the norm. In agriculture, tractor-trailer systems are used to spread seed, fertilizer and top soil. In the case of the Department of Defense, a tractor pulls a fiberglass trailer that carries metal sensors to scan the ground for unexploded ordnance, which is further explained in [2]. Despite the computational expense of MPC, these applications involve process that are slow enough for MPC to produce good results. 1.1 Background Auburn University, with the support of the U.S. Army Corps of Engineers, has been researching ways to control o?-road tractor-trailer systems for conducting geophysical sur- veys, with the trailer being the object of interest for control. A geophysical survey involves ascanofagivenareainsearchofmetalobjectsintheground[9]. In an e?ort to streamline the U.S. Military, the Department of Defense (DOD) has initiated numerous military base closures around the United States. Many of these bases contained training sites for tactical strategies that are less commonly used in modern military 2 operations. According to [3], the DOD closed or realigned over 800 defense locations and relocated over 125,000 personnel since 2005, in an operation called Base Realignment and Closure (BRAC). When a base closes, the land is sold to private developers. However, there are legitimate safety concerns regarding unexploded ordnance (UXO) still remaining on plots of land that used to be training sites. To ensure that a questionable field is safe, the Army Corps of Engineers conducts geo- physical surveys to look for potential UXO. While there have been no recorded incidents of injuries or fatalities during these surveys due to accidental discharge, the risk is still a con- cern. This is why the Army Corps of Engineers has tasked Auburn University in developing an unmanned robot vehicle that would carry out the task of searching for UXO. Auburn has since used a setup consisting of a GPS guided trailer made of fiberglass, towed by a tractor [5], which would allow the scanning of the ground for metal objects without interference from the metal inside the tractor. 1.2 Previous Work at Auburn University Previously, linear state feedback controllers [9], LQG controllers [10], and hybrid back- stepping controllers [11] were used in the guidance of the surveying system. In all cases, these control methods work very well in guiding the trailer to the path, as long as the path is straight. However, due to the complexity of tractor-trailer system dynamics, keeping the trailer on the path becomes much more di?cult when going around turns. The most accu- rate controller found so far was the linear state feedback controller, which had a maximum recovery error of 20.4cm [9]. Conversely, the hybrid backstepping controller had maximum errors of up to 40cm [11], and the LQG controller had maximum errors of around 44cm [10]. Both authors conclude, however, that these errors might be improved by fine-tuning the gains. This thesis proposes, on the other hand, that the biggest source of error is the non- linearity of a tractor-trailer hitch connection. According to [14], when the angle between 3 Figure 1.1: Hitch Angle vs Time the tractor and the trailer exceeds 7?,linearizedtractor-trailermodelsbecomelessaccurate. Figure 1.1 shows a time plot of the hitch angle, where the maximum hitch angle reaches 47.8? as a simulated tractor-trailer system navigates an S-curve with turn radii of 5 meters. The biggest issue during a geophysical survey is recovering out of a turn. As seen in Figure 1.2, the robot overshoots the beginning of the straight sections of the desired path, and then it corrects itself. In the case of Figure 1.3, the robot overcompensates and undergoes an oscillatory correction maneuver to return to the desired path. Finally, in Figure 1.4, the robot does a combination of overshooting the straight section reentries, as well as overcompensating for errors. With these errors, the ground scanners on the trailer could potentially miss locations on the ground that contain critical information. In each of the afore-mentioned works, the robot follows what is known as a ?Dubins path.? According to Lester E. Dubins, the shortest path between two oriented points consists of straight lines and turns of constant radii [4]. However, dynamic constraints make it infeasible for a non-holonomic trailer to follow a path perfectly where the curvature changes instantaneously. One way to address this problem is to design paths with clothoid turns, 4 Figure 1.2: Path Following Using Linear State Feedback [9] Figure 1.3: Path Following Using LQG [10] 5 Figure 1.4: Path Following Using Hybrid Backstepping [11] as studied in [6]. A clothoid is an arc where the curvature changes linearly, as opposed to instantaneously. They are commonly used in highway and railway design, and several clothoid computation algorithms exist, such as the one introduced in [7]. For each of the experiments done in this thesis, however, a constant radius turn will be used. This will force an error into the system intentionally, and it would allow the controller?s recovery performance to be measured. 1.3 Proposing Model Predictive Control as a Potential Solution The goal of this research is to develop a control algorithm that will improve the guidance of a trailer to a desired path during and after making a turn. Unlike the controllers previ- ously used at Auburn University which choose guidance inputs based on the trailer?s current position relative to the path, this thesis proposes designing a model predictive controller that chooses guidance inputs based on where the trailer is predicted to go, based upon a model. This is similar to the way a human drives a car, as a human subconsciously predicts where 6 Figure 1.5: A Model Predictive Control Flowchart [8] his or her car is going to go given a forward and steering input, and then the human adjusts the forward and steering controls of the car accordingly. Of course, it is fair to ask the question of whether or not a computer can make model calculations fast enough to determine the optimal control inputs. After all, if a computer takes too long to predict an optimal solution, the control system becomes useless. Also, if the model is oversimplified, a model predictive controller?s performance would be no better than a classical controller. For this application, however, the robot moves at a forward speed of 1 meter per second. Because of the low speed of the system, inertial properties as well as tire slip properties of the tractor and trailer will be ignored, and a simplified kinematic tractor-trailer model will be used, making the nonlinear MPC calculations feasible. In general, MPC follows an algorithm depicted by the flowchart in Figure 1.5. First, the controller makes a prediction, choosing a particular steer angle. Then, using a model of the system, the controller looks at where the trailer will go if the robot uses that steer angle. It will then compare the result to the desired path. If the result does not meet tolerance specifications, it will make another prediction. Otherwise, the controller will use this steer angle (now called a set-point) as a target angle to move the steering wheel to. The controller then determines the voltage into the steering motor by making control calculations, and it feeds that voltage into the motor. The controller also runs the input back through the model and compares the result from the process?s feedback sensors (in this case, a GPS receiver). 7 1.4 Outline This thesis introduces a numerical approach to a non-linear feedforward control problem, and it stresses real-time feasibility. It is necessary for the control system to determine inputs quickly, while maintaining a high degree of accuracy. Two tractor-trailer models are presented in Chapter 2. The first, a 4th-order kinematic model, is a standard control model that is currently being used at Auburn University. The second model, a 9th-order bicycle model, has not been used at Auburn yet, due to its complexity and di?culty for a computer to numerically compute in real-time. Chapter 3 presents the numerical MPC method responsible for choosing the appropriate set points, and it shows how these set points can be achieved through classical control method design. Chapter 4 includes results from computer simulations with the two competing tractor-trailer models described in Chapter 2, as well as results from an experimental run on a Segway RMP 400. Finally, Chapter 5 presents conclusions from the results of this work and proposes future work. 8 Chapter 2 Kinematic and Dynamic Modeling of the System This chapter presents the derivation of the aforementioned tractor-trailer models. First, akinematicmodelisderivedforafront-steeringtractor,aswellasaskid-steeringtractor, towing a trailer. Then, a dynamic bicycle model for the same system is derived using Newtonian mechanics. The chapter then closes with a discussion on why the kinematic model is preferable for MPC with present technology, but also how the dynamic model would be preferable in other scenarios. 2.1 Kinematic Model The kinematic model is a basic, ?no-frills? model that can satisfy the prediction require- ment for Model Predictive Control in low-speed applications. The primary advantage to using a kinematic model is its simplicity. A computer could numerically integrate a kine- matic model accurately, using a much larger time step than when numerically integrating a dynamic model. The disadvantage of a kinematic model is that it assumes (1) the ground is perfectly flat, (2) the tires have perfect traction with the ground (no slip), and (3) it ig- nores inertia and lateral forces. Due to the low-speed operation during a geophysical survey, making these assumptions do not impact the accuracy of the predictions significantly. 2.1.1 List of Parameters & Velocity Diagram Table 2.1 describes the variables used for a Kubota RTV, while Figure 2.1 shows the connected tractor-trailer system. 9 X" Y" ? 2" ? 1" r 1 r c" a" b" d e" ?" &x 1 ?" Figure 2.1: Schematic for a Tractor-Trailer System Kinematic Model 10 Parameter Description Value (if constant) X 1,ref Absolute X Position of Tractor variable Y 1,ref Absolute Y Position of Tractor variable X 2,ref Absolute X Position of Trailer variable Y 2,ref Absolute Y Position of Trailer variable X 1 Absolute X Position of Tractor (back axle) variable Y 1 Absolute Y Position of Tractor (back axle) variable ?x 1 Forward Velocity of the Tractor 1m/s 1 ,r 1 Heading and Angular Velocity of Tractor variable 2 ,r 2 Heading and Angular Velocity of Trailer variable Front Tire Steering Angle of Tractor variable Angle Di?erence between Tractor and Trailer ( 1 2 ) 1 2 a Length from Tractor GPS receiver to center of front tire 0.75 m b Length from Tractor GPS receiver to center of back tire 1.21 m c Length from Tractor GPS receiver to hitch 1.74 m d Length from Trailer GPS receiver to hitch 3.0 m e Length from Trailer GPS receiver to trailer tire 1.0 m Table 2.1: List of Tractor-Trailer System Parameters (kinematic model) Figure 2.2: Steering to Angular Velocity 2.1.2 Kinematic Model Derivation Considering the tractor coordinates first, it is assumed that there is no lateral velocity at the back axle of the tractor. Using this assumption, the tractor?s angular velocity can then be related to its forward velocity and the steering angle of the front tire as shown in Figure 2.2, which leads to Equation (2.1). r 1 = ?x 1 tan (a + b) (2.1) 11 r 2 d e# +e () r 2 &x 2 r 1 c# a# b# c?b () r 1 &x 1 X 2# X 1# Y 2# Y 1# Figure 2.3: Tractor and Trailer Hitch Velocities 12 Next, by studying Figure 2.3, the forward and lateral velocities at the hitch are found relative to both the tractor and the trailer. Velocity of the hitch in tractor coordinates: ?x 1 ? i 1 (cb)r 1 ? j 1 Velocity of the hitch in trailer coordinates: ?x 2 ? i 2 +(d + e)r 2 ? j 2 These two velocity expressions both describe the same velocity, and therefore, they can be related to each other using a rotation matrix: 2 6 4 ?x 2 (d + e)r 2 3 7 5 = 2 6 4 cos sin sin cos 3 7 5 2 6 4 ?x 1 (cb)r 1 3 7 5 (2.2) An equation for the angular velocity of the trailer can be extracted from the above relation as shown in Equation (2.3): r 2 = ?x 1 (d + e) sin (cb)r 1 (d + e) cos (2.3) While an equation for the forward velocity of the tractor could be extracted from the matrix rotation, it is unnecessary to do so. Instead, by finding the tractor?s global position and considering the headings of both the tractor and the trailer, the trailer?s global position can be found as well. This eliminates several integration steps, and it allows for a larger numerical integration time step size. Equations (2.4) and (2.5) show the translation to global coordinates for the tractor. ? X 1 =?x 1 cos 1 (2.4) ? Y 1 =?y 1 sin 1 (2.5) 13 To summarize the derivation so far, Equations (2.1), (2.4), and (2.5) give the position and heading of the back axle of the tractor, while Equation (2.3) gives the angular velocity of the trailer. Now, some kinematic relations are considered to locate the points of interest (i.e. the GPS receivers) on both the tractor and the trailer. Equations (2.6) and (2.7) give the antenna location on the tractor: X 1,ref = X 1 + bcos 1 (2.6) Y 1,ref = Y 1 + bsin 1 (2.7) Finally, the trailer?s GPS receiver location can be calculated using the following Equa- tions (2.8) and (2.9): X 2,ref = X 1,ref ccos 1 dcos 2 (2.8) Y 2,ref = Y 1,ref csin 1 dsin 2 (2.9) 2.1.3 Skid-Steer Vehicle Modification Many skid-steer vehicles, such as the Segway Robotics Mobility Platform (RMP) 400, have an existing steering control system that only requires an angular velocity input, re- moving the need for Equation (2.1). However, there is often a time delay between the angular velocity input, and when the robot reaches that angular velocity. The design of a compensator for this delay will be addressed in Chapter 3. 2.2 Dynamic Bicycle Model The dynamic bicycle model introduces the possibility of adding exterior input forces to the model, such as bumps and dips in the ground. The major di?erence in the model 14 Parameter Description Value X 1 Absolute X Position of Tractor (center of gravity) variable Y 1 Absolute Y Position of Tractor (center of gravity) variable X 2 Absolute X Position of Trailer (center of gravity) variable Y 2 Absolute Y Position of Trailer (center of gravity) variable ?x 1 Forward Velocity of Tractor 1m/s ?y 1, ?y 1 Lateral Velocity and Acceleration of Tractor variable ?x 2 , ?x 2 Forward Velocity and Acceleration of Trailer variable ?y 2 , ?y 2 Lateral Velocity and Acceleration of the Trailer variable 1 ,r 1 , ?r 1 Heading, Angular Velocity, and Angular Acceleration of Tractor variable 2 ,r 2 , ?r 2 Heading, Angular Velocity, and Angular Acceleration of Trailer variable H x ,H y Forward and Lateral Hitch Forces (tractor side) variable H 0 x ,H 0 y Forward and Lateral Hitch Forces (trailer side) variable Front Tire Steering Angle of Tractor variable Angle Di?erence between Tractor and Trailer ( 1 2 ) 1 2 a Length from Tractor CG to center of front tire 0.75 m b Length from Tractor CG to center of back tire 1.21 m c Length from Tractor CG to hitch 1.74 m d Length from Trailer CG to hitch 3.0 m e Length from Trailer CG to trailer tire 1.0 m m 1 Mass of the Tractor 1225 kg I 1 Yaw Inertia of the Tractor m 1 ? a ? b m 2 Mass of the Trailer 30 kg I 2 Yaw Inertia of the Trailer m 2 ? d ? e C 1 Cornering Coe?cient of the Front Tractor Tire 80 ? m 1 C 2 Cornering Coe?cient of the Back Tractor Tire 80 ? m 1 C 3 Cornering Coe?cient of the Trailer Tire 80 ? m 2 Table 2.2: List of Tractor-Trailer System Parameters (dynamic model) assumptions is that the tires no longer have perfect traction with the ground, and they have to push against the ground in order to control a vehicle?s direction. These advantages come at a cost, however, as the numerical computation requires a significantly smaller time step size than the kinematic model. 2.2.1 List of Parameters and Acceleration/Force Diagram Table 2.2 describes the variables used for a Kubota RTV, while Figure 2.4 shows the connected tractor-trailer system. 15 X" Y" ? 2" ? 1" r 1 ,& r,& c" a" b" d e" ?" &y 1 , &y 2 , F y1 F y2 F y3 &x 1 ?" m 2 ,I m 1 ,I Figure 2.4: Schematic for a Tractor-Trailer System Dynamic Model 16 X" Y" X 1" Y 1" ? 1" &r 1 c" a" b" &y 1 +xr F y1 F y2 m,I ?" H y &y 1 r, x Figure 2.5: Tractor Accelerations and Forces 2.2.2 Force and Acceleration Analysis on Tractor and Trailer As seen in Figure 2.5, the tractor has three lateral forces acting on it: two produced by the tires, and a lateral force produced at the hitch. There are two lateral acceleration components influenced by these forces ? a lateral perturbed acceleration ?y 1 ,andacentripetal acceleration ?x 1 r 1 . These forces and accelerations relate as described by Equations (2.10) and (2.11), using Newtonian mechanics. m 1 ?y 1 + m 1 ?x 1 r 1 = F y1 + F y2 H y (2.10) I 1 ?r 1 = aF y1 bF y2 + cH y (2.11) 17 X" Y" X 2" Y 2" ? 2" r,& d e" &y 2 +xr F 3 m 2 ,I !H y &x 2 ,! &y 2 r Figure 2.6: Trailer Accelerations and Forces Next, the trailer?s accelerations and forces are examined as seen in Figure 2.6. In similar fashion to the tractor equations, Equations (2.12) and (2.13) describe the lateral motion of the trailer. m 2 ?y 2 + m 2 ?x 2 r 2 = F y3 + H 0 y (2.12) I 2 ?r 2 = dH 0 y eF y3 (2.13) Unlike the tractor, which was said to have a constant forward velocity (and hence no forward acceleration), the trailer does have a forward acceleration component. Equation (2.14) describes this motion: 18 r 1 ,& c" a" b" &y 1 +xr?c& 1 &y 1 r?c 2 r 2 ,& d e" & 2 +x & 2 yrd 2 Figure 2.7: Tractor & Trailer Hitch Accelerations m 2 ?x 2 m 2 ?y 2 r 2 = H 0 x (2.14) 2.2.3 Relative Forces and Accelerations To relate the dynamic tractor and trailer models, similarly to the kinematic model, the accelerations at the hitch can be determined with respect to both the tractor and the trailer separately (see Figure 2.7), so they can be related using a rotation matrix (Equation (2.15)). 2 6 4 ?x 2 (?y 2 r 2 + dr 2 2 ) ?y 2 +?x 2 r 2 + d?r 2 3 7 5 = 2 6 4 cos sin sin cos 3 7 5 2 6 4 (?y 1 r 1 cr 2 1 ) ?y 1 +?x 1 r 1 c?r 1 3 7 5 (2.15) Using the same rotation matrix as Equation (2.15), Equation (2.16) relates the forces. Note that by Newton?s Third Law, the force acting on the hitch from the perspective of the 19 c" a" b" H y x d e" !H y ! x Figure 2.8: Hitch Forces tractor has to be equal and opposite to the force acting on the hitch from the perspective of the trailer, as shown in Figure 2.8. 2 6 4 H 0 x H 0 y 3 7 5 = 2 6 4 cos sin sin cos 3 7 5 2 6 4 H x H y 3 7 5 (2.16) 2.2.4 Tire Model Derivation Historically, tires have been di?cult for engineers to model accurately. In most cases, tire models are derived experimentally, as opposed to theoretically. For this model, however, a somewhat non-intuitive approach called the ?slip angle? model is used. The slip angle concept does not imply skidding, but instead implies that as tires roll and experience a lateral force, some portions of the tire begin to slip from their point of contact with the 20 ? 1" r 1 c# a# b# &y 1,tire &x 1,tire m 1 ,I ?" H y & 1 ?" X 1# X 1,(re# Y 1# Y 1,(re# Figure 2.9: Tractor Motion from Tire Coordinate Frame ground, while other portions of the tire are still essentially ?locked? to the ground by static friction. For a more comprehensive description of the slip angle concept, refer to [12]. When a vehicle is traveling straight, there is virtually no di?erence between a vehi- cle?s velocity and a tire?s velocity. However, if a perturbation is introduced in the vehicle?s movement, the tire will react based on its motion relative to the vehicle. Figure 2.9 shows an exaggeration of a vehicle?s perturbed motion from the coordinate frame of the tire. For the following calculations, ?x will be the tire?s in-line rolling velocity, ?y will be the tire?s lateral velocity, and will be the angle di?erence between the vehicle?s forward motion and its lateral motion. From Figure 2.9, the motion of the rear tractor tire can be related to the motion of the vehicle as shown in Equations (2.17), (2.18), and (2.19). 21 1 =arctan ? ?y 1 ?x 1 ? (2.17) ?x =?x 1 cos 1 +?y 1 sin 1 (2.18) ?y = ?x 1 sin 1 +?y 1 cos 1 br 1 (2.19) The force produced by the tire is the arctangent of the ratio between the lateral tire velocity ?y to its rolling velocity ?x,multipliedbythenegativeofthecorneringcoe?cient (Equation (2.20)). F y2 = C 2 arctan ? ?x 1 sin 1 +?y 1 cos 1 br 1 ?x 1 cos 1 +?y 1 sin 1 ? (2.20) Similarly, without any steering angle, the front tire force would be modeled with Equa- tion (2.21). F y1 = C 1 arctan ? ?x 1 sin 1 +?y 1 cos 1 + ar 1 ?x 1 cos 1 +?y 1 sin 1 ? (2.21) Conversely, if a steering angle is included, the front tire force would be modeled with Equation (2.22). F y1 = C 1 arctan ? ?x 1 sin( 1 + )+?y 1 cos( 1 + )+ar 1 cos ?x 1 cos( 1 + )+?y 1 sin( 1 + )+ar 1 sin ? cos (2.22) Finally, in similar fashion to Equation (2.20), the trailer tire force is modeled by Equa- tion (2.23). F y3 = C 3 arctan ? ?x 2 sin 2 +?y 2 cos 1 er 2 ?x 2 cos 2 +?y 2 sin 2 ? (2.23) 22 2.2.5 Equations Collected & Translation to Global Coordinates Now that the tractor trailer model equations of motion have been derived, Equations (2.10) through (2.16) can be combined to make Equations (2.24) through (2.28). The indi- vidual state variables can be isolated by applying Cramer?s Method. m 1 ?y 1 +m 2 ?y 2 cosm 2 ?x 2 sin = F y1 m 1 ?x 1 r 1 +F y2 (m 2 ?x 2 r 2 F y3 )cosm 2 ?y 2 r 2 sin (2.24) I 1 ?r 1 cm 2 ?y 2 cos+ cm 2 ?x 2 sin = aF y1 bF y2 + c(m 2 ?x 2 r 2 F y3 )cos+cm 2 ?y 2 r 2 sin (2.25) dm 2 ?y 2 I 2 ?r 2 =(d + e)F y3 dm 2 ?x 2 r 2 (2.26) ?y 1 sinc?r 1 sin+ ?x 2 = ?x 1 r 1 sin ?y 1 r 1 cr 2 1 cos+ ?y 2 r 2 + dr 2 2 (2.27) ?y 1 cosc?r 1 cos ?y 2 d?r 2 = ?x 1 r 1 cos+ ?y 1 r 1 cr 2 1 sin+ ?x 2 r 2 (2.28) Finally, to translate each vehicle component from local to global coordinates, the fol- lowing rotation is used: 2 6 4 ? X i ? Y i 3 7 5 = 2 6 4 cos? i sin? i sin? i cos? i 3 7 5 2 6 4 ?x i ?y i 3 7 5 (2.29) where i =1for the tractor, and i =2for the trailer. 23 Figure 2.10: Dynamic Model vs Kinematic Model 2.3 Discussion Both the kinematic and dynamic models were tested using MATLAB. Using a 4th- order Runge Kutta approximation, the kinematic model can produce an accurate estimate (<0.01% error) using a time step size of 0.025 seconds. On the other hand, the dynamic model requires a step size of 0.0001 seconds using a 6th-order Runge Kutta approximation to achieve an accurate estimate. Depending on the speed of the computer, the small step size required for the dynamic model could hinder the viability of using the model with MPC. Fortunately, both models allow for a test of MPC when there is model error. Figure 2.10 shows these two models being run using a starting point where the CG of the tractor is at the origin, a forward velocity of 1m/s, and a common set of steering instructions that would guide the tractor around an S-curve. These models produce similar results, but they are by no means the same. Figure 2.11 shows how the models deviate as they go into the first turn, and after 6 seconds of a hard turn to the left, the models deviate by about 0.25 meters. 24 Figure 2.11: Position Di?erences between the Kinematic and Dynamic Models 2.4 Conclusion This chapter presented two models, a kinematic model and a dynamic model, which could be used in the design of an MPC controller. The kinematic model takes significantly less computation e?ort, but the dynamic model is more realistic in that it considers the momentum of the vehicles, and it acknowledges that the tires have slip. The slight di?erence in the outputs of these two models allows for the testing of an MPC controller?s ability to compensate for model error before physically implementing the control system on a robot. 25 Chapter 3 Applying MPC to a Tractor Trailer System This chapter describes the Model Predictive Control (MPC) algorithm used to control the tractor-trailer system. First, a front-wheel steering tractor model is considered, followed by a skid-steering vehicle. MPC follows two key steps: set point calculations, and control calculations. To test these control methods, an S-curve path was designed with two 5-meter radius turnarounds as shown in Figure 3.1. 3.1 Set Point Calculations When learning to drive a motor vehicle, a human driver gradually gains some idea of how the vehicle will respond to a certain steering input. When preparing to make a turn at atra?clight(particularlyleftturns),thedrivinginstructortellsthenewdrivertovisualize an arc from the vehicle?s current position to where the vehicle will be after making the turn. When the light turns green, the driver follows the arc, anticipating the car?s response to a steering input by using preexisting knowledge of the vehicle?s response from previous turns. In the MPC algorithm, the same principle is applied using a model to predict the outcome, given a steering input. Figure 3.2 on page 28 illustrates the steps taken for the set point calculations. Subscript k indexes how many cycles the set-point calculator has run, while subscript i tracks the number of guesses the computer has made during its current prediction cycle. Every time the algorithm recycles, the current position and heading of both the tractor and trailer are accounted for, and an initial guess is made by using the previous tractor steer angle. The algorithm then uses the model to predict the path the trailer would follow over next 4 seconds. Next, using the desired path as a reference, a cost function is calculated based on how far 26 -10 -5 0 5 10 0 2 4 6 8 10 12 14 16 18 20 X-Position (m) Y- Po s i t i o n ( m ) S-curve with 5m turn radius Figure 3.1: Simulation Path the predicted path is from the desired path. If this is the first iteration of the prediction cycle, the steer angle is moved 1? in the direction of the path, and the process is repeated to calculate another cost value. On subsequent iterations, Newton?s Method is applied in an e?ort to bring the cost as close to zero as considered reasonable. Once a steering angle has been determined that minimizes cost, that steer angle is sent to the steering controller as a reference. 3.1.1 Initial Setup At the start of the prediction cycle, the algorithm receives position and heading informa- tion from a GPS receiver attached to the trailer (X k, Y k ,? k ). If the tractor is a front-steering vehicle, the algorithm also takes the current steer angle ( k ) from a wheel encoder attached to the steering wheel. On the other hand, if the tractor is a skid-steering vehicle, the al- gorithm notes the current angular velocity of the tractor. The algorithm also assumes a constant forward velocity (v)ofthetractor.Forthefirstprediction,thealgorithmusesthe 27 Figure 3.2: Set Point Calculations Flowchart 28 Figure 3.3: Test Points Relative to Desired Path current steering angle and the model to predict the path the trailer would take over the next 4secondsusingthatsamesteeringangle. Figure 3.3 shows an overhead view of this happening. The trailer is located on the left of the plot, the black line is the intended path, the blue circles indicate the trailer?s predicted locations ? one meter apart from each other, and the red lines indicate the distance between each prediction point and the path. Now, to find the optimal steer angle, a cost function is defined. Ordinarily, the MPC cost function takes the form shown in Equation (3.1). J = N X j=1 ? Q j (r j x j ) 2 + R j u 2 j ? (3.1) When using linear MPC, the analytical algorithm shown in Appendix A can be used to calculate the control input. In the case of a nonlinear approach, iterative methods have to 29 be applied. In many cases, nonlinear MPC is infeasible to implement in real time, but this research explores a way to reach an input decision quickly. The cost function is first redefined as the sum of the distances between predicted position estimates and the path: J i = 4 X j=1 l j (?1) j (3.2) where l j is the shortest distance between the j-th prediction point and the path. To ensure that this summation converges to zero, all test points to the right of the path are assigned a positive value, while the test points to the left are assigned a negative value. Notice that the cost function defined by Equation (3.2) is no longer quadratic, and instead it has been surrounded by absolute value brackets. Without the absolute value brackets, the summation could go negative if all prediction points were on the left side of the path, and a negative cost would imply the existence of a steer input better than optimal. The absolute value brackets simply state that the length summations that go negative are just as unfavorable as positive summations. To determine what the sign should be for each point, some trigonometry is used. First, the algorithm determines for each point, what angle the path makes with the trailer?s heading and the nearest point on the path: ? path j =arctan ? Y path j Y trailer k X path j X trailer k ? . The algorithm then determines the angle that the prediction point makes with the trailer?s current heading: ? pred j =arctan ? Y trailer j Y trailer k X trailer j X trailer k ? .Afterthat,itcomparesthesetwoangles,anditassignsa cost function value: J i = P 4 j=1 l j ? sign ? path j ? pred j . It is fair to ask a few questions at this point, starting with ?Why go to all this e?ort?? The reason is that this ensures (in most cases) that the interior of the cost function crosses zero on the y-axis of a cost versus steer angle plot. This sets things up nicely for using Newton?s Method to find the zero of the cost function quickly and e?ciently. It is important to point out that this cost function will not return the same solution as Equation (3.1), but 30 the horizon can be adjusted to mimic the solution of (3.1) by heavily weighing the later predictions. The follow-up question would then be, ?Why not just use the derivative form of Newton?s Method to minimize the cost function in Equation (3.1)?? The problem is that each iteration of the derivative form of Newton?s Method requires at least two cost evaluations to compute a derivative. This doubles the computational expense of the minimization process. If the robot?s processor is fast enough to compute these calculations in real time, using (3.1) might be feasible, but if the processor cannot keep up, unpredictable results would occur. There are two pitfalls to this redefined cost function, however. One being that there could potentially be more than one solution to the cost function on any given point on the path. First, there is the desired solution that keeps the system moving along the path toward the goal. Then, there may be another solution where the tractor is spinning at a dangerously high turn rate, but the kinematic model evaluation shows that the sum of the trailer?s position predictions still add up to zero. This is especially prevalent on a skid-steer robot, as the distance between the tractor?s center of rotation and the hitch is long, and a sharp turn causes the trailer to yaw in the other direction for a brief moment. On the other hand, a front-steer robot is less likely to have that problem as its center of rotation is the back axle, and the distance from the back axle to the hitch is relatively short compared to the rest of the vehicle. Another pitfall is that if the robot drifts too far away from the path, the robot could reach a position and heading where no steering input exists which would allow the cost function to reach zero. Although it is tempting to use the derivative form of Newton?s Method to find the minimum in these cases, simulator tests have shown that the derivative form of Newton?s Method could inadvertently converge onto the maximum of the cost function, not the minimum. The fallback method in both of these situations is Golden Section Search, which while ine?cient, guarantees that the solution is a minimum, and it limits the search range to a user-defined interval. 31 3.1.2 Finding Minimum Cost Using Newton?s Method Finding the minimum cost is a trial-and-error process, but a good initial guess is the same steer angle used for the previous prediction cycle. The algorithm uses this guess along with the model to predict where the tractor and trailer would go. Then, the second guess is 1? to the right or left of that angle (depending on which side of the initial guess the path is on). With two points available, the algorithm can now predict a third point using Equations (3.3) and (3.4). J 0 i = J i J i1 i i1 (3.3) i+1 = i J i J 0 i (3.4) Figure 3.4 on page 33 illustrates the Newton?s Method process. Each frame in Figure 3.4 depicts the results of a prediction. On the top subplot of each frame, the trailer GPS receiver is located on the far left-hand side of the plot. The black line is the path. The blue circles are prediction points based o? of the model, one second apart from each other. The red lines represent the distance from each point to the path. On the bottom subplot of each frame, the x-axis is the steering angle, while the y-axis is the cost. The blue line is the cost function that occurs under the current tractor-trailer state conditions, which was computed only for this demonstration, but is impractical to compute in real time. Finally, the red line tracks each prediction made using Newton?s Method. The cycle repeats itself until the change in steer angle is less than 0.5?.Asseenin the Figure, the prediction algorithm ran five times to arrive at the steering angle with a cost near zero. By reusing the previously calculated steering angle, however, the algorithm usually arrives at a usable steering angle after three predictions. Figure 3.5 on page 34 shows an overhead simulation of the system guiding to the path using this MPC approach, assuming the controller could guide to the set points instantaneously. 32 Figure 3.4: Newton?s Method Demonstration 33 Figure 3.5: MPC Set Point Simulation If the tractor vehicle is a skid-steer vehicle, such as the Segway RMP 400, the controller uses the same prediction method as before, but it chooses the necessary turn rate (instead of the steer angle) in order to keep the trailer on the path. 3.1.3 Finding Minimum Cost using Golden Section Search In some cases, the Newton?s Method approach will not work, as the cost function does not cross zero on the y-axis. This is especially likely to happen with tractor-trailer systems where the distance between the center of rotation on the tractor and the hitch is long. It can also happen when the path following error exceeds a certain point. Figure 3.6 shows an instance where using MPC on a Segway RMP 400 and trailer system, the cost function would not reach zero, no matter what steering input is used. To address this, the cost function is first squared to eliminate the possibility of it going negative, and the Golden Section Search method of optimization is used. Again, it is tempting to try the derivative form of Newton?s Method to locate the mini- mum, but in some cases, a local minimum and local maximum are very close together. This could result in Newton?s Method converging onto a local maximum. Once this happens, the 34 Figure 3.6: Newton?s Method Incompatibility robot continues to follow this method, maintaining its steering input on the local maximum of the cost function and loses the path completely. On the other hand, the Golden Section Search method guarantees a minimum, provided that a local minimum exists on the chosen interval.[16] The prediction points for the Golden Section search method are chosen the following way. First a prediction interval is chosen (in the case of the Segway, -40 deg/sec to 40 deg/sec). If l 0 defines the length of the full interval, and l 1 and l 2 define the space between the left and right edges of the interval and the intermediate prediction points, the locations of l 1 and l 2 can be determined by satisfying the following conditions: l 1 + l 2 = l 0 (3.5) l 1 l 0 = l 2 l 1 (3.6) By substituting Equation (3.5) into Equation (3.6), the following equation results: 35 l 1 l 1 + l 2 = l 2 l 1 (3.7) Next, the reciprocal is taken, and R is defined as l 2 l 1 , resulting in the following: 1+R = 1 R (3.8) By multiplying both sides by R, Equation (3.8) becomes R 2 +R1=0,andthepositive root, or the golden ratio can be solved for. The result is R = p 51 2 t 0.61803. To set up the algorithm, x l and x u define steer inputs at the lower and upper limits of the prediction interval. The intermediate points x 1 and x 2 are then chosen using the golden ratio: x 1 = x l + p 51 2 (x u x l ) (3.9) x 2 = x u p 51 2 (x u x l ) (3.10) The costs of these two intermediate points are then evaluated and a comparison is made. Figure 3.7 illustrates this process. In the cost function presented, J(x 2 )