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
)