METHODS FOR MINIMIZING NAVIGATION ERRORS INDUCED
BY GROUND VEHICLE DYNAMICS
Except where reference is made to the work of others, the work described in this thesis is
my own or was done in collaboration with my advisory committee. This thesis does not
include proprietary or classified information.
_______________________________
William E. Travis
Certificate of Approval:
_____________________________
George Flowers
Professor
Mechanical Engineering
_____________________________
David M. Bevly, Chair
Assistant Professor
Mechanical Engineering
_____________________________
John Y. Hung
Associate Professor
Electrical Engineering
_____________________________
Stephen L. McFarland
Dean
Graduate School
METHODS FOR MINIMIZING NAVIGATION ERRORS INDUCED
BY GROUND VEHICLE DYNAMICS
William E. Travis
A Thesis
Submitted to
the Graduate Faculty of
Auburn University
in Partial Fulfillment of the
Requirements for the
Degree of
Master of Science
Auburn, Alabama
May 11, 2006
iii
METHODS FOR MINIMIZING NAVIGATION ERRORS INDUCED
BY GROUND VEHICLE DYNAMICS
William E. Travis
Permission is granted to Auburn University to make copies of this thesis at its discretion,
upon request of individuals or institutions and at their expense. The author reserves all
publication rights.
__________________________________
Signature of Author
__________________________________
Date of Graduation
iv
THESIS ABSTRACT
METHODS FOR MINIMIZING NAVIGATION ERRORS INDUCED
BY GROUND VEHICLE DYNAMICS
William E. Travis
Master of Science, May 11, 2006
(B.S., Auburn University, 2004)
145 Typed Pages
Directed by David M. Bevly
A navigation system was designed using an extended Kalman filter for an
autonomous ground vehicle competing in the 2005 DARPA Grand Challenge. An
overview of this system is provided, and errors in the navigation solution are explained.
These errors are attributed to vehicle dynamics unaccounted for in the navigation model.
Investigation of these errors begins with the development of a nonlinear simulation to
provide vehicle state information in a controlled environment. These vehicle states are
used in various navigation models to show difference in navigation accuracy when lateral
vehicle dynamics are taken into account. Accuracy with and without GPS measurements
is examined. The study then utilizes experimental data to provide similar results. Also,
sources of error stemming from typical velocity sensors are explained.
v
Finally, a method is proposed to utilize a laser scanner to provide measurements for
use in the navigation models incorporating lateral vehicle motion. This method could
also be used to provide a vehicle controller lateral error from a defined corridor. The
method is explained, and experimental results from a simple test bed are shown.
vi
ACKNOWLEDGEMENTS
Without the love and support from others, this thesis would not have been possible.
First and foremost I give my thanks to God, who has always met my needs in abundance.
He has offered me many blessings throughout life, and always placed the right people in
my life at the right time. As I go forward, I continue to pray He uses me to complete His
will.
The Lord has also provided me with two parents whose love and support molded me
into who I am today. They are the perfect role models, exemplifying love, hard work,
determination, and good values. I thank them for their many sacrifices that allowed me
to pursue my goals and dreams. I hope one day I can give back even a small portion of
what you gave to me.
The members of the GPS and Vehicle Dynamics Lab, past and present, vastly
contributed to my education. I thank you for explanations to the many, many questions I
have had. Special thanks go to Rob Daily and Adam Simmons for providing answers to
the hardest of my questions. I also owe a special thank you to my advisor, Dr. Dave
Bevly, for welcoming me into the lab as an undergraduate. Without his technical and
financial support, I would not have had the opportunity to attend graduate school. I thank
him for allowing me to work on some of the many problems and projects that
distinguishes the GPS and Vehicle Dynamics Lab from the many other university
research labs.
vii
Finally, much gratitude is owed to Kimberly Majercik. She stood by me when
stresses of life seemed overwhelming, and I thank her for being patient and understanding
throughout the graduate education process. Most of all, I thank her for accepting me as a
companion throughout life, and I look forward to the many experiences we will share
together in the future.
viii
Style manual or journal used: ASME Journal of Dynamics Systems, Measurement, and
Control.
Computer software used: Microsoft Word 2003.
ix
TABLE OF CONTENTS
List of Figures................................................................................................................... xii
List of Tables .................................................................................................................... xv
Nomenclature................................................................................................................... xvi
1. Introduction................................................................................................................. 1
1.1. Objectives and Contributions.............................................................................. 2
1.2. Outline and Prior Art .......................................................................................... 3
2. DARPA Grand Challenge........................................................................................... 6
2.1. Sensor Suite ........................................................................................................ 9
2.2. Navigation Algorithm ....................................................................................... 12
2.3. Performance ...................................................................................................... 21
2.3.1. NQE .......................................................................................................... 26
2.3.2. Grand Challenge ....................................................................................... 29
2.4. Summary........................................................................................................... 30
3. Vehicle Simulation.................................................................................................... 31
3.1. Vehicle Model................................................................................................... 31
3.1.1. Yaw Equations of Motion Derivation....................................................... 32
3.1.2. Roll and Pitch Equation of Motion Derivation......................................... 36
3.2. Tire Model ........................................................................................................ 37
3.3. Sensor Models................................................................................................... 41
x
3.4. Model Validation .............................................................................................. 43
3.5. Summary........................................................................................................... 46
4. Analysis of Vehicle Dynamics on Navigation.......................................................... 47
4.1. Generation of Vehicle Slip................................................................................ 50
4.2. Navigation Corruption by Sideslip ................................................................... 54
4.2.1. Typical Navigation Model ........................................................................ 56
4.2.2. Modified Navigation Model ..................................................................... 57
4.2.3. Expanded Navigation Model .................................................................... 59
4.2.4. Simulation Results of Sideslip Corruption ............................................... 61
4.2.5. Experimental Results of Sideslip Corruption ........................................... 68
4.3. Navigation Corruption by Longitudinal Slip .................................................... 71
4.3.1. Navigation Model with Wheel Speed....................................................... 74
4.3.2. Navigation Model with Wheel Speed and Radar...................................... 76
4.3.3. Experimental Results of Longitudinal Slip Corruption ............................ 77
4.4. Summary........................................................................................................... 83
5. INS Aiding With A Lidar ......................................................................................... 85
5.1. Test Apparatus .................................................................................................. 86
5.1.1. Experimental Setup................................................................................... 86
5.1.2. IMU........................................................................................................... 86
5.1.3. Lidar.......................................................................................................... 87
5.2. Navigation Models............................................................................................ 89
5.3. Experimental Results ........................................................................................ 95
5.3.1. Scenario 1 ? Five State Estimator............................................................. 95
xi
5.3.2. Scenario 2 ? Six State Estimator............................................................. 100
5.4. Summary......................................................................................................... 102
6. Conclusion .............................................................................................................. 103
6.1. Summary......................................................................................................... 103
6.2. Future Work.................................................................................................... 106
6.2.1. Autonomous Navigation System Development...................................... 106
6.2.2. Navigation Corruption by Vehicle Dynamics......................................... 106
6.2.3. Lidar/INS Navigation.............................................................................. 107
References....................................................................................................................... 108
Appendices...................................................................................................................... 114
Appendix A: DARPA Grand Challenge Vehicle........................................................... 115
Appendix B: Test Vehicle.............................................................................................. 120
Appendix C: Vehicle Simulation Parameters ................................................................ 124
xii
LIST OF FIGURES
Figure 2.1: The 2004 Grand Challenge entrants did not perform as hoped....................... 7
Figure 2.2: Team Terramax crosses the finish line............................................................ 8
Figure 2.3: A Rockwell Collins GIC-100 and Navcom Starfire DGPS receiver............. 10
Figure 2.4: Measurements from the magnetometers compared to GPS. ......................... 12
Figure 2.5: EKF position estimate during a simulated outage......................................... 22
Figure 2.6: Position estimate error during the simulated outage. .................................... 23
Figure 2.7: Heading estimate with and without magnetometer measurements. .............. 24
Figure 2.8: Discrepancy between GPS course and estimated heading. ........................... 26
Figure 2.9: Dead reckoning after a 200 foot tunnel at the NQE...................................... 27
Figure 2.10: The vehicle approaching and traveling through the tunnel. ........................ 29
Figure 2.11: The Grand Challenge course. ...................................................................... 30
Figure 3.1: Schematic used to develop the nonlinear yaw model.................................... 32
Figure 3.2: Schematic of longitudinal road grade............................................................ 33
Figure 3.3: Lateral road grade depiction.......................................................................... 33
Figure 3.4: Dugoff tire curve with varying normal loads. ............................................... 39
Figure 3.5: Front tire curve. .............................................................................................. 40
Figure 3.6: Rear tire curve. ............................................................................................... 40
Figure 3.7: Comparison of simulated and measured roll angle. ...................................... 44
Figure 3.8: Simulated yaw rate compared with measured yaw rate. ............................... 45
xiii
Figure 3.9: Measured and simulated sideslip................................................................... 45
Figure 4.1: Lateral tire curve saturating on different surfaces......................................... 51
Figure 4.2: Bicycle schematic showing slip angles. ........................................................ 53
Figure 4.3: Heading estimates of the different navigation models from simulated data. 62
Figure 4.4: Yaw gyro bias estimates from simulated data............................................... 63
Figure 4.5: Jump in velocity estimate due to neglected terms......................................... 64
Figure 4.6: Corrupted longitudinal accelerometer bias estimate. .................................... 64
Figure 4.7: Roll estimates from the different navigation models. ................................... 65
Figure 4.8: Position error during an outage. .................................................................... 67
Figure 4.9: Estimated sideslip during an outage.............................................................. 68
Figure 4.10: Heading estimates of the navigation models with experimental data. ........ 69
Figure 4.11: Position error during an artificial GPS outage. ........................................... 70
Figure 4.12: Heading estimates during a GPS outage. .................................................... 71
Figure 4.13: Tire coordinate frame. ................................................................................. 73
Figure 4.14: Velocity estimate corrupted by wheel slip. ................................................. 78
Figure 4.15: Estimate with wheel speed bias removed.................................................... 79
Figure 4.16: Estimated wheel speed bias before and during a GPS outage..................... 80
Figure 4.17: Velocity estimate using wheel speed and bias estimate during an outage. . 81
Figure 4.18: Estimated vehicle velocity with Doppler radar. .......................................... 82
Figure 4.19: Doppler radar bias on different terrain........................................................ 83
Figure 5.1: Corner detection with a Lidar........................................................................ 88
Figure 5.2: Calculating lateral offset. .............................................................................. 89
Figure 5.3: Lateral distance measurement. ...................................................................... 94
xiv
Figure 5.4: Velocity estimate using a Lidar..................................................................... 96
Figure 5.5: Heading estimate using a Lidar..................................................................... 96
Figure 5.6: Lateral error estimated using an IMU and Lidar........................................... 97
Figure 5.7: Estimated longitudinal accelerometer bias.................................................... 98
Figure 5.8 Estimate of the yaw rate gyro bias. ................................................................ 98
Figure 5.9: Lateral error estimate assuming no lateral velocity and ?
ylat
=0.01................ 99
Figure 5.10: Lateral error estimate assuming no lateral velocity and ?
ylat
=0.1.............. 100
Figure 5.11: Velocity estimated with second navigation model.................................... 101
Figure 5.12: Scenario 2 heading estimate...................................................................... 101
Figure 5.13: Lateral error estimate using lateral acceleration input. ............................. 102
Figure A.1: RASCAL at the starting gate during the 2005 DGC. ................................. 115
Figure A.2: Steering motor; throttle and brake servos................................................... 116
Figure A.3 Flow of information on RASCAL............................................................... 117
Figure A.4: Obstacle detection sensors on RASCAL.................................................... 118
Figure B.1: Infiniti G35 with data logging console. ...................................................... 120
Figure B.2: The Crossbow IMU; Starfire and Beeline. ................................................. 121
Figure B.3: The G35 data logging computer and power distribution center. ................ 122
xv
LIST OF TABLES
Table 2.1: Process Noise Statistics .................................................................................. 18
Table 2.2: Measurement Noise Statistics......................................................................... 21
Table 3.1: Sensor Characteristics Used in the Sensor Model .......................................... 43
Table 4.1: Process Noise Variances for Sideslip Corruption Investigation..................... 61
Table 4.2: Measurement Noise Variances for Sideslip Corruption Investigation ........... 61
Table 5.1: Process Variance Values for Lidar/INS Aiding.............................................. 92
Table 5.2: Measurement Variance Values for Lidar/INS Aiding.................................... 92
NOMENCLATURE
Chapter 2
A Jacobian of nonlinear vector field
B
w
Process disturbance input matrix
C Measurement matrix
E
?
Estimated east position
L Kalman filter gain matrix
N
?
Estimated north position
P Covariance estimate
Q Continuous process noise matrix
R Discrete measurement noise matrix
V
?
Estimated velocity
1
?
?M
b
Estimated TCM2 magnetometer pitch bias
1
?
?M
b
Estimated TCM2 magnetometer roll bias
1
?
?M
b
Estimated TCM2 magnetometer yaw bias
2
?
?M
b
Estimated Microstrain magnetometer pitch bias
2
?
?M
b
Estimated Microstrain magnetometer roll bias
2
?
?M
b
Estimated Microstrain magnetometer yaw bias
xvi
r
b
?
Estimated yaw rate gyro bias
?
b
?
Estimated pitch rate gyro bias
?
b
?
Estimated roll gyro bias
r Measured IMU yaw rate
x? Estimated state vector
y Measurement vector
?
?
Estimated vehicle pitch
g
?
?
Estimated longitudinal road grade
IMU
?
&
Measured IMU pitch rate
? Standard deviation
?
?
Estimated vehicle roll
M
? Roll plus lateral accelerometer measurement
IMU
?
&
Measured IMU roll rate
?? Estimated vehicle heading
Chapter 3
CG Center of gravity
C
?
Tire cornering stiffness
xvii
xviii
C
s
Tire longitudinal stiffness
F
x
Longitudinal drive force
F
xBL
Longitudinal back left tire force
F
xBR
Longitudinal back right tire force
F
xFL
Longitudinal front left tire force
F
xFR
Longitudinal front right tire force
F
y
Lateral cornering force
F
yBL
Lateral back left tire force
F
yBR
Lateral back right tire force
F
yFL
Lateral front left tire force
F
yFR
Lateral front right tire force
F
z
Normal force
F
zBL
Normal back left tire force
F
zBR
Normal back right tire force
F
zFL
Normal front left tire force
F
zFR
Normal front right tire force
I
x
Roll inertia of the vehicle
I
y
Pitch inertia of the vehicle
I
z
Yaw inertia of the vehicle
K
p
Pitch stiffness
xix
K
r
Roll stiffness
L Vehicle wheelbase
L
D
Dugoff coefficient
V Velocity
V
BL
Velocity of back left tire
V
BR
Velocity of back right tire
V
FL
Velocity of front left tire
V
FR
Velocity of front right tire
V
x
Longitudinal velocity
V
y
Lateral velocity
a Distance from center of gravity to front axle
b Distance from center of gravity to rear axle
b
?
Walking bias
c
?
Constant turn on bias
g Gravity
k
f
Front spring rate
k
r
Rear spring rate
m Vehicle mass
r Yaw rate
s Longitudinal tire slip
xx
s
f
Vehicle front suspension width
s
r
Vehicle rear suspension width
t
f
Vehicle front track width
t
r
Vehicle rear track width
w
?
Gaussian sensor noise
? Lateral tire slip
?
BL
Back left tire slip angle
?
BR
Back right tire slip angle
?
FL
Front left tire slip angle
?
FR
Front right tire slip angle
? Sideslip
? Steer angle
? Roll angle
?
g
Lateral road grade
? Clean sensor measurement
?
m
Dirty sensor measurement
?
?
Bias noise standard deviation
?
?
Bias time constant
? Gaussian noise on bias
? Pitch angle
xxi
?
g
Longitudinal road grade
Chapter 4
R
CG/T
Position vector from tire to center of gravity
V
CG
Velocity at the center of gravity
V
T
Velocity at the tire
e
Position
Position estimate error
Chapter 5
V
m
Velocity measurement from Lidar
y
lat
Lateral error in global frame
y
V
Vehicle lateral position
y
d
Lateral distance from vehicle to corridor
?
m
Heading measurement from Lidar
1
1. INTRODUCTION
The popularity of autonomous ground vehicles, or AGVs, is quickly increasing with
the advent of better and more reliable technology. There is a large niche for such a
vehicle, ranging from agricultural machines, to civilian and military transportation, to
unmanned combat machines, to search and rescue vehicles, and so on. A critical
component for these AGVs is a navigation system to provide vehicle and position
information. A control system must know an AGV?s precise location, either global or
local, as well as speed and direction of travel, in order to properly direct the vehicle and
accomplish the desired objective. The control system can also utilize some vehicle state
estimates from a navigation system to maintain vehicle stability as the vehicle encounters
unfavorable terrain or while operating aggressively. Obstacle detection algorithms also
need the navigation information to correctly map perceived obstacles in a vehicle or
global coordinate frame so the vehicle will successfully and efficiently avoid anything in
the path of travel.
In parallel with the autonomous vehicle boom, civilian vehicle control systems are
expanding their capability to compensate for driver inadequacies and ensure passenger
safety. A need currently exists for more vehicle information to improve control
robustness and reliability. Currently, there are few methods to accurately and/or cheaply
measure lateral vehicle motion (Daily, 2004). In the near future, such control systems
will rely upon improved navigation technologies to provide information about the
2
vehicle, its movement, and its position. For instance, a lane keeping control system could
use the position information to control the vehicle and remain in the lane. Eventually,
passenger vehicles might become autonomous on selected roads or highways, which
would lead to reduced accident rates and driver fatalities. The foundation for passenger
vehicle automation is a solid navigation system.
1.1. OBJECTIVES AND CONTRIBUTIONS
To meet the stringent requirements of AGV and future passenger vehicle control
systems, current technologies will need to be refined and hardened, and new methods
need to be developed to adequately and safely navigate through many environments and
over varying terrains. The subsequent chapters of this thesis detail commonly used
ground vehicle navigation systems, along with frequently used sensors which measure
basic vehicle motion. The inadequacies of the navigation systems and sensors are
investigated, and methods to improve navigational accuracy by expanding current
systems are proposed. An improvement in the accuracy of vehicle position and other
vehicle information is seen by expanding the navigation model.
In addition, a new method to provide a lateral error using a laser scanner is offered.
This lateral error measurement could be used in a vehicle controller to enhance path
following capabilities. For instance, waypoint following controllers can potentially
become unstable if the distance between waypoints is too large and the angle of approach
to the desired waypoint is too large. Oscillations occur if this scenario is realized, and
3
eventually the system can become destabilized. A lateral error measurement can be used
by the controller to effectively damp out any oscillations caused by a large angle of
approach to the next waypoint. By altering the navigation model, a lateral velocity
measurement can be estimated, which could be used in the expanded navigation model
mentioned above. Also, a laser scanner can be used to overcome the shortcomings of
navigation systems in urban environments, as the stable bias of the sensor provides a
suitable alternative to GPS in some situations. However, the laser scanner could still be
augmented with GPS if desired.
1.2. OUTLINE AND PRIOR ART
Chapter 2 briefly describes participation in the DARPA hosted Grand Challenge. To
compete in this event, a completely autonomous vehicle was built capable of traversing
rough terrain in a desert environment (Behringer, 2004, 2005; Ozguner, 2004). A
navigation system was constructed to provide vehicle and navigation information to
various system components. While analyzing the performance of the navigation system,
it was noticed the dynamics of the vehicle introduced errors in the estimated vehicle state
information. Scheding (1999) forewarned that neglecting some vehicle dynamics may
inject errors into the navigation system, but no analysis of the errors was carried out.
Fortunately the system as a whole was robust enough to compensate for the small
inaccuracies. However, aggressive vehicle operation could have induced larger errors
that would quickly propagate through the autonomous system creating undesired
4
behavior. These phenomena were investigated and solutions to improve their robustness
are suggested in the following chapters.
To thoroughly investigate navigation errors caused by vehicle dynamics, a vehicle
simulation was created to replicate the effects in a controlled setting. Chapter 3 provides
the derivation of the vehicle model and sensor models used in the simulation. The model
is validated by comparing it to experimental data.
Chapter 4 describes typical navigation systems and proposes an expanded navigation
architecture which captures some of the vehicle dynamic effects that cause navigation
error. The different systems are first analyzed with the simulation to better explain how
their outputs are corrupted by the vehicle?s dynamics. The systems are then analyzed
with experimental data to prove their existence in real world scenarios. Julier (2003)
proposed a method which utilized a vehicle model to estimate the error inducing
dynamics in a navigation algorithm. This thesis, however, utilizes a direct measurement
of the dynamics in a kinematic navigation model. Outputs of common navigation sensors
can also provide inaccurate measurements under certain conditions. Their effect on the
navigation system is assessed in this chapter as well.
The Grand Challenge also provided knowledge about a variety of sensors for obstacle
detection. A method is proposed in Chapter 5 which uses a common obstacle detection
sensor in a navigation system. Specifically, a laser scanner is used, but the concept is
open to other types of environmental sensors, such as cameras, radar, or sonar. Laser
5
scanners currently serve several purposes on ground vehicles, and their bias is relatively
stable (Ye, 2002). Simultaneous localization and mapping (SLAM) is a technique
combining a laser scanner with GPS and/or INS to map obstacles as the vehicle is
traveling (Guivant, 2000; Hirokawa, 2004; Talaya, 2004). This work differs from SLAM
as it draws information from the environment to estimate a local lateral error. This
method could improve navigational robustness in environments that typically stress
current navigation systems relying on GPS and/or INS measurements, but could also be
coupled with a variety of sensors to enhance the system. Hirokawa (2004) demonstrated
a navigation system utilizing a laser scanner and carrier phase GPS measurements.
6
2. DARPA GRAND CHALLENGE
The Defense Advanced Research Projects Agency (DARPA) hosted an event called
the Grand Challenge in March of 2004 to spur technological growth in the area of
unmanned ground vehicles. The intent of the Grand Challenge was to have teams build
an autonomous ground vehicle (AGV) to compete in a timed race crossing different
terrain types and encountering obstacles for a one million dollar prize. The inaugural
event did not have the desired outcome. The furthest distance a competitor achieved was
7 out of 142 miles. Most vehicles did not make it out of the starting gate. Auburn
University teamed with a limited liability corporation named SciAutonics to enter a
vehicle dubbed RASCAL (Robust, Autonomous, Sensor Controlled All terrain Land
vehicle). More information on this autonomous system can be found in Appendix A.
Figure 2.1: The 2004 Grand Challenge entrants did not perform as hoped.
DARPA again hosted the Grand Challenge in October of 2005 in hopes experience
gained in the first event would propel competitors to produce more capable vehicles.
Knowledge from the first event proved beneficial to teams as five completed the 140 mile
Grand Challenge course in 2005. Auburn University again partnered with SciAutonics to
enter the small AGV as a competitor. The additional time helped improve the vehicle
controller over a broad range of speeds and develop a more complex navigation system
that provided information to the different vehicle subsystems. Auburn University also
partnered with Team Terramax and assisted by providing the initial vehicle controller as
well as vehicle software models. Figure 2.2 displays Team Terramax crossing the finish
line demonstrating the 2005 Grand Challenge was a success (Ozguner, 2004).
7
Figure 2.2: Team Terramax crosses the finish line.
Vehicle navigation was a critical component of the vehicles in the DARPA Grand
Challenge. It provides other systems with state information about the vehicle, such as
speed, orientation, and position. Therefore, the navigation solution had to be accurate, as
well as robust to disturbances that would affect its accuracy. The Grand Challenge
courses offered challenges to navigation systems by limiting GPS availability and
crossing terrain capable of causing vehicle slip. Errors in the navigation solution can
quickly propagate throughout all vehicle subsystems causing the vehicle to drive to
incorrect points and to incorrectly position located obstacles.
8
9
2.1. SENSOR SUITE
The Grand Challenge entry vehicle contained a variety of sensors to determine states
critical to the vehicle controller such as position, heading, and speed. A strategy of
redundancy was employed to provide measurements from some sensors when others
were not available or in the event of the failure of a particular sensor.
The cornerstone of vehicle localization was a single antenna Navcom Starfire DGPS
receiver. It generated unbiased measurements of position (north and east), velocity
(north, east, and up), and course at 5 Hz. With the corrections broadcast by Navcom, this
GPS receiver is capable of producing position measurements accurate to less than 10 cm.
However, the output rate of the receiver was too low to adequately control the vehicle.
An inertial measurement unit (IMU) was used to obtain high update rate
measurements. A Rockwell Collins GIC-100 tactical grade 6 degree of freedom IMU
measured linear accelerations and angular rates at 50 Hz. These measurements, however,
were inherently corrupted with biases and noise. Dead reckoning with the unit provides
acceptable results for a short period of time if the initial biases are eliminated, but the
biases are not constant and therefore must be continually estimated and removed from the
measurement to provide a reliable navigation solution.
Figure 2.3: A Rockwell Collins GIC-100 (left) and Navcom Starfire DGPS receiver (right).
The ATV?s onboard speedometer was used as an additional speed sensor. The output
rate of this sensor was dependent upon vehicle speed, so compensation was needed to
provide a more consistent measurement to the controller. The sensor waited until a
certain number of revolutions of the drive shaft occurred before sending the measurement
to the computer. It did not report speeds less than 1.5 m/s either. Also, the measurement
was corrupted by wheel slip, which appeared as a sudden, large change in the bias if the
bias was not modeled as a function of time. To calibrate the sensor, a scale factor was
calculated to match the speedometer output to GPS velocity. This scale factor was not
constant across all ranges of speed and would corrupt the speed estimate during a GPS
outage.
Magnetometers are often used in aerial applications to provide orientation
information. They sense the earth?s magnetic field, thus all measurements are referenced
from magnetic north and not true north. This difference can easily be calibrated and
remains fairly constant if the sensor remains in a region near its calibration point. The
AGV utilized two magnetometers to measure the vehicle?s heading, roll, and pitch
10
11
angles. A TCM2 magnetometer provided 16 Hz measurements that contained high noise
but a slow bias drift rate. A Microstrain 3DM-GX1 IMU and magnetometer provided 50
Hz measurements that had very little noise, but the bias drifted quickly (the IMU was not
used). Figure 2.4 displays vehicle yaw measurements from the two magnetometers and
GPS. The quickly changing bias in the Microstrain magnetometer is immediately
evident. The TCM2 magnetometer bias was fairly constant during this run but the noise
statistics varied. It was discovered that the magnetometers could help initialize the
navigation algorithm, but once the vehicle started moving they were of little use because
the magnitude and drift rate of their biases and changing noise statistics was greater than
that of the other sensors.
70 80 90 100 110 120
20
40
60
80
100
120
140
160
Time (s)
Ya
w (
d
e
g
)
TCM2
Microstrain
GPS
Figure 2.4: Measurements from the magnetometers compared to GPS.
2.2. NAVIGATION ALGORITHM
The localization algorithm used was an extended Kalman?Bucy filter (EKF), outlined
in detail by Stengal (1994). This method assumes the system can be represented in the
following form:
()
()vuxhy
wuxfx
+=
+=
,
,&
2.1
where the Jacobian of the vector field is
1?
=
?
?
=
?
?
k
x
A
x
x
x
f &
2.2
12
and the measurement matrix is
1?
=
?
?
k
x
C
x
h
2.3
The process noise covariance matrix denoted by Equation 2.4.
[ ] )(?QwwE
T
= 2.4
It is important to realize that Q must be continuous when used with this specific
algorithm. The discrete conversion occurs within the Kalman filter, using the process
noise input matrix, B
w
.
1?
=
?
?
k
x
w
B
w
x&
2.5
The matrix B
w
was set to identity to add process noise to all states. The measurement
noise covariance matrix is defined in a similar manner as Q.
[ ] )(
k
T
tRvvE =
2.6
The covariance of the estimate error is defined in Equation 2.7.
()()[ ] )(??
k
T
tPxxxxE =?? 2.7
Note this equation requires information about the actual state, x, which is not available.
However, the Kalman filter is able to estimate the covariance of the estimate error using
the A, C, Q, and R matrices.
The Kalman?Bucy filter algorithm handled the system nonlinearities by continuously
propagating the system model and discretely propagating the measurement update, as
shown in the following equations.
13
State Estimate Continuous Propagation
?
?
?
?
?
?
?
? ?
+
+
?
=
?
k
t
k
t
duxf
k
tx
k
tx
1
),(),(?)
1
(?)(? ????
2.8
Covariance Estimate Continuous Propagation
??????? d
T
w
BQ
w
B
k
t
k
t
T
APPA
k
tP
k
tP
?
?
?
+
?
?
?
?
?
++
+
?
=
?
)()(
1
)()()()()
1
()(
2.9
Kalman Filter Gain Calculation
1
)()()()()()()(
?
?
?
?
?
?
?
+
??
=
k
tR
k
tC
k
tP
k
tC
k
tC
k
tP
k
tL
TT
2.10
State Estimate Discrete Update
?
?
?
?
?
? ?
?+
?
=
+
)(?)()()()(?)(?
k
tx
k
tC
k
ty
k
tL
k
tx
k
tx
2.11
Covariance Estimate Discrete Update
[ ] )()()()(
?
?=
+
k
tP
k
tC
k
tLI
k
tP
2.12
Kalman filtering is a proven method for blending measurements to eliminate various
sensor deficiencies while utilizing the strengths of each sensor by statistically weighting
each measurement. The EKF combined the bias free, low update GPS measurements
with the other measurements to produce a bias free, high update navigation solution. An
EKF is as accurate and less computationally intensive as some higher order filters if the
sample rate is high enough (St. Pierre, 2004). This efficiency was an advantage with the
algorithm because the state vector already imposed a moderate computational burden by
containing 16 states. GPS and inertial measurements were loosely coupled, meaning the
14
15
inertial errors were corrected with a computed GPS solution. A tightly coupled system
was considered, where the raw GPS pseudo-range measurements amend the IMU errors,
but due to time constraints and the overall satisfaction with the loosely coupled system, it
was not constructed.
The geometric relationships between the sensors on board the AGV were easily and
accurately measured. Using this information, kinematic relationships between the
sensors were derived. These relationships, otherwise known as a kinematic model, made
up the vector fields listed in Equation 2.1. A vehicle model could have been used in
place of the kinematic model, but knowledge of vehicle parameters would have been
required. For the AGV, the parameters were either unknown, or had the potential to vary,
so the kinematic model improved the robustness of the navigation solution by relying
only on the kinematic relationships between the sensors.
The kinematic model evolved with the vehicle as new sensors were added. The first
model was a simple, four state linear model estimating the vehicle?s velocity, course,
longitudinal accelerometer bias, and gyro bias. The second model added states of north
position and east position, which are described in the Kalman filter with a first order,
nonlinear differential equation. The nonlinearities required the use of the EKF mentioned
above. The third and fourth models were intended for use during the National
Qualification Event (NQE) and the Grand Challenge. Those models used kinematic
relationship between lateral acceleration, velocity, and yaw rate, to provide an effective
16
measurement of roll, and the GPS up and forward velocity to generate a road grade
measurement. Using this effective measurement, states of roll plus lateral accelerometer
bias, pitch, and road grade became observable (Bevly, 2004). The fourth model utilized
roll, pitch, and yaw measurements from the two magnetometers to add redundancy to the
estimator. The need of the third model existed because magnetometer problems were
experienced during the 2004 Grand Challenge due to their sensitivity to nearby metallic
objects. However, only the fourth model is presented in this work since it was actually
used during the events.
A navigation algorithm with a sixteen state kinematic model was constructed for use
in the NQE and DARPA Grand Challenge event. This model estimated velocity,
longitudinal accelerometer bias, course, yaw rate gyro bias, north and east position, roll,
pitch, and road grade, as well as IMU and magnetometer biases when GPS was available.
During a GPS outage or iterations where GPS was not available, the navigation algorithm
dead reckoned using the IMU, wheel speed sensor, and magnetometers with their
respective biases removed. The vehicle controller used the estimated heading and
velocity states to guide the vehicle along the desired path. The orientation states of roll,
pitch, road grade, and heading, were used to transfer the data recorded from the obstacle
detection sensors from the vehicle frame to a global frame. The nonlinear differential
equations for this model are shown in Equation 2.13.
)?sin(
??
)?cos(
??
0
?
?
?
???
?
?
?
??
VE
VN
b
br
ggaV
r
r
gx
=
=
=
?=
??=
&
&
&
&
&
0
?
0
?
??
0
?
??
=
=
?=
=
?=
g
IMU
IMU
b
b
b
b
?
??
??
?
?
?
?
&
&
&
&
&
&
&
0
?
0
?
0
?
0
?
0
?
0
?
2
2
2
1
1
1
=
=
=
=
=
=
?
?
?
?
?
?
M
M
M
M
M
M
b
b
b
b
b
b
&
&
&
&
&
&
2.13
These nonlinear equations were continuously propagated in the EKF time update of the
state estimate. However, the time propagation of the covariance estimate required a
linear system matrix. The linear system matrix used the Jacobian of the nonlinear model,
which was calculated with Equation 2.2. The matrix shown below is the linear system
matrix used in Equation 2.9.
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??
=
0000000000000000
0000000000000000
0000000000000000
0000000000000000
0000000000000000
0000000000000000
0000000000000000
0000000000000000
0000000100000000
0000000000000000
0000000001000000
00000000000000)?cos(
?
)?sin(
00000000000000)?sin(
?
)?cos(
0000000000000000
0000000000000100
00000000000000
??
??
V
V
gg
A
2.14
17
Biases were modeled as random walks in the process noise matrix. These were used
as tuning parameters for the EKF because they directly influenced the amount of filtering
on the estimated states. The other entries in the matrix captured the system noise, which
was determined by gathering static sensor measurements. Assuming the sensor noises
are uncorrelated, the process noise covariance matrix is a diagonal matrix with the
covariances in Table 2.1. The time update in the EKF assumes this matrix to be
continuous. Therefore the measured, discretized values are multiplied by the sample rate
(dt) in order to approximate the continuous covariance values.
Table 2.1: Process Noise Statistics
22
0.65
az
dt? =
22
0.038129
r
dt? =
21
10
br
?
0?
=
22
0.17851 dt
?
? =
&
21
10
b?
?
?
=
0
22
0.088147 dt
?
? =
&
28
10
b?
?
?
=
22
0.000001
N
dt? =
22
0.000001
E
dt? =
27
10
g
b?
?
?
=
24
1
10
bM?
?
?
=
26
1
10
bM?
?
?
=
28
1
10
bM?
?
?
=
2
2
0.01
bM?
? =
2
2
0.01
bM?
? =
2
2
0.1
bM?
? =
The discrete measurement update in the EKF utilized statistically weighted
measurements from the sensors listed in Section 2.1 to overcome integration errors. Two
calculations were included in addition to the raw sensor measurements. GPS velocities
were used to solve for road grade using Equation 2.15.
18
GPSx
GPSup
g
V
V
=?
2.15
Linear equations of vehicle roll as a function of lateral acceleration were derived using
knowledge of the vehicle?s dynamics and a basic sensor model. A basic sensor model
can be written as a scale factor on the actual state, plus bias (constant and/or moving),
plus noise.
wbSFxy ++=
2.16
Applying this equation to a lateral accelerometer measurement leads to the following
equation.
( ) wbVrgVSFa
ayMyy
++++= )sin(?
&
2.17
Under the small angle assumption, the V
y
term goes to zero because vehicle sideslip can
be ignored, and the roll component can be simplified. The yaw rate measurement, r, will
have its own inherent scale factor and bias which need to be removed. Manipulating
Equation 2.17, applying the small angle approximation, and removing the scale factors
leads to a measurement of vehicle roll plus lateral accelerometer bias.
()
ayryM
gbbrVa
g
???= )(
1
?
2.18
The measurements for the 16 state EKF are listed in Equation 2.19.
[
]
T
MAGMAGgMAGMAG
MMAGMAGGPSGPSGPSWSGPS
ENVVy
2121
21
...
?????
????=
2.19
19
The measurement matrix, C, was adjusted accordingly depending upon the availability of
the different measurements. The vehicle computing system was run at 50 Hz, but some
sensors had slower output rates. For instance, GPS measurements were output at only 5
Hz. When a measurement was unavailable, its corresponding value was set to zero.
States dependent upon an unavailable measurement were unobservable, and the algorithm
integrated the kinematic model to provide estimates. The resulting measurement matrix
when all measurements are available is shown in Equation 2.20.
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
=
0010000000000010
0000010000000010
0000001000000000
1001001000000000
0000001000000000
0000000000100000
0100000000100000
0000100000100000
0000000000010000
0000000000001000
0000000000000010
0000000000000001
0000000000000001
C
2.20
Table 2 displays the values used in the measurement covariance matrix, R. These values
were experimentally obtained by calculating the standard deviation of a static data set.
However, the noise statistics for GPS course and GPS road grade are a function of the
GPS velocity noises (Bevly, 2004).
20
Table 2.2: Measurement Noise Statistics
22
0.05
V
? =
22
0.3
VWS
? =
2
2
V
V
?
?
?
??
=
??
??
22
0.1
N
? =
22
0.1
E
? =
2
2
g
V
V
?
?
?
??
=
??
??
22
0.08135
calc
?
? =
22
1
0.19995
M?
? =
22
1
0.024445
M?
? =
22
1
0.036297
M?
? =
22
2
0.003
M?
? =
22
2
0.003
M?
? =
22
2
0.003
M?
? =
2.3. PERFORMANCE
The algorithm?s performance was evaluated based on the amount of error during a
simulated GPS outage. Two reasons for this evaluation method are as follows: first,
when enough satellites are in view and the receiver is outputting valid messages, the EKF
successfully tracks the GPS measurements, and second, a real GPS outage would
eliminate the truth measurement, degrading the accuracy of the error analysis. Figure 2.5
is a plot of GPS and estimated position during a test run. The circles signify the
beginning and end of the simulated outage, starting before the first turn and concluding at
the end of the straight section.
21
40 50 60 70 80 90 100 110
-40
-35
-30
-25
-20
-15
-10
East (m)
N
o
rth
(m
)
GPS EKF
Figure 2.5: EKF position estimate during a simulated outage.
The error growth which occurs at the onset of the outage can be seen in the above
figure. Figure 2.6 is a plot of the navigation error. Over the 25 second outage, the
vehicle was traveling 3.2 m/s. The maximum error for this period of time at this speed
was slightly less than a meter.
22
380 390 400 410 420 430 440 450
-0.2
0
0.2
0.4
0.6
0.8
1
E
rro
r (m
)
Time (s)
Figure 2.6: Position estimate error during the simulated outage.
The IMU and speedometer were the sensors whose biases impacted the navigation
solution the most. The leading error source during this run was due to the calibration
error in the speedometer. The vehicle experienced slight lateral translation, or slipped, as
it rounded the corner, also introducing a small navigation error. These two error sources
are deduced by eliminating other possible error sources. The tactical grade IMU
contained mechanical rate gyros with a bias drift rate of less than one degree per square-
root-hour, so the bias error over this period of time is very small. Therefore, error due to
IMU bias drift can be assumed to be negligible. As stated earlier, the magnetometers
were not much use during a run and were statistically weighted out of the EKF after
23
initialization. Therefore they did not have much impact on the navigation solution once
the vehicle was in motion. To prove the magnetometers were ignored once the vehicle
was moving, a Kalman filter that did not use magnetometer measurements was compared
to the algorithm on the AGV. Figure 2.7 shows the output of the navigation system
utilizing the magnetometers compared to the system that did not use the information.
When the vehicle is moving, there is virtually no difference in the estimated heading.
80 85
40
50
60
70
80
90
100
110
120
130
140
Heading
H
ead
i
n
g
(d
eg
)
GPS
No Mag
Mag
Figure 2.7: Heading estimate with
The effects of the calibration error could be
wheel speed bias was estimated, the calib
when the bias was modeled with high enough dynam
90 95 100 105 110
Time (s)
and without magnetometer measurements.
removed when GPS was available. If a
ration error effects would appear in the estimate
ics. However, when GPS was lost,
24
25
bias states were held constant because they were unobservable. Therefore if the vehicle
changed speeds during an outage, this bias estimate would be incorrect because the
calibration error would change proportionally with speed. The navigation algorithm on
the AGV without the estimated bias state was compared with another Kalman filter that
included this bias state. It was determined there was no benefit to including the bias state
because the dead reckoning errors resulting from each estimator were similar.
Another source of navigation error was attributed to vehicle slip. Vehicle slip can
disrupt the navigation algorithm even in the presence of GPS. Longitudinal and lateral
slip occur on moving ground vehicles. Longitudinal slip is generated by a difference in
the vehicle?s velocity and the wheel?s velocity, and lateral slip is created when the
vehicle translates laterally. Wheel slip can corrupt the speedometer measurement by
causing a sudden jump in the estimated bias (if the modeled bias dynamics have a high
enough bandwidth). Also, it reports a false speed value to the Kalman filter which can
directly inject error into the speed and position estimates. Sideslip also leads to a less
accurate estimate because GPS and integrated IMU measurements differ. Evidence of
sideslip induced error can be seen in Figure 2.6 at 420 seconds when the vehicle enters a
turn. Figure 2.8 is a plot of the estimated heading when GPS is available. At 395 and
420 seconds, a discrepancy due to sideslip can be seen between estimated heading and
GPS course.
85 90 95 100 105 110 115
50
100
150
H
eadi
ng (de
g
)
GPS
KF
85 90 95 100 105 110 115
-20
-10
0
10
20
E
r
r
o
r (d
eg
)
Time
Figure 2.8: Discrepancy between GPS course and estimated heading.
The error caused by vehicle slip is seen on multiple estimated states and is influenced
by the level of filtering in the EKF. This phenomenon prompted further research on the
influence of vehicle dynamics on navigation algorithms. Preliminary work is discussed
in (Travis, 2005). Chapter 4 presents a detailed analysis showing how a navigation
solution is corrupted by vehicle slip.
2.3.1. NQE
The NQE was the final qualification event for the Grand Challenge. Of the 195 initial
entrants, 43 teams successfully completed a pre-selection qualification and participated at
the NQE. After the NQE, the best 23 teams got the opportunity to start the Grand
26
Challenge. The main goal of the NQE was to complete four different tracks, each
approximately 2.5 miles long, while avoiding various obstacles. The daunting challenge
to the navigation system was a 200 foot covered tunnel that blocked GPS signals.
The first NQE run successfully navigated through the tunnel, but failed shortly
thereafter. As the vehicle approached an obstacle, it suddenly swerved and made contact
with a barrier. Upon examining the data, it was discovered the vehicle accurately dead
reckoned for two minutes. Figure 2.9 shows the dead reckoning performance of the
vehicle.
Figure 2.9: Dead reckoning after a 200 foot tunnel at the NQE.
The tunnel is outlined in white at the right of the figure. GPS waypoints are denoted
by dark blue dots. The corridor the vehicle was required to remain in a given corridor,
which is shown shaded in light blue. The EKF output is indicated by red dots. The EKF
output remained in the center of the corridor throughout most of the outage. However, to
assess the accuracy of the dead reckoned navigation solution, the areas where the vehicle
avoided obstacles must be examined. As the vehicle made the left hand turn encircled by
27
28
a white oval, it successfully avoided an obstacle. Again, the vehicle avoided an obstacle
after it made a right turn onto the widened corridor. Had the navigation solution been
incorrect, the vehicle would not have successfully avoided these two obstacles.
The cause of the failure can also be seen in Figure 2.9. When the GPS unit began
receiving signals again, it reported an accuracy of 10 centimeters on the first waypoint.
This information was incorrect as Starfire corrections are not instantaneous. The reported
waypoint was actually 10 meters from the EKF position estimate. The GPS receiver also
reported pure zeros in its calculation for velocities, which ultimately blew up the EKF.
The addition of a few simple lines of code ignored these false messages at the expense of
having to dead reckon for approximately four minutes after an outage.
All other NQE runs were completed by the vehicle. Figure 2.10 shows the vehicle
approaching and traveling through the 200 foot tunnel during the third run at NQE.
Similar performance is seen as the vehicle remains in the center of the corridor. The
yellow dashes represent obstacles.
Figure 2.10: The vehicle approaching and traveling through the tunnel.
2.3.2. Grand Challenge
The navigation system was not stressed during the actual Grand Challenge as the
vehicle was shut down before it encountered any obstacles that would obstruct satellite
view. Therefore, it tracked GPS velocity, course, and position for the duration of the
vehicle?s operation in the event. Figure 2.11 is a plot of the DARPA Grand Challenge
course in green and the estimated vehicle position as reported from the navigation system
in black. Total distance traveled was approximately 16 miles before a power cable
disconnected and crashed the obstacle avoidance algorithm.
29
driving
direction
Start / Finish
elimination point
Figure 2.11: The Grand Challenge course.
2.4. SUMMARY
A navigation system for an autonomous AGV was developed utilizing an extended
Kalman-Bucy filter. The system also provided vehicle state information to other vehicle
subsystems. The vehicle was invited to participate in the DARPA Grand Challenge,
where it completed 16 miles before a hardware failure occurred. While analyzing data,
peculiar navigation errors were noticed when the vehicle completed turns at higher
speeds. It was concluded the error sources were the vehicle dynamics, specifically
vehicle slip.
30
31
3. VEHICLE SIMULATION
A MATLAB? vehicle simulation was developed to replicate driving scenarios
typically producing undesirable sideslip angles to study their influence on navigation
systems. A derived vehicle model was used to simulate the vehicle response during input
maneuvers. The simulated response was compared with experimental data to validate the
simulation?s use in this study. The simulation was then used as a tool to evaluate the
effect sideslip has on a Kalman filter both when GPS is and is not available. Parameters
used in the simulation can be found in Appendix C.
3.1. VEHICLE MODEL
A five degree of freedom vehicle model was chosen to simulate various driving
scenarios. A nonlinear yaw model was chosen to replicate the dynamics when a vehicle
experiences large sideslip angles. Linear roll and pitch models were developed using the
small angle assumption that the roll and pitch angles would be less than 10 degrees.
Inputs into the simulation were a velocity profile (V), steer angle profile (?), and time
vector. The GPS velocity, filtered using a forward-backward filter to attenuate noise, and
steer angle were used as inputs to simulate an experimental run. The physical dimensions
of the car such as front and rear track width (t
f
and t
r
), distance from the front and rear
axle to the CG (a and b), and suspension position (s
f
and s
r
) were measured. Front and
rear spring rates (k
f
and k
r
) were also measured. Inertias (I
x
, I
y
, and I
z
) and CG height
(h
CG
) were approximated. Values for the model parameters, as well as details about the
experimental test bed, are given in Appendix B.
3.1.1. Yaw Equations of Motion Derivation
The equations of motion for the yaw model were found by summing forces and
moments about the CG. A vehicle schematic about the yaw axis is shown in Figure 3.1.
?
BL
?
BR
?
FL
?
FR
ab
t
r t
f
F
xBL
F
xBR
V
FR
V
FL
F
xFL
F
xFR
V
BR
V
BL
F
yFL
F
yFR
F
yBR
F
yBL
?
?
V
x
V
V
y
?
r
x
y
z
Figure 3.1: Schematic used to develop the nonlinear yaw model.
Summing the moments about the CG yields the following equation:
[]
()()
yBRyBLxBRxBL
r
xFRxFLyFRyFL
yFRxFRyFLxFL
f
zCG
FFbFF
t
FFFFa
FFFF
t
rIM
+??+
++++
+??==?
2
)sin()sin()cos()cos(
)sin()cos()sin()cos(
2
????
????&
3.1
Road grade in the longitudinal direction is denoted by ?
g
. The effects must be accounted
for when deriving the equations of motion.
32
?
g
Z
z
X
x
Figure 3.2: Schematic of longitudinal road grade.
Summing forces in the x direction yields Equation 3.2.
)sin()cos()cos(
)sin()sin(
gxFRxFL
xBRxBLyFRyFLx
mgFF
FFFFxmF
???
??
?++
++??==? &&
3.2
Lateral road grade, ?
g
, has a similar effect to longitudinal road grade, as shown by Figure
3.3.
Figure 3.3: Lateral road grade depiction.
Equation 3.3 is found by summing forces along the y axis.
)sin()sin()sin(
)cos()cos(
gxFRxFL
yBRyBLyFRyFLy
mgFF
FFFFymF
???
??
+++
+++==? &&
3.3
The acceleration vector consists of a component in line with the vehicle?s direction of
travel ( ), and a centripetal component ( ). The vector is translated to the vehicle
frame for use in the equations of motion.
T
a
v
N
a
v
33
NT
aaa
vv
v
+=
3.4
Equation 3.4 can be rewritten to include the variables defined in Figure 3.1.
VVa
v
&
v
v
?+= ?
3.5
The velocity vector is split into x and y components.
)sin(
)cos(
?
?
VV
VV
y
x
v
v
=
=
3.6
Velocity in the z axis is assumed to be zero for this study. The time derivative of
Equation 3.6 yields the acceleration component in the vehicle?s direction of travel.
)cos()sin(
)sin()cos(
???
???
&
v
&
v
&
&
v
&
v
&
VVV
VVV
y
x
+=
?=
3.7
The centripetal acceleration due to rotation about the z axis, or yaw rate, is translated to
the vehicle frame using the following equation.
( ) ( )
yx
VrVrV )sin()sin( ???
vvv
+?=?
3.8
Combining the acceleration components yields the vehicle?s longitudinal acceleration.
())sin()cos( ??? rVVx +?=
&
v
&
v
&&
3.9
Lateral acceleration in the y direction is found in the same manner.
() )cos()sin( ??? rVVy ++=
&
vv
&&
3.10
Substituting the x acceleration equation into Equations 3.2 yields:
( ) ( ) ( )
()
?
?
?
?
?
?
+?=
++++??
)sin()cos(
)cos()sin(
???
??
rVVm
FFFFFF
xFRxFLxBRxBLyFRyFL
&
v
&
v
3.11
and substituting the y acceleration given in Equation 3.10 into Equation 3.3 yields:
34
( ) ( ) ( )
()[])cos()sin(
)sin()cos(
???
??
rVVm
FFFFFF
xFRxFLyBRyBLyFRyFL
++=
+++++
&
vv
3.12
The total drive force is assumed to be split equally among the rear tires as seen in
Equation 3.13.
xBRxBLx
FFF +=
3.13
The following equations are found by solving for yaw acceleration, sideslip rate, and
total drive force at the rear axle using Equations 3.1, 3.11, and 3.12.
Yaw Acceleration
)()sin()(
... )cos()()sin()(
... )cos()(
2
)(
2
yBRyBL
z
xFRxFL
yFRyFL
z
yFRyFL
xFRxFL
z
f
xBRxBL
z
r
FF
I
b
FF
FF
I
a
FF
FF
I
t
FF
I
t
r
+??+
++?+
?+?=
?
??
?&
3.14
Sideslip Rate
[
] } r
V
gVFF
FFFF
m
gxFRxFL
yFRyFLyRRyRL
???+
?
?
?
+++=
)sin(
1
)sin()sin()sin()(
... )cos()()(
1
?
???
??
&
v
&
v
&
3.15
Drive Force
)sin()(
... )sin()sin()()cos((
?
????
yFRyFL
gx
FF
grVVmF
++
?
?
?
?
?
?
???=
&
v
&
v
3.16
In the vehicle simulation, the yaw acceleration and sideslip rate were numerically
integrated to obtain yaw rate, heading, and sideslip. Once the drive force (F
x
) was
calculated, it was divided up among the four wheels. A positive drive force represented
acceleration, and the total force was split between the two rear wheels (F
x_OR
, F
x_IR
). A
35
negative drive force indicated braking and was split with a 60:40 ratio between the front
(F
x_OF
, F
x_IF
) and rear axles as shown in Equation 3.17.
2
6.0 ,
2
4.00
2
0
x
xFLxFR
x
xBLxBR
x
xBLxBR
F
FF
F
FFx
F
FFx
====?<
==??
&&
&&
3.17
3.1.2. Roll and Pitch Equation of Motion Derivation
Pitch (?) and roll (?) were approximated as linear, second order systems centered at
the CG. Equations 3.18 and 3.19 were numerically integrated to obtain pitch and roll.
These equations are valid only at small roll and pitch angles.
Pitch
CGpy
hxmKI &&
&&&
=++ ??? 7.0
3.18
Roll
CGrx
hymKI &&
&&&
=++ ??? 7.0
3.19
When the vehicle?s front and rear axle spring stiffness (k
f
, k
r
) are known, the vehicle?s
pitch stiffness K
p
is defined by Dixon (1996) as the following:
rfp
kbkaK
22
22 +=
3.20
Similarly, the roll stiffness K
r
is shown in Equation 3.21, where s
f
and s
r
are the distances
between the left and right springs on the front and rear.
)(
2
1
22
rrffr
skskK +=
3.21
Knowledge of the roll and pitch angles provides a more accurate dynamic calculation
of the normal loads at each corner of the vehicle. In a static state, the normal loads are
simply a function of the vehicle?s weight, wheel base, and track width. However, weight
is transferred to and from the corners of the vehicle when the vehicle moves. Therefore
36
the normal loading becomes a function of not only the static normal load, but also
includes effects from roll, pitch, and acceleration.
3.2. TIRE MODEL
A Dugoff tire model was used to simulate the nonlinear tire behavior. This specific
model was chosen for its ability to approximate the tire curve with inputs of lateral and
longitudinal tire stiffness (C
?
and C
s
) and peak friction coefficient (?). This model also
has the ability to couple longitudinal and lateral tire slip. As longitudinal tire slip
increases, the amount of available lateral force decreases (Olson, 2003). The Dugoff tire
model is shown in the following equations. Longitudinal tire slip is denoted by s, and
lateral tire slip is denoted by ?.
1 otherwise
)2( ,1 if
)tan(2
)1(
222
=
?=<
+
?
=
f
LLfL
CC
sF
L
DDD
s
z
D
?
?
?
3.22
Using f, the lateral and longitudinal tire force is solved given a normal load and slip angle
or percentage. Lateral tire force is given by Equation 3.23.
f
s
C
F
y
?
?=
1
)tan(?
?
3.23
Longitudinal force is provided using Equation 3.24.
f
s
sC
F
s
x
?
=
1
3.24
Tire peak force is a function of normal load. Under static conditions on a level
surface, the normal load at a front and rear tire is given by Equation 3.25.
37
L
mga
F
L
mgb
F
zRzF
2
,
2
==
3.25
However, dynamic conditions and road grade induce weight transfer due to roll and pitch.
The body orientation angles described in Section 3.1.2 were used to calculate the normal
load at each wheel. Lateral weight transfer was assumed to be caused by lateral
acceleration and roll angle. It was calculated at the front and rear axles.
?
?
?
?
?
?
?
?
+=? ?
f
f
CG
ZF
k
g
y
t
h
L
b
mgF
&&
?
?
?
?
?
?
?
?
+=? ?
r
r
CG
ZB
k
g
y
t
h
L
a
mgF
&&
3.26
Longitudinal weight transfer was simulated only with longitudinal acceleration under the
assumption the pitch angles remain small. This study focused on maneuvers that excited
the lateral vehicle dynamics; therefore this assumption is safe because no maneuvers
creating significant pitch angles were input into the simulation.
L
xmh
F
CG
ZP
&&
=? 3.27
Distributing the weight transfer effects among the four tires yields Equation 3.28.
22
ZP
ZFZFL
F
F
L
mgb
F
?
??+=
22
ZP
ZFZFR
F
F
L
mgb
F
?
???=
22
ZP
ZRZBL
F
F
L
mga
F
?
+?+=
22
ZP
ZRZBR
F
F
L
mga
F
?
+??=
3.28
38
Figure 3.4 displays a lateral tire curve generated by the Dugoff model with varying
normal loads. No longitudinal slip was included in this curve.
-50 -40 -30 -20 -10 0 10 20 30 40 50
-5000
-4000
-3000
-2000
-1000
0
1000
2000
3000
4000
5000
Tire Slip Angle (deg)
L
a
te
ra
l F
o
rc
e
(N
)
Figure 3.4: Dugoff tire curve with varying normal loads.
The tire cornering stiffness, C
?
, was experimentally determined by operating the
vehicle at high speeds around corners of the track owned by Auburn University?s
National Center for Asphalt Technology (NCAT). Figure 3.5 and Figure 3.6 show the
linear region of the tire curve for a front and rear tire. Front and rear tire cornering
stiffness was estimated to be approximately 800 N/deg and 1300 N/deg per tire,
respectively. The peak friction coefficient, ?, served as a tuning parameter for the
simulation, and was selected to be 0.84 for the front and 0.81 for the rear.
39
Figure 3.5: Front tire curve.
Figure 3.6: Rear tire curve.
40
The tire curves were generated utilizing a sideslip measurement from a two antenna
GPS receiver. The method used to measure sideslip is given in Appendix B. Once the
sideslip measurement was taken, a longitudinal and lateral velocity at the CG could be
moved to the individual tires using Equation 3.29.
CGTCGT
RrVV
/
vvv
?+=
3.29
Once the lateral and longitudinal tire velocities were known, the tire slip angle (?) was
calculated.
?? ?
?
?
?
?
?
?
?
?
=
?
x
y
V
V
1
tan
3.30
Lateral tire force was calculated using Equations 3.1 and 3.3.
r
L
I
Vr
L
ma
F
r
L
I
Vr
L
mb
F
z
yf
z
yf
&
&
?=
?
?
?
?
?
?
+=
)cos(
1
?
3.31
3.3. SENSOR MODELS
Expressions for clean measured accelerations (without noise or biases) were found by
taking the time derivative of the velocity vector at the CG and adding the centripetal
acceleration effects due to rotation about each axis. The acceleration measurement in the
z axis consists solely of centripetal acceleration and gravity components since the car is
restricted to operate on a level surface.
Longitudinal Acceleration Measurement
)sin()sin()()cos(
gx
grVVa ????? +++?=
&
v
&
v
3.32
Lateral Acceleration Measurement
41
)sin()cos()()sin(
gy
grVVa ????? ++++=
&
v
&
v
3.33
Vertical Acceleration Measurement
)sin()sin()cos()sin(
ggz
ggVVa ???????? +?+??=
&
v
&
v
3.34
Inertial sensors were simulated by adding Gaussian noise, a constant bias, and a
walking bias to the raw measurement, as shown in Equation 3.35.
Sensor Model
???
?? wbc
m
+++=
3.35
The variable ?
m
denotes actual sensor output, ? is the true acceleration or turning rate, c
?
is the constant turn on bias, b
?
is the walking bias modeled with a Markov process given
by Equation 3.36, and w
?
is Gaussian noise.
Markov Process
]1,0[~ ,
21
2
??
?
?
?
?
?
?
?
?
t
bb
?
+?=
&
3.36
Inputs to the Markov process are the sensor sampling rate (?t), Markov time constant (?
?
),
bias standard deviation (?
?
), and Gaussian noise (?). Values used for the time constant
and standard deviation for each sensor are listed in Table 3.1 (Gebre-Egziabher, 2004).
42
43
Table 3.1: Sensor Characteristics Used in the Sensor Model
Sensor Noise
(w
?
)
Turn On Bias
(c
?
)
Bias Time
Constant (?
?
)
Bias Variation
(?
?
)
Automotive Grade
Rate Gyro
~N[0,1] >0.6 m/s
2
300 sec 180 ?/hr
Automotive Grade
Accelerometer
~N[0,1] >0.3 deg/s 100 sec 1.2x10-3 g
3.4. MODEL VALIDATION
The model was first compared to actual data to validate its use as a tool to investigate
how sideslip corrupts a navigation solution. The simulated data was compared with
steady state maneuvers and transient maneuvers to confirm the model?s accuracy.
Experimental maneuvers were performed with the Infiniti G35, which is described in
detail in Appendix B. The simulation assumed the vehicle was on level terrain because
of its inability to compensate for varying road grade. Also, an assumption of no
longitudinal slip was made. Because of this assumption, the vehicle was put in neutral
before starting the maneuver. Figure 3.7 displays the measured and simulated roll data
during two abrupt lane change maneuvers. The discrepancy seen in Figure 3.7 is due to
the ?2? of road crown as the vehicle moves from one lane to the other.
2400 2600 2800 3000 3200 3400
-5
0
5
Time (s)
R
o
ll (d
e
g
)
Acutal
Simulated
Figure 3.7: Comparison of simulated and measured roll angle.
Figure 3.8 shows the simulated yaw rate and the actual vehicle yaw rate, while Figure
3.9 is the measured sideslip generated during this maneuver with the simulated sideslip.
The small disagreements in these states are primarily due to the assumption of zero road
grade and zero longitudinal slip. The experimental run traversed from the far right lane
to right side of the left lane in this maneuver. Therefore the vehicle experienced
approximately 2? to 0? of lateral road grade. The ?2? of crown on the road impacts
sideslip significantly. A similar maneuver starting in the far left lane showed similar
error for negative sideslip angles, but the positive sideslip angles were matched correctly
44
5300 5400 5500 5600 5700
-30
-20
-10
0
10
20
30
Time (s)
Ya
w Ra
t
e
(
d
e
g
/
s
)
Actual
Simulated
Figure 3.8: Simulated yaw rate compared with measured yaw rate.
5300 5350 5400 5450 5500 5550 5600 5650
-6
-4
-2
0
2
4
Time (s)
Si
d
e
s
l
i
p
(
d
e
g
)
Actual
Simulated
Figure 3.9: Measured and simulated sideslip.
45
46
3.5. SUMMARY
A MATLAB? simulation was written which included a nonlinear vehicle model to
simulate the dynamics of the vehicle during various maneuvers. The model included
vehicle?s yaw, roll, and pitch dynamics, lateral and longitudinal effects, and a nonlinear
Dugoff tire model. There is good correlation between the simulated and actual
measurements, therefore it can be concluded the model sufficiently captures the vehicle?s
dynamics and can be used to examine the effect of sideslip on a navigation solution.
47
4. ANALYSIS OF VEHICLE DYNAMICS ON NAVIGATION
The demand for accurate and reliable navigation for ground vehicles is proliferating
as passenger vehicle control systems become more complex and as the market for
autonomous capability continues to grow. In addition to vehicle state information, a
reliance on a navigation solution will be required as vehicle stability control systems
increase their intelligence beyond the current versions of Electronic Stability Control
(ESC), Vehicle Dynamic Control (VDC) and so forth. Future intelligent and safety
vehicle systems may use this information to provide better lane keeping capability, higher
accuracy vehicle tracking, or enhanced driver assistance systems. Also, control of
AGV?s requires precise navigation information. The need for reliable navigation
information will increase as passenger vehicles continue to acquire AGV-like
capabilities. An increase in the precision of the navigation solution can directly lead to
better and more robust vehicle controllers.
Vehicle dynamicists often use models to provide detailed vehicle state information to
the driver or a control system. These models are often intricate, requiring parameters that
are hard or expensive to measure, and can be excessively complicated for navigational
purposes (Venhovens, 1999). Navigation specialists sometimes use a simplified vehicle
model based on kinematic relationships. However, these models sometimes neglect key
states such as lateral velocity present in the actual dynamics of the ground vehicle
operating at normal speeds (Dissanayake, 2001; Godha, 2005). A tradeoff exists to
48
account for the critical states sometimes neglected by navigation models while keeping
the model complexity to a minimum. This tradeoff is fundamental to the next generation
of automobile control systems and AGV navigation (He, 2002; Daily, 2004). Vehicle
model parameters are often hard to obtain or change frequently. For instance, tire
parameters are often unknown, and the owner might change values for the mass, inertias,
and CG location by loading the vehicle. Therefore, a relatively simple kinematic model
capturing similar states as a vehicle model will be more robust to those model changes
common to the everyday passenger vehicle.
The focus of this chapter is to show how vehicle slip induces errors in a navigation
solution unless accounted for both when GPS is available and when dead reckoning
through a GPS outage. Many scenarios exist where non-negligible amounts of slip are
generated. A vehicle traveling on low friction surfaces will quickly break the assumption
of no slip. Active control systems that steer the vehicle away from an impending
accident will generate large amounts of vehicle slip in order to maximize the force at the
ground to quickly alter the vehicle?s path and minimize the chance of a collision.
Similarly, an AGV suddenly maneuvering to avoid an obstacle can generate noticeable
vehicle slip. The vehicle slip generated in both of these scenarios can produce navigation
errors. A vehicle equipped with a lane keeping system traveling on an icy road might
experience difficulties because the navigation solution and map database do not agree due
to these navigation errors.
The main contribution of this section is to show lateral vehicle slip can corrupt
navigation solutions from a Kalman filter by introducing discrepancies between measured
states and integrated inputs, and therefore must be taken into account for systems
49
requiring precise vehicle navigation information. Previous work has explained the
importance of lateral vehicle slip in navigation through the use of vehicle models
(Scheding, 1999; Julier, 2003); however the work in this thesis utilizes a kinematic
navigation model to explain the consequences of neglecting slip (Travis, 2005). The
contribution of this chapter requires a measurement of vehicle heading, which was
obtained using a two antenna GPS receiver. Slip is calculated from raw measurements
rather than estimated with a vehicle model. While a multiple antenna GPS system on a
passenger vehicle might seem like a distant concept, many vehicles today already come
equipped with a single antenna system. The cost of adding a second antenna would arise
at the chipset level of the GPS board, which could be an inexpensive addition (Alban,
2004). Many commercial systems which provide improved positioning accuracy through
differential corrections, known as DGPS, are currently available such as WAAS (Wide
Area Augmentation System), Coast Guard corrections, Starfire, Omnistar, etc. In
addition, systems developed by the Federal Highway Administration (FHWA) such as
NDGPS and HA-NDGPS seek to provide high accuracy navigation for transportation
purposes (Cook, 2000; Arnold, 2001; FHWA, 2005).
To investigate the affect of vehicle slip on navigation solutions, the vehicle
simulation outlined in Chapter 3 was developed to demonstrate the corruption by slip in a
controlled environment. Data was also collected on a test vehicle to for further validation
of how the navigation accuracy can be degraded under scenarios where slip is produced.
Three navigation models were utilized to present the solution accuracy when slip was and
was not accounted for. Simulated and experimental data were analyzed when navigating
with and without GPS.
50
4.1. GENERATION OF VEHICLE SLIP
An explanation of how sideslip occurs is necessary to understand the errors that can
potentially arise in a navigation solution. The tire is the vehicle?s interface with the road
which transfers all drive forces to the ground and generating the required lateral forces to
turn the vehicle. Using the explanation presented in (Milliken, 1995), as the tire rolls,
parts of the tread either adhere to the road or slide. When a force from the vehicle is
applied to the tire/road interface, the static coefficient of friction is exceeded in local
areas on the tread. Areas where the friction coefficient is surpassed begin to slide, which
generates a force to push the vehicle. The generated force is approximated as linear,
proportional to slip until the tire saturates. When saturation occurs, the force remains
constant as no more lateral or longitudinal force can be applied to direct the vehicle. The
linear increase and saturation can be seen in the tire curve below.
-10 -5 0 5 10
-3000
-2000
-1000
0
1000
2000
3000
Tire Slip Angle (deg)
La
t
e
r
a
l
Fo
r
c
e
(
N
)
?=0.8
?=0.2
C
?
Figure 4.1: Lateral tire curve saturating on different surfaces.
The effective stiffness of the tire is used to characterize the tire, and is denoted by a
cornering stiffness (C
?
) and tractive stiffness (C
s
). These parameters and the peak tire
force are functions of numerous factors such as terrain, normal force, toe, camber, road
conditions, materials, etc, and are rarely known exactly due to the highly complex
relationship between these functions. Approximations with look up tables or empirical
models, such as Pacjeka?s Magic Model or the Dugoff Tire Model (Figure 3.4), are often
used to sufficiently characterize the tire (Pacejka, 1987; Dugoff, 1970). Ultimately, the
vehicle control forces in the linear region are a function of the tire stiffness and lateral
and/or longitudinal slip (? and s, respectively), shown by Equation 4.1.
?
?
?=
?=
CF
sCF
y
sx
4.1
51
In the above equation, s is the slip due to the difference in wheel speed and vehicle speed,
and ? is the slip due to the difference in the tire pointing direction and the direction of
travel (Gillespie, 1992). Wheel slip can be calculated using a wheel encoder and a sensor
to measure vehicle velocity, such as GPS, radar, or optical sensor. Definitions of
longitudinal and lateral tire slip are provided in Equation 4.2.
?
?
?
?
?
?
?
?
=
??
=
?
xT
yT
v
wV
V
V
V
RV
s
1
tan?
4.2
The lateral slip at each tire can be translated to the vehicle?s center of gravity (CG) to
obtain overall vehicle slip, more commonly known as vehicle sideslip (?). Sideslip is an
angle defined as the difference between the vehicle?s course (?) and its heading (?). This
difference can be seen in the figure below, which is typically referred to as a ?bicycle
schematic.?
52
Figure 4.2: Bicycle schematic showing slip angles.
Vehicle heading is the direction the vehicle is pointed, while course is the direction of the
vector created by the vehicle?s direction of travel. Both of these angles are referenced
clockwise from North. In practice, this angle is measured or estimated at the CG, then
can be translated to other locations on the vehicle for analysis or to extract further
information. In Figure 4.2, the slip angle at the CG was translated to each end of the
vehicle to provide the tire slip angles.
Model based Kalman filters have proven to be effective tools to estimate vehicle
states but often require significant knowledge about the vehicle (Anderson, 2004).
Kinematic relationships between sensors on a vehicle are often used to define state
equations in a Kalman filter because they require less model knowledge and are not
affected by to changes in the model, such as vehicle mass or tire parameters. However,
these kinematic models sometimes depict the car as if it were on rails by assuming zero
53
lateral velocity, or by assuming a proportional relationship between steer angle and yaw
rate. These assumptions are valid at low speeds for passenger vehicles as the car is
generating minute slip angles and the lateral velocity is virtually zero. The assumptions
break down at higher speeds as lateral velocities are generated that invalidate the
kinematic model, as will be shown in this chapter. When this occurs a degradation of the
performance of navigation or control systems may be observed. For these assumptions,
speed also must be considered relative to the vehicle parameters, or the terrain on which
the vehicle is operating. A small All Terrain Vehicle (ATV) with soft tires maneuvering
at 20 m/s on pavement could generate the same slip angles as a sports coupe with
performance tires traveling 35 m/s. Similarly, a car turning on dry pavement at 10 m/s
would generate very small slip angles, while the same car traveling at the same speed
would generate significantly larger slip angles if it were on a low friction surface such as
ice.
4.2. NAVIGATION CORRUPTION BY SIDESLIP
A simple but common estimator utilizes a single antenna GPS receiver and an IMU to
track vehicle position, orientation, and speed. The GPS receiver measures the vehicle?s
course, speed, and position, while the IMU measures the vehicle?s yaw rate and
longitudinal acceleration. The Kalman filter compares the integrated yaw rate gyro with
the course measurement, and the integrated accelerometer to the speed measurement, to
estimate the vehicle?s speed, orientation, longitudinal accelerometer bias, and yaw rate
gyro bias. The yaw rate gyro provides a measurement of the time derivative of heading,
r=?&
4.3
54
and course is the combination of heading and sideslip.
??? +=
4.4
Typically a heading measurement is unavailable, so the course measurement is
substituted. Under straight or low speed driving, the integrated yaw rate, or heading, and
course measurements should be similar.
???
??
+????
???=
?
?
dtr
dtr
0
0
&
&
4.5
Similarly, small sideslip angles fulfill the small angle approximation, so vehicle speed
can be approximated with an integrated longitudinal accelerometer. However, vehicle
speed gains a lateral component when sideslip is non-zero.
)sin()cos( ??
yx
xx
VVV
dtaV
+=
?=
?
v
4.6
Maneuvers producing sideslip, such as an emergency lane change, introduce a
discrepancy in the yaw rate gyro bias estimate because heading from an integrated yaw
rate gyro is compared to GPS course in the innovation in the EKF (Equation 2.11).
However, GPS course is measuring the angle of the velocity vector (heading plus
sideslip), and the heading is measuring the angle the vehicle is pointing. Further
complications arise because the longitudinal accelerometer in the IMU is measuring a
centripetal acceleration neglected in the models, and GPS velocity is measuring both
longitudinal and lateral velocity. These neglected terms can lead to significant state
estimate errors if a vehicle is generating even small sideslip angles. As will be seen in
the ensuing subsections, kinematic relationships that account for sideslip should be
55
56
included in the state equations in the Kalman filter in order to produce a more accurate
navigation solution.
Three navigation models were utilized to examine the solution accuracy when vehicle
sideslip is ignored and when it is taken into account. All algorithms use the EKF given
by Equations 2.8 to 2.12. Noise statistics for the process covariance matrix (Q) were
determined by calculating the standard deviation of experimental static sensor data.
Since the data consisted of discrete samples, it required conversion to the time domain by
a multiplication factor of the sample rate so it could be used in the EKF (Stengal, 1994).
The statistics for the measurement covariance matrix (R) were taken from data sheets
from the GPS receivers used. Biases were modeled as a random walk, and their
respective entries in the Q matrix were used to tune the EKF.
4.2.1. Typical Navigation Model
The most common states critical to a vehicle controller are speed and orientation.
Additionally, accurate position is required for autonomous operation and future stability
and control routines. A typical, computationally inexpensive navigation model on
ground vehicles provides states of speed, orientation about the z axis, position, and sensor
biases using inputs of longitudinal acceleration and yaw rate. Equation 4.7 displays a six
state kinematic navigation model including the listed states.
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
=
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
=
)?sin(
?
)?cos(
?
0
?
0
?
?
?
?
?
?
?
?
?
?
?
V
V
br
ba
E
N
b
b
V
x
r
axx
r
ax
&
&
&
&
&
&
&
4.7
The A matrix used in Equation 2.8 is computed as the Jacobian of this vector field
with respect to the states evaluated at the previous time step. Note the simplified
kinematic coupling of the vehicle states. The velocity estimate is assumed to be in the
longitudinal direction of the vehicle, while the heading estimate lumps heading and
course into one state. Measurements for this system consist of GPS velocity, course, and
position. The measurement matrix is a 4x6 matrix of zeros unless a GPS message is
received. The four entries linked to the respective states change to a value of 1 (Equation
4.8).
)North (GPS ,1)5,3(
Speed) (GPS ,1)1,1(
=
=
C
C
East) GPS( 1)6,4(
Course) (GPS ,1)3,2(
=
=
C
C
4.8
It is important to note the system is unobservable when GPS is not available, and
estimates are calculated by dead reckoning with the IMU.
4.2.2. Modified Navigation Model
A modified version of the model listed above estimates vehicle roll using a roll rate
gyro and lateral accelerometer using methods defined by (Bevly, 2004). These sensors
are common on some current passenger vehicles. An estimate of vehicle roll can be
useful in stability control systems to provide additional vehicle information. It has been
shown that vehicle roll angle is a function of CG location, not just height, and therefore
57
lateral acceleration is not always a sufficient indicator of vehicle instability (Whitehead,
2004; Travis, 2004). Inputs are longitudinal acceleration, yaw rate, and roll rate,
resulting in the state equations for this system are listed below in Equation 4.9.
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
=
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
+
=
)?sin(
?
)?cos(
?
0
?
0
?
0
?
?
?
?
??
?
?
?
?
?
?
?
??
?
?
?
V
V
b
br
ba
E
N
b
b
b
b
V
x
IMU
r
axx
ay
r
ax
ax
&
&
&
&
&
&
&
&
&
4.9
Using an additional kinematic relationship, a vehicle roll measurement can be
obtained by subtracting the centripetal acceleration from the lateral accelerometer
measurement as show in Equation 4.10.
())
?
(
1
ryM
brVa
g
??=?
4.10
Any lateral accelerometer bias will appear in this calculation; therefore the estimate is
actually a combination of roll angle and lateral accelerometer bias. Bevly also proposed
a method to separate the states by applying a low pass filter to the estimate to obtain the
bias and applying a high pass filter to obtain the change in roll angle.
The measurement matrix is a 5x8 matrix of zeros. When a GPS message is received,
the following matrix entries change (Equation 4.11).
58
East) (GPS 1)8,5(
)( ,1)5,3(
Speed) (GPS ,1)1,1(
M
=
=
=
C
C
C
?
North) (GPS ,1)7,4(
Course) (GPS ,1)3,2(
=
=
C
C
4.11
4.2.3. Expanded Navigation Model
A navigation algorithm was derived to compensate for vehicle slip using kinematic
relationships that are sometimes disregarded. Inputs are from sensors common to most
current passenger vehicles: longitudinal acceleration, lateral acceleration, yaw rate, and
roll rate. The vector field containing these kinematic equations is shown in Equation
4.12.
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
++
++
?
?
????
?+?
=
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
=
))
?
/
?
(atan?sin()
??
(
))
?
/
?
(atan?cos()
??
(
0
?
0
?
0
)
?
sin()
?
(
??
0
)
?
(
??
?
?
?
?
?
?
?
?
?
?
?
2/122
2/122
xyyx
xyyx
IMU
r
rxayy
ryaxx
r
ay
y
ax
x
VVVV
VVVV
b
br
gbrVba
brVba
E
N
b
b
b
V
b
V
x
?
?
?
?
?
?
?
?
&
&
&
&
&
&
&
&
&
&
&
&
4.12
59
The obvious difference between this modified model and the previously presented
models is the velocity estimate is now broken up into longitudinal and lateral
components, and additional centripetal acceleration terms are included. The main
assumption of this model is there is no vertical velocity. This assumption is valid for this
study because the vehicle was constrained to a level surface in simulation and in
experimental tests. However, in scenarios where vertical velocities will be encountered
(hills, banked turns, etc), more centripetal acceleration effects will appear in these
equations and should be included in the navigation model to improve the estimation
accuracy. GPS can provide the necessary measurements of vertical velocity, and pitch
with a three antenna system, to make the navigation model observable.
Measurements for this system are longitudinal and lateral velocity, heading (not
course), roll, and position. Currently, many of these measurements are not available on
passenger vehicles but are readily accessible on many AGV platforms. However,
obtaining these measurements on a highway vehicle is not a distant concept as GPS
technology continues to become more prevalent in automotive systems. Sideslip has
been effectively measured using a yaw rate gyro and GPS, which is currently on many
vehicles (Bevly, 2000). This study obtained a sideslip calculation from a two antenna
GPS receiver. If sideslip is known, the GPS velocity measurement can be separated into
longitudinal and lateral components for use in the state equations given in Equation 4.12.
Discussion on how sideslip was obtained for this research is provided in Appendix B.
Using the listed measurements, the C matrix is a 6x10 matrix of zeros unless a GPS
measurement is received. In that instance, the measurement matrix is as follows:
Heading) (GPS ,1)5,3(
Speed) Lateral (GPS ,1)3,2(
Speed) alLongitudin (GPS ,1)1,1(
=
=
=
C
C
C
East) (GPS 1)10,6(
North) (GPS ,1)9,5(
Roll) (GPS ,1)7,4(
=
=
=
C
C
C
4.13
Also, an effective sideslip estimate can be calculated using V
x
and V
y
as follows:
?
?
?
?
?
?
?
?
=
?
x
y
V
V
1
tan?
4.14
60
The process and measurement covariance matrices for the three models shared
variances on common states. Also, the parameters did not change when using the models
with simulated or experimental data, since the sensor simulation was tuned to match the
actual sensors on the test bed. The values are listed in the tables below.
Table 4.1: Process Noise Variances for Sideslip Corruption Investigation
dt
ax
22
2.0=?
42
1
?
= e
bax
? dt
ay
22
5.0=?
42
1
?
= e
bay
?
dt
r
22
05.0=?
62
1
?
= e
br
? dt
22
0107.0=
?
?
62
1
?
= e
b?
?
62
1
?
= e
N
?
62
1
?
= e
E
?
Table 4.2: Measurement Noise Variances for Sideslip Corruption Investigation
22
5.0=
V
?
2
2
?
?
?
?
?
?
=
V
V
?
?
?
22
00174.0=
?
?
22
00698.0=
?
?
22
03.0=
M?
?
22
1.0=
N
?
22
1.0=
E
?
4.2.4. Simulation Results of Sideslip Corruption
A step lane change maneuver was input into the simulation discussed in Chapter 3 to
excite the yaw dynamics. In the following plots, the numbers one, two, and three refer to
the typical navigation model, modified model, and expanded model, respectively, listed
in Section 4.2.1 to 4.2.3. The output of models 1 and 2 are often similar because of the
similarity of their kinematic models (both neglect sideslip).
61
As seen in Figure 4.3, the heading estimates from models 1 and 2 lose accuracy when
slip is generated. The estimate follows the heading measurement from the integrated yaw
gyro until a GPS course measurement is received. The residual between the estimate and
measurement becomes large because the model does not account for sideslip. This error
is then multiplied by the EKF gain, and added to the time update to minimize the error
between the estimate and the course measurement. This process occurs in Equation 2.11.
Another casualty of this phenomenon is the yaw gyro bias estimate. Figure 4.4 shows
error due to sideslip being injected in the yaw gyro bias estimate. However, when
sideslip is taken into account by model 3, the severity of inaccuracies in the gyro bias
estimate is reduced up to 1.5 deg/s for this maneuver. Additionally, the heading estimate
correctly follows the true vehicle heading.
33 33.5 34 34.5 35 35.5 36 36.5 37
0
5
10
15
Time (s)
Ya
w E
s
t
i
ma
t
e
(
d
e
g
)
1&2
3
True Course
True Heading
Figure 4.3: Heading estimates of the different navigation models from simulated data.
62
10 15 20 25 30 35 40 45 50
-0.5
0
0.5
1
1.5
2
Time (s)
Ya
w Ra
t
e
Gy
r
o
Bi
a
s
(
d
e
g
/
s
)
1&2
3
True b
r
Figure 4.4: Yaw gyro bias estimates from simulated data.
Comparable evidence is seen in the velocity and accelerometer bias estimates. When
lateral velocity is near zero, the three models produce similar estimates. When slip is
introduced, the longitudinal estimate using models 1 and 2 diverge from the estimates
using 3. This is due to the fact that models 1 and 2 neglect the centripetal acceleration
caused by lateral velocity and yaw rate. Figure 4.5 shows the error in the velocity
estimate caused by the models that neglect centripetal acceleration. Figure 4.6 displays
the inaccuracy in the longitudinal accelerometer bias estimate.
63
15 20 25 30 35 40 45
19.95
20
20.05
20.1
20.15
Time (s)
S
p
eed
E
s
t
i
m
at
e (m
/
s
)
1&2
3
True
Figure 4.5: Jump in velocity estimate due to neglected terms.
20 25 30 35 40 45
-0.2
0
0.2
0.4
0.6
0.8
Time (s)
A
ccel
er
o
m
et
er B
i
as (m
/
s
2
)
1&2
3
ax
3
ay
True b
ax
True b
ay
Figure 4.6: Corrupted longitudinal accelerometer bias estimate.
64
Although roll is not directly influence by sideslip, knowledge of this state is useful to
vehicle controllers and could be used in algorithms to calculate other vehicle states.
Figure 4.7 displays the roll estimate from models 2 and 3. The estimate from model 2
contains the lateral accelerometer bias from the measurement. However, model 3
correctly captures the roll angle with the use of the dual antenna GPS system. Note that a
third antenna could be added to estimate pitch in a similar fashion.
20 25 30 35 40 45 50
-3
-2
-1
0
1
2
3
4
5
Time (s)
R
o
ll (d
e
g
)
2
3
True
Figure 4.7: Roll estimates from the different navigation models.
Although the sideslip effects initially appear small, the loss of GPS during this period
would devastate the estimate accuracy. There are two primary sources of error in models
1 and 2 during a GPS outage. Without GPS, the estimator integrates yaw rate to obtain a
course measurement. However, this estimate is actually heading. Since the position
estimates are a function of vehicle velocity and course, error will be introduced by using
heading in lieu of course. Additionally, if the outage occurs while the vehicle is slipping,
65
the yaw gyro bias estimate will be incorrect and lead to a large position error. Model 3
overcomes the shortcomings of models 1 and 2 by continuing to estimate longitudinal
and lateral velocity, which provides an effective sideslip estimate. The heading estimate
from the integrated yaw gyro is added to the sideslip estimate, and used to estimate north
and east position more accurately. Models 1 and 2 use only heading to estimate north
and east position, which is incorrect when sideslip is present because north and east
position are dependent upon direction of travel (course). Also, Figure 4.8 displays
position error during both circumstances. The total error was calculated from the norm of
east and north error as shown below, where True denotes the true value of the state and
Est denotes the estimated value of the state.
22
)()(
EstTrueEstTrueposition
EENNe ?+?= 4.15
Using the same set of data as Figure 4.3 through Figure 4.7, a GPS outage was simulated
before the maneuver was executed at 31 seconds (dashed line) and at 32.4 seconds when
the vehicle was generating sideslip (dotted line). The models neglecting sideslip effects
produced substantially larger errors when the outage occurred during the maneuver.
Their accuracy was also poorer than model 3 when the outage occurred before the
maneuver because vehicle heading, not course, was used to estimate north and east
position when slip was generated.
66
0 10 20 30 40 50
0
10
20
30
40
50
60
70
Time (s)
P
o
sitio
n
E
r
ro
r
(m
)
1&2 - Outage During Slip
1&2 - Outage Before Slip
3 - Outage During Slip
3 - Outage Before Slip
Figure 4.8: Position error during an outage.
Model 3 also displays some ability to predict sideslip during a GPS outage for a short
period of time by integrating the lateral and longitudinal accelerometers. The model used
the sideslip measurement from the dual antenna GPS receiver to estimate lateral and
longitudinal velocity and correct inertial errors. This sideslip estimate would be of use to
stability control systems in many vehicles. Figure 4.9 shows the sideslip estimate during
the GPS outage using model 3. The growing error in the above figure was due to the bias
drift in the accelerometers.
67
31 32 33 34 35 36 37 38
-6
-4
-2
0
2
4
Time (s)
Si
d
e
s
l
i
p
(
d
e
g
)
During Maneuver
Before Maneuver
True
Figure 4.9: Estimated sideslip during an outage.
4.2.5. Experimental Results of Sideslip Corruption
Slip was generated experimentally to show the estimator accuracy using the various
navigation models. An emergency lane change maneuver was executed on a straight
section of the aforementioned test track. As in Section 0, models 1 and 2 refer to the
typical and modified model, and model 3 refers to the expanded model. It should be
noted the process noise statistics in Q can change during an experimental run as different
levels of vibration are experienced throughout the vehicle. This matrix was initially set
and not revisited. In simulation, this is sufficient because the process noise is constant.
However in the real world, errors can be produced due to a change in the process noise
model. These changes are hard to detect, therefore the process noise matrix is often left
constant which leads to a sub-optimal filter.
68
Figure 4.10 shows the decrease in heading accuracy with GPS during the maneuver
using models 1 and 2. Conversely, the estimator using model 3 correctly tracks the
vehicle heading. This is similar to the simulation results shown in Figure 4.3. Notice the
increase in the level of noise caused by an incorrect yaw gyro bias estimate from models
1 and 2. The Kalman filter could be tuned to decrease the severity of heading error by
adjusting the appropriate bias parameters in the process noise covariance matrix.
However, this might lead to an inability to accurately track the sensor bias, which would
impact the dead reckoning solution.
197.5 198 198.5 199 199.5
-95
-90
-85
-80
-75
Time (s)
Ya
w Es
t
i
ma
t
e
(
d
e
g
)
1&2
3
True Course
True Heading
Figure 4.10: Heading estimates of the navigation models with experimental data.
A GPS outage was artificially simulated at 118 seconds in Figure 4.11. During this
period of time, sideslip was generated. Therefore this section of data was chosen to
display the errors generated by the assumption of zero lateral velocity. The position error
was calculated using Equation 4.15. Estimates from experimental data display similar
69
results as the estimates from the simulated data. Position error grows during the outage,
but the model incorporating slip experiences a smaller error growth rate.
0 20 40 60 80 100 120
0
20
40
60
80
100
120
Time (s)
P
o
sitio
n
E
r
ro
r
(m
)
1&2
3
Figure 4.11: Position error during an artificial GPS outage.
Figure 4.12 provides insight as to a difference exists between the navigation models.
When GPS is removed at 118 seconds, the yaw gyro bias is incorrectly estimated using
models 1 and 2, which leads to an incorrect heading estimate. Model 3 captures the
motion of the vehicle and provides a more accurate heading estimate for a short period of
time during the outage.
70
116 117 118 119 120 121 122 123
75
80
85
90
95
Time (s)
Ya
w Es
t
i
ma
t
e
(
d
e
g
)
1&2
3
True Course
True Heading
Figure 4.12: Heading estimates during a GPS outage.
4.3. NAVIGATION CORRUPTION BY LONGITUDINAL SLIP
Wheel speed sensors are perhaps the most common velocity sensor on ground
vehicles. Their low cost, small size and ease of use are beneficial to manufacturers, and
they are proven navigation instruments (Hay, 2005). However, an inherent flaw with a
wheel speed sensor is that it measures wheel speed, not vehicle speed. When longitudinal
slip is generated, these two measurements can drastically differ. Longitudinal wheel slip
is defined by the speed difference between the vehicle and the tire, and is given by
Equation 4.16.
Vehicle
VehicleTire
V
VV
s
?
=
4.16
Large engine torques can yield a high wheel speed measurement that differs from the
vehicle?s speed over ground. Similarly, a hard braking maneuver could lock up a tire.
71
72
The slip is dependent upon tire characteristics, as explained in Section 4.1 and with
Equation 4.1. The wheel speed sensor would then report zero velocity, but the vehicle
might be traveling with a high rate of speed. Terrain only magnifies the difference
between wheel speed and vehicle speed. A small slip percentage is typically generated
by the drive torque at the tire. On ice, the same drive torque could produce a
considerable percentage of wheel slip. In order to mitigate the effect of wheel slip, the
wheel speed sensor is often placed on an un-driven wheel.
To effectively use a wheel speed measurement for navigation, it must be translated
from the tire to the vehicle?s center of gravity. If a wheel speed sensor on a front tire is
used, an additional translation from the tire frame to the vehicle frame must be
completed. As a vehicle turns, a velocity difference between inner and outer tires is
introduced. Also, if sideslip is present, the wheel speed sensors on the tires cannot
capture the lateral velocity. Figure 4.13 shows a front left tire at a given steer angle (?),
along with distances to the CG.
Figure 4.13: Tire coordinate frame.
Using Equation 4.17 the wheel velocity can be moved to the CG with knowledge of the
yaw rate.
TCGTCG
RrVV
/
?+=
vv
4.17
First, the wheel velocity should be separated into x and y components and translated to
the vehicle coordinate frame.
[ ] [ ]
()
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
??+
++?=
y
f
x
y
xLFyFL
x
yFLxFLFLCG
t
ar
VVVVV
v
v
vv
v
2
)sin()cos()sin()cos(
/
????
4.18
Simplifying Equation 4.18 yields the complete velocity translation from the front left tire
to the vehicle?s CG.
[]
y
xLFyFL
x
f
yFLxFLFLCG
arVV
r
t
VVV
v
v
v
+++
?
?
?
?
?
?
+?=
)sin()cos(
2
)sin()cos(
/
??
??
4.19
Following a similar procedure, the velocity at any tire can be moved to the CG. Equation
4.20 shows the transformation from the front right tire.
73
[]
y
xLFyFL
x
f
yFLxFLFRCG
arVV
r
t
VVV
v
v
v
+++
?
?
?
?
?
?
??=
)sin()cos(
2
)sin()cos(
/
??
??
4.20
In most circumstances rear tires do not require a tire-to-vehicle frame transformation.
Therefore a lateral wheel velocity has no influence on the longitudinal vehicle velocity.
Therefore, the velocity from the back left tire to the CG is as follows:
[]
y
yBL
x
r
xBLBLCG
brVr
t
VV
v
v
v
?+
?
?
?
?
?
?
+=
2
/
4.21
Finally, the transformation from the rear right tire to the CG is calculated.
[]
y
yBR
x
r
xBRBRCG
brVr
t
VV
v
v
v
?+
?
?
?
?
?
?
?=
2
/
4.22
4.3.1. Navigation Model with Wheel Speed
The navigation system is simplified in this section by estimating states only
pertaining to vehicle velocity. Therefore, the system nonlinearities induced by the
position estimate are eliminated and a simpler, discrete linear Kalman filter (LKF) is
used. The guaranteed optimality provided by a linear Kalman filter is a welcomed
benefit.
The LKF is similar in design to the EKF but the equations are slightly different
because there are no nonlinear approximations. For more information, see Gelb (1974).
The equations for an LKF are listed below.
State Estimate Extrapolation
1111
??
??
+
??
?
+=
kkkkk
uBxAx
4.23
Covariance Estimate Extrapolation
1111 ??
+
??
?
+=
k
T
kkkk
QAPAP
4.24
74
Gain Calculation
()
1?
??
+=
k
T
kkk
T
kkk
RCPCCPL 4.25
State Update
( )
??+
?+=
kkkkkk
xCyLxx ???
4.26
Covariance Update
()
1?
?+
+=
kk
T
kkk
CRCPP 4.27
Another simplification in this section is the use of a rear wheel speed measurement,
as opposed to a front wheel speed measurement. This eliminates the lateral wheel
velocity contribution to the longitudinal vehicle velocity. Also, tests are run in a manner
to minimize sideslip so the lateral components can be neglected. After these
simplifications, the velocity transformation from the left rear tire to the vehicle?s CG is
given by Equation 4.28.
r
t
VV
r
xBLBLCG
2
/
+=
v
4.28
The navigation model to estimate longitudinal vehicle speed and longitudinal
accelerometer bias is given in Equation 4.29.
?
?
?
?
?
?
?
=
?
?
?
?
?
?
?
?
=
0
?
?
?
?
axx
ax
x
ba
b
V
x
&
&
&
4.29
The sole input is longitudinal acceleration. This leads to the continuous state transition
matrix, A
c
, and continuous input matrix, B
c
, below.
?
?
?
?
?
?
=
?
?
?
?
?
? ?
=
0
1
,
00
10
cc
BA
4.30
Measurements of GPS velocity and wheel speed translated to the CG yield the
measurement vector and measurement matrix given by Equation 4.31.
75
?
?
?
?
?
?
=
?
?
?
?
?
?
=
01
01
,
/
c
BLCG
GPS
C
V
V
y
4.31
Note that the continuous system matrices must be discretized before used in the LKF.
The addition of GPS velocity allows a wheel speed bias estimate to be observable.
Addition of a third state is required to estimate wheel speed bias in the above system.
?
?
?
?
?
?
?
?
?
?
?
=
?
?
?
?
?
?
?
?
?
?
?
?
?
?
=
0
0
?
?
?
?
?
axx
ws
ax
x
ba
b
b
V
x
&
&
&
&
4.32
The state matrices when estimating a wheel speed bias are defined as follows:
?
?
?
?
?
?
=
?
?
?
?
?
?
?
?
?
?
=
?
?
?
?
?
?
?
?
?
? ?
=
101
001
,
0
0
1
,
000
000
010
ccc
CBA
4.33
Note the same input and measurements are used for both models.
4.3.2. Navigation Model with Wheel Speed and Radar
Doppler radar is a common speed sensor which can be utilized on ground vehicles. It
is helpful on some terrains because it is insensitive to wheel slip. The radar bias however
may be sensitive to changes in the sensor?s pointing angle and can be terrain dependent.
Therefore a bias associated with the radar should be estimated to provide an accurate
velocity estimate.
The navigation model used is similar to the model estimating wheel slip, with an
additional state to estimate the Doppler radar bias.
76
?
?
?
?
?
?
?
?
?
?
?
?
=
?
?
?
?
?
?
?
?
?
?
?
? ?
=
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
=
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
=
0
0
0
1
,
0000
0000
0000
0010
,
0
0
0
?
?
?
?
?
?
cc
axx
dop
ws
ax
x
BA
ba
b
b
b
V
x
&
&
&
&
&
4.34
The measurement vector is altered by the addition of the Doppler radar velocity
measurement. It is listed with the measurement matrix in Equation 4.35.
?
?
?
?
?
?
?
?
?
?
=
?
?
?
?
?
?
?
?
?
?
=
1001
0101
0001
,
/ c
dop
BLCG
GPS
C
V
V
V
y
4.35
4.3.3. Experimental Results of Longitudinal Slip Corruption
The Infiniti G35 was driven in a straight line on a gravel surface to show wheel slip in
the longitudinal direction can be devastating to the accuracy of the navigation solution
when using a wheel speed sensor to provide a redundant measurement of velocity.
Figure 4.14 displays the corrupted navigation solution when wheel slip is not accounted
for in the estimator while GPS is available. At 4.5 seconds there is a 1 m/s error in the
velocity estimate due to wheel slip. Recall that wheel slip is related to applied drive force
at the wheel, and therefore related to acceleration.
77
0 2 4 6 8 10 12 14 16
0
2
4
6
8
10
12
Time (s)
V
e
lo
c
ity
(m
/s
)
Wheel Speed
GPS
Estimate
Figure 4.14: Velocity estimate corrupted by wheel slip.
Estimating the wheel speed bias effectively lessens the velocity estimate error when
GPS is available, as shown by Figure 4.15. The longitudinal slip is captured in the bias
estimate if the bias is modeled with high enough dynamics, along with any radial
displacements caused by flexing of the sidewall as the tire is loaded during operation.
78
0 2 4 6 8 10 12 14 16
0
2
4
6
8
10
12
Time (s)
V
e
lo
c
ity
(m
/s
)
Wheel Speed
GPS
Estimate
Figure 4.15: Estimate with wheel speed bias removed.
When GPS is lost the bias estimate becomes unobservable and remains constant.
Figure 4.16 shows the same data given in Figure 4.14. In this figure, a GPS outage is
simulated at t=4 seconds. The effect of the bias remaining constant can be seen.
79
0 5 10 15 20
-0.4
-0.2
0
0.2
0.4
0.6
0.8
1
1.2
1.4
Time (s)
W
h
ee
l
S
p
eed B
i
a
s
(m
/
s
)
Figure 4.16: Estimated wheel speed bias before and during a GPS outage.
Because the wheel speed bias is unobservable when GPS measurements are unavailable,
any changes in the bias cannot be compensated. The dead reckoned navigation solution
is corrupted by changes in slip or wheel radius during a GPS outage. Figure 4.17 shows
the error in the velocity estimate is proportional to the constant bias offset.
80
0 2 4 6 8 10 12 14 16
0
2
4
6
8
10
12
Time (s)
V
e
lo
c
ity
(m
/s
)
Wheel Speed
GPS
Estimate
Figure 4.17: Velocity estimate using wheel speed and bias estimate during an outage.
The Doppler radar velocity measurement was used to correct the inadequacies of the
wheel speed measurement using the estimator model described in Equations 4.34 and
4.35. Figure 4.18 shows the same data with a velocity estimate using wheel speed and
radar measurements. It initially appears the radar measurement is the solution to
accurately measuring ground vehicle speed. The corruption due to longitudinal wheel
slip is subdued resulting in a higher precision velocity estimate. However, the radar bias
soon degrades the estimate as well, which is seen below.
81
0 2 4 6 8 10 12 14 16
0
2
4
6
8
10
12
Time (s)
V
e
lo
c
ity
(m
/s
)
Radar
Wheel Speed
GPS
Estimate
Figure 4.18: Estimated vehicle velocity with Doppler radar.
The radar bias is not constant enough for it to be considered an ideal measurement. The
speed measurement is a function of the sensor?s distance and angle relative to the ground.
Therefore, the Doppler radar output is affected when the vehicle rolls or pitches due to
acceleration or road grade because the distance and angle of the unit changes relative to
the ground. Surface conditions also impact the Doppler radar output because the height
and texture of different terrains alter the reflectivity of the microwave signal. Figure 4.19
is a plot showing the radar bias on different terrain.
82
Figure 4.19: Doppler radar bias on different terrain.
4.4. SUMMARY
A demonstration of navigational accuracy was performed utilizing a nonlinear vehicle
simulation to provide a controlled environment and experimental data to emulate an
actual scenario. The accuracy of the navigation estimates was improved with and without
GPS by accounting for the lateral velocity generated when a vehicle turns. The model
developed to include sideslip effects tracked the vehicle?s velocity and heading more
accurately than traditional models that typically neglect the lateral dynamics. The model
also better compensated the inherent sensor biases which directly lead to a dead
reckoning solution with less error.
In the longitudinal direction, scenarios were presented that expose the shortcomings
of typical dead reckoning sensors. A wheel speed sensor can aid in estimating velocity
83
84
when the wheel slip is taken into account, and can handle GPS outages well if slip
remains constant. When slip varies, the bias estimate becomes corrupted and increases
the position error. Similarly, a Doppler radar can provide adequate ground speed
measurements during a GPS outage unless the terrain on which the vehicle is traveling
changes. The radar speed bias is a function of terrain, and is not observable during an
outage.
85
5. INS AIDING WITH A LIDAR
Autonomous capability and vehicle control systems require reliable and robust
navigation solutions in multiple environments. GPS has become an effective tool but is
not suitable for all environments. Satellites are not always in direct view of the GPS
receiver, which is hindering the development of reliable AGVs. Environmental
constraints can either completely block the signal, or degrade the signal to the point
where it is of no use. Jamming can cause similar issues in combat environments, where
the opposition can cheaply and effectively knock out the GPS signals or broadcast
incorrect messages. If the GPS signal is suddenly lost, jammed, or known to be incorrect,
some critical system states become unobservable without additional sensors. In these
circumstances, the navigation solution generally relies on integrating noisy, biased IMU
measurements, affecting navigation accuracy and control capabilities.
Laser scanners are quickly making their presence known in the navigation field and
are proven to have a variety of uses (Lei, 2005). In this chapter, a light detection and
ranging device (Lidar) is used in conjunction with data from a MEMS based IMU to
formulate a suitable navigation solution (Travis, 2005). Lidar is has been used as a
navigation tool on both aerial (Campbell, 2003) and ground vehicles (Hirokawa, 2004;
Talaya, 2004). Specifically, this chapter examines the operation of a ground vehicle in an
indoor corridor environment. The use of a Lidar compensates the lack of GPS by
providing the following measurements of critical vehicle states: heading, velocity, and
lateral error (Hirokawa, 2004). This type of navigation could be extended to many
environments, such as an urban canyon, forest, indoor corridor, or in jammed
environments.
5.1. TEST APPARATUS
5.1.1. Experimental Setup
A cart was outfitted with an IMU and Lidar. The cart had four independent wheels
that were free to rotate about the x and z axes of a vehicle coordinate frame (x being
forward, z being down). It was manually pushed down a corridor at different velocities
while data was logged from the IMU and Lidar. The absence of an engine or motor to
translate the cart reduced the amount of vibration that the sensitive IMU would measure.
The majority of the noise stemmed from a fan on a power supply and irregularities along
the ground where the cart was rolled. Data from each run was passed through a Kalman
filter, post process, to develop estimates of velocity, heading, and lateral error.
5.1.2. IMU
The IMU used in this analysis was a Crossbow IMU400CC-100, capable of providing
3 axis acceleration and angular velocity measurements. The IMU utilizes MEMS based
accelerometers and rate gyroscopes, which have inherent walking biases and scale factor
errors. The Crossbow output was recorded at 100Hz. Two scenarios are covered in this
chapter and require two different input matrices. The first input set contains longitudinal
acceleration and yaw rate as given in Equation 5.1,
?
?
?
?
?
?
=
r
a
u
x
1
5.1
and the second input set adds lateral acceleration as shown in Equation 5.2.
86
?
?
?
?
?
?
?
?
?
?
=
y
x
a
r
a
u
2
5.2
5.1.3. Lidar
Lidar is used to ?visually? access the environment and extract information, to update
the IMU data. The SICK LMS 221 Lidar projects infrared laser pulses into the local
environment and records their ?time of flight? and reflected intensity at 180 scanning
points over the range 0? to 180? at a rate of 13.32 milliseconds per scan. The scanning
points and the associated distances give a 2D image of the environment in a polar
coordinate position and distance at each time step (?
k
, d
?k
). Using these measurements, a
local heading, velocity, and lateral position from a defined line, y
lat
, are found. The
measurement vector is as follows:
?
?
?
?
?
?
?
?
?
?
=
m
lat
m
m
m
y
V
y ?
5.3
The Lidar?s heading estimate ?
m
is found as a function of the center of the hallway. A
hallway?s center can be estimated by halving the distance between the corners of the
hallway. Each corner in a hallway has the unique distinction of an edge. This is due to
the fact the change in distance for a given set of data points (?, d
?
) and (? +??, d
? +??
)
located at a corner is large, as shown in Figure 5.1. These corners are determined as
spikes in the derivative of the scanned points.
87
Figure 5.1: Corner detection with a Lidar.
Once the center of the hallway is determined, the ?
m
estimate is determined as the angle
between the 90? centerline of the Lidar and the center of the hallway as seen in Figure
5.2. Additionally, the lateral position of the Lidar is found as a distance from the
hallway?s center. Two distances
i
d
,?
are determined at ? = ?60? angles from the center
of the hallway. These distances are used to determine the lateral distance from the Lidar
to the edge of the wall y
lat,i
.
88
Figure 5.2: Calculating lateral offset.
All of the Lidar?s data was run through a 10 point moving average filter to minimize
the effect of any spikes occurring in the measurement from the detection of a false
obstacle. The tests were run in a fairly controlled environment with smooth corridors,
minus occasional doorways and small, miscellaneous objects. Other types of corridors,
such as rows of trees lining a road, will be less consistent and generate more data spikes.
The moving average filter aids in smoothing out the corridors and provides the Kalman
filter a more discernible measurement.
5.2. NAVIGATION MODELS
To thoroughly investigate the capabilities of utilizing Lidar for vehicle navigation,
two kinematic models were derived. The first mathematical analysis assumes zero
vehicle sideslip, so the discrepancy between vehicle heading and course is due only to
error and not to a lateral velocity. The following five states are estimated in the first
89
scenario: velocity, longitudinal accelerometer bias, heading, yaw gyro bias, and lateral
error. Inputs to the system from the IMU are stated in u
1
of Equation 5.1 and
measurements are provided by the Lidar as shown in Equation 5.3. Note that the
component for lateral error, , is a lateral velocity in the environmental coordinate
frame, not the vehicle coordinate frame. There is zero lateral velocity in the vehicle
coordinate system due to the zero sideslip assumption. Therefore, the state derivative
vector is defined by the following:
x&
lat
y&
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
=
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
=
)?sin(
?
0
?
0
?
?
?
?
?
?
?
1
?
?
x
r
axx
lat
r
ax
x
V
br
ba
y
b
b
V
x
&
&
&
&
&
&
5.4
The Jacobian of this model yields the A matrix used in Equation 2.8.
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
=
00)?cos(
?
0)?sin(
00000
01000
00000
00010
?? V
A
5.5
The measurement matrix corresponding to the measurements presented in Equation 5.3 is
as follows:
?
?
?
?
?
?
?
?
?
?
=
10000
00100
00001
C
5.6
In some instances, the sideslip can be used as an analysis tool or as a focal point of
control and merits further investigation. Therefore, a second kinematic model was
90
developed that acknowledges that the vehicle can translate laterally within its own
coordinate frame, creating an additional lateral acceleration component. This model uses
the same measurements from the Lidar but couples them with the inputs from u
2
in
Equation 5.2. For this scenario, the sixth estimated state is lateral velocity in the vehicle
frame, as shown below.
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
+
?
?
=
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
=
y
yx
r
axx
y
lat
r
ax
x
a
VV
br
ba
V
y
b
b
V
x
)?cos(
?
)?sin(
?
0
?
0
?
?
?
?
?
?
?
?
2
??
?
&
&
&
&
&
&
&
5.7
The continuous state matrix is the Jacobian of the nonlinear state derivative vector.
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
=
000000
)?cos(00)?sin(
?
)?cos(
?
0)?sin(
000000
001000
000000
000010
????
yx
VV
A
5.8
Equation 5.9 displays the measurement matrix when all measurements are available.
?
?
?
?
?
?
?
?
?
?
=
010000
000100
000001
C
5.9
91
Input variances were obtained by recording static data, and were then placed along
the diagonal of the process covariance matrix (Q). Biases are assumed to be constant in
the system model, but they were modeled in the process covariance matrix to prevent the
Kalman filter gains from going to zero. If this were to happen, the bias states would
remain constant when they change in reality. The first scenario used a process
covariance matrix of the form shown in Equation 5.10.
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
=
2
2
2
2
2
0000
0000
0000
0000
0000
ylat
br
r
bax
ax
Q
?
?
?
?
?
5.10
The process noise covariance matrix for the second scenario adds a variance for the
lateral velocity state and is shown below.
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
?
=
2
2
2
2
2
2
00000
00000
00000
00000
00000
00000
ay
ylat
br
r
bax
ax
Q
?
?
?
?
?
?
5.11
The process noise and measurement noise statistics between the two models were
shared. The values used are listed in the tables below.
Table 5.1: Process Variance Values for Lidar/INS Aiding
dt
ax
22
032.0=?
42
1
?
= e
bax
? dt
r
22
0029.0=?
62
1
?
= e
br
? dtdt
ylat
222
1.0 ,01.0 ,0=? dt
ay
22
0534.0=?
Table 5.2: Measurement Variance Values for Lidar/INS Aiding
22
028.0=
V
?
22
001.0=
?
?
22
1
02.0=
ylat
?
92
Total lateral movement is a function of V
x
, V
y
, and ?. In the first scenario neglecting
sideslip, the standard deviation in the lateral error is already accounted for in the ?
ax
and
?
r
terms in the Q matrix. In theory, ?
ylat
, should be set to zero to refrain from injecting
unnecessary process noise into the system. This parameter, ?
ylat
, essentially can become a
tuning knob to attempt to estimate unmodeled disturbances. The process noise on the
lateral error state is non-Gaussian because of unmodeled dynamics, which technically
makes the Kalman filter sub-optimal. However, but the effects are minimal in many
situations including the one investigated in this chapter. In the second scenario, ?
ax
and
?
r
again account for some of the process noise in y
lat
, but there is additional process noise
due to the lateral acceleration measurement incorporated into the system. As it turns out,
accounting for the process noise when estimating V
y
automatically injects the process
noise into the lateral error state. Because , the disturbances modeled with
?
)
?
,
?
(?
yxlat
VVfy =
ay
are included in the lateral error estimate through the lateral velocity estimate.
Therefore the process noise on the lateral error state, ?
ylat
, may remain zero. Together,
these statistics form the continuous process covariance matrix for both scenarios.
Measurement noise statistics were calculated by averaging multiple static data sets
taken along the testing path. For the Lidar, noise is sufficiently estimated by examining a
static, or stationary, set of data at the beginning, middle, and end of the hall. For a more
thorough examination of the noise, techniques in detection and estimation could be used
to determine more appropriate noise characteristics (Adams, 2000).
The lateral accelerometer measurement was assumed to be unbiased for the second
scenario. The mean of the lateral acceleration data was removed before input into the
93
Kalman filter. With a measurement of lateral distance to the corridor in the vehicle frame
(shown in Figure 5.3), lateral velocity and lateral accelerometer bias is observable.
y
dr
, 0?
y
dl
, 0?
y
lat1
y
lat2
?
1
??
2
?
Figure 5.3: Lateral distance measurement.
This lateral distance measurement could be incorporated into the proposed algorithm
from Section 4.2.3 to more accurately navigate with or without GPS. To do so, the
following model must be incorporated:
?
?
?
?
?
?
?
?
?
?
?
?
=
?
?
?
?
?
?
?
?
?
?
?
?
?
=
V
ay
V
V
ayy
y
b
y
x
y
ba
x
?
?
?
,
?
0
?
?
&
&
&
5.12
The Jacobian of this model is given in Equation 5.13.
?
?
?
?
?
?
?
?
?
? ?
=
001
000
010
A
5.13
The measurement vector and measurement matrix are defined as follows:
94
95
][] [ 100 ,
1
== Cyy
d
This lateral distance measurement was not available at the time of this study. However,
the proposed method could be used to determine vehicle lateral velocity and sideslip if
the proper measurements are provided. Typically a Lidar can offer this measurement if
set up to do so.
5.3. EXPERIMENTAL RESULTS
5.3.1. Scenario 1 ? Five State Estimator
As stated earlier, scenario one investigates the use of Lidar in vehicle navigation
when the vehicle?s lateral velocity component is assumed to be zero. As shown in Figure
5.4 and Figure 5.5, the Kalman filter algorithm using the Lidar measurements accurately
estimates velocity and heading states.
0 5 10 15 20
-0.5
0
0.5
1
1.5
2
Time (s)
V
e
lo
c
i
t
y
(
m
/s
)
Lidar
?a
x
KF
Figure 5.4: Velocity estimate using a Lidar.
0 5 10 15 20
-2
-1.5
-1
-0.5
0
0.5
1
Time (s)
He
a
d
i
n
g
(
d
e
g
)
Lidar
?r
KF
Figure 5.5: Heading estimate using a Lidar.
96
The heading estimate in Figure 5.5 captures the high dynamics as measured by the IMU
during short durations, but follows the Lidar measurement long term to remove the effect
caused by biases in the system.
Lateral error in Figure 5.6 quickly tracks an initial offset of 0.25 meters, but is slow to
respond to other lateral changes. This is due to over filtering because the lateral error
covariance was set to zero. Subsequent results will show the level of filtering can be
adjusted to improve the estimate by changing the lateral error covariance value or by
including the lateral vehicle dynamics.
0 5 10 15 20
-0.7
-0.6
-0.5
-0.4
-0.3
-0.2
-0.1
0
0.1
Time (s)
L
a
te
r
a
l E
r
r
o
r
,
y
la
t
(m)
Lidar
KF
Figure 5.6: Lateral error estimated using an IMU and Lidar.
Figure 5.7 and Figure 5.8 show the longitudinal accelerometer bias and yaw rate gyro
bias, respectively. As seen in the figures, the biases are estimated accurately as they
jump to an initial offset then vary appropriately to capture the slow bias drift.
97
0 5 10 15 20
-1.5
-1
-0.5
0
0.5
1
1.5
2
2.5
L
o
n
g
i
t
u
d
in
a
l
A
c
c
e
le
r
o
m
e
te
r
B
i
a
s
(
m
/s
2
)
Time (s)
Figure 5.7: Estimated longitudinal accelerometer bias.
0 5 10 15 20
-0.15
-0.1
-0.05
0
0.05
0.1
Y
a
w Gy
r
o
B
i
a
s
(
d
e
g
/
s
)
Time (s)
Figure 5.8 Estimate of the yaw rate gyro bias.
98
To investigate the slow tracking of the lateral error estimation, the ?
ylat
?knob? was
tweaked to account for unmodeled disturbances or dynamics in the system. Discrete
values of 0.01 and 0.1 meters were inserted into the process covariance matrix. This
value has minimal affect on the velocity, heading, and bias estimates, but it has an
obvious affect on the lateral error estimate. Figure 5.9 and Figure 5.10 show the lateral
position estimates with the low and high process noise values. As ?
ylat
increases, the
estimate converges on the Lidar measurement and provides less filtering.
0 5 10 15 20
-0.7
-0.6
-0.5
-0.4
-0.3
-0.2
-0.1
0
0.1
Time (s)
L
a
te
r
a
l E
r
r
o
r
,
y
la
t
(m)
Lidar
KF
Figure 5.9: Lateral error estimate assuming no lateral velocity and ?
ylat
=0.01.
99
0 5 10 15 20
-0.7
-0.6
-0.5
-0.4
-0.3
-0.2
-0.1
0
0.1
Time (s)
L
a
te
r
a
l E
r
r
o
r
,
y
la
t
(m)
Lidar
KF
Figure 5.10: Lateral error estimate assuming no lateral velocity and ?
ylat
=0.1.
5.3.2. Scenario 2 ? Six State Estimator
The second scenario investigated the effectiveness of navigation with Lidar when a
lateral acceleration measurement is available. Often this measurement is corrupted by
process noise or vehicle dynamics, such as vibration or roll, or there are no suitable
measurements to make the system observable. The test bed did not contain many of these
additional sources of error so the lateral accelerometer recorded fairly clean data. There
was no lateral velocity sensor on board which made lateral velocity and lateral
accelerometer bias unobservable. Bias was removed post process and assumed constant.
This is a safe assumption because of the short duration of each test. The additional input
has minimal affect on the velocity and heading states, shown in Figure 5.11 and Figure
5.12. However the lateral error estimate, shown in Figure 5.13, is significantly improved
with the use of a lateral accelerometer.
100
0 5 10 15 20
-0.5
0
0.5
1
1.5
2
Time (s)
V
e
lo
c
i
t
y
(
m
/s
)
Lidar
?a
x
KF
Figure 5.11: Velocity estimated with second navigation model.
0 5 10 15 20
-2
-1.5
-1
-0.5
0
0.5
1
Time (s)
He
a
d
i
n
g
(
d
e
g
)
Lidar
?r
KF
Figure 5.12: Scenario 2 heading estimate.
101
0 5 10 15 20
-0.7
-0.6
-0.5
-0.4
-0.3
-0.2
-0.1
0
0.1
Time (s)
L
a
te
r
a
l E
r
r
o
r
,
y
la
t
(m)
Lidar
KF
Figure 5.13: Lateral error estimate using lateral acceleration input.
5.4. SUMMARY
A method using a Lidar to detect the edges of a defined corridor and process the
information in a Kalman filter to produce a navigation solution was proposed. The idea
is not limited to only a laser scanner and can be used with any environmental sensor
capable of providing distance and angle measurements to an object. One navigation
model was developed to estimate lateral error, while another was developed to estimate
lateral error and lateral vehicle velocity using a lateral accelerometer input. The level of
filtering was adjusted by tuning covariance parameters. It was shown the use of a lateral
accelerometer improved the estimate without requiring any modeling of unknown
disturbances.
102
103
6. CONCLUSION
6.1. SUMMARY
This thesis has shown the importance of including sideslip in a kinematic navigation
model when concerned with high accuracy navigation. Sideslip induces estimate errors
in states critical to vehicle controllers, such as heading and velocity. Degradation in the
position estimate was observed as well. Future vehicle control systems will require more
a precise navigation solution to operate correctly, therefore sideslip must be accounted
for when navigating. Work presented in this thesis also examined the use of a laser
scanner with an IMU to provide the vehicle with a lateral path error and lateral velocity
estimate. The lateral error and lateral could be utilized by ground vehicle control systems
to achieve better performance. The lateral velocity estimate could also be integrated into
a navigation system to reduce navigation errors due to sideslip.
The DARPA Grand Challenge provided the opportunity to develop a navigation
system for an autonomous vehicle. An estimation algorithm was written using an
extended Kalman-Bucy filter to provide an accurate navigation solution with and without
GPS, as well as vehicle state information, to the various vehicle subsystems. The
algorithm performed well while the vehicle was in competition, surviving GPS outages
lasting four minutes at the National Qualification Event. A hardware failure halted the
vehicle in the Grand Challenge before meeting any obstacles that would stress the
navigation system. However, the system performed exactly as designed during the event.
104
While testing the navigation system for the AGV, it was noticed that position error
grew when the vehicle took corners at high speeds. The soft tires of the AGV permitted
tire slip as the vehicle accelerated laterally. It was hypothesized the vehicle dynamics
were corrupting the navigation solution, due to the fact that vehicle sideslip creates a
discrepancy within the Kalman filter between the integrated yaw rate gyro input and the
GPS course measurement. When sideslip exists, these two angles are not equal. It was
concluded this phenomenon warranted further investigation.
To examine the navigation accuracy in the presence of sideslip, a five degree of
freedom, nonlinear vehicle model was derived to determine vehicle state information
with inputs of steer angle and velocity. The vehicle model was implemented in a
MATLAB? simulation so the response of various driving maneuvers could be studied in
a controlled environment. The simulation output was validated by comparing it to
experimental data collected on an Infiniti G35.
Once the simulation?s validity was confirmed, its output, along with experimental
data, was used to show the errors stemming from vehicle sideslip by comparing typical
navigation models with a model that accounted for the lateral vehicle dynamics.
Kinematic navigation models were used to estimate vehicle states important to control
systems. No vehicle model parameter knowledge was necessary to develop reliable
navigation solutions. Sideslip was found with the use of a dual antenna GPS receiver,
which only required simple geometric information about the vehicle to translate the slip
angle to different points on the vehicle. The two antenna system also provided a roll
measurement to estimate the roll dynamics. Roll is a critical component of the expanded
model as it leads to a more accurate and robust lateral velocity estimate.
105
Effects from longitudinal slip were also studied. Wheel speed sensors can be used to
provide a redundant velocity measurement to improve dead reckoning accuracy.
However, in the presence of wheel slip the performance of the system with wheel speed
sensors was shown to degrade. Doppler radar was used to complement the wheel speed
measurement, but it was shown the radar measurement bias, although not susceptible to
wheel slip, can be terrain dependent. It was shown in scenarios where terrain remains
fairly constant a Doppler radar can provide an accurate velocity measurement during a
GPS outage.
Although the effects of dead reckoning errors can be reduced, they are not removed.
Therefore, a navigation system incorporating a laser scanner was developed investigate
its performance in a GPS denied environment. A Lidar/INS solution was shown to
provide accurate state estimates for vehicle navigation within a defined corridor. The
reliable measurements and statistical properties of the Lidar form a suitable companion
for an IMU, similar to GPS, when using a Kalman filter to calculate a solution. Inertial
errors due to integration of noise and bias are removed, while providing high update rate
estimates of the dynamic vehicle states. A proposed method of tuning the performance of
the Kalman filter has been offered by means of varying a process noise statistic on lateral
velocity to account for unmodeled dynamics and/or disturbances to the system. It was
shown how a lateral velocity estimate could be extracted to calculate sideslip, and for use
in the proposed navigation model incorporating the lateral vehicle dynamics. A
significance of this research is it does not rely on GPS, and therefore could be used in
urban environments, in situations where GPS signal is jammed, or in other GPS denied
environments.
106
6.2. FUTURE WORK
6.2.1. Autonomous Navigation System Development
As the autonomous vehicle market continues to expand, there will be a push to
navigate cheaply, reliably, and robustly. To fulfill these demands, a tightly coupled
approach should be investigated, possibly using a cascaded approach. Tightly coupled
navigation algorithms are less vulnerable to signal loss, and still maintain limited
navigation capability when fewer than four satellites are visible to the GPS antenna. As
AGVs begin operating in more environments, this approach can provide significant
advantages, as not all environments are favorable to GPS. Cascading the navigation
model breaks up large matrices into multiple smaller ones. The computation expense is
decreased exponentially, which means the algorithms could run on slower, cheaper
machines.
6.2.2. Navigation Corruption by Vehicle Dynamics
This study presented analysis of the errors arising from longitudinal and lateral
vehicle slip. Throughout this thesis, the total vehicle slip was kept decoupled. Therefore
it was sliding purely longitudinally or laterally, not both. Future work should analyze the
potential error when longitudinal and lateral slip occur simultaneously. For instance, a
vehicle on ice could be translating sideways with the tires locked up.
Vehicle pitch dynamics should be included in the navigation model to study the
improvement in estimated states. This thesis showed the improvement of including a
centripetal acceleration term which was a function of the vehicle?s yaw rate. However,
there are still centripetal accelerations due to roll and pitch rate that were not accounted
for. Although the yaw rate is commonly much higher than the roll or pitch rate (therefore
107
its centripetal acceleration term is larger), some accuracy should be gained by adding the
other effects. The addition of a third GPS antenna would provide the necessary means to
make vehicle pitch observable in a navigation system.
6.2.3. Lidar/INS Navigation
This preliminary study of Lidar as a navigation aid has provided a foundation for
more extensive research. The next step is to try similar tests in different environments.
Ideally, defined corridors should consist of road edges, but other objects along the side of
the road may suffice. A SLAM based approach might be used to localize the obstacles
and define a corridor, especially if the objects are widely spaced. The use other
environmental sensors should be investigated as well. Auto manufacturers are beginning
to include vision systems and laser scanners in vehicles. The presented research could be
expanded to function on an automobile, where the estimates are used in a navigation
system, stability system, or lane keeping control system.
108
REFERENCES
1. Adams, M.D., ?Lidar Design, Use, and Calibration Concepts for Correct
Environmental Detection,? IEEE Transactions on Robotics and Automation, 16(6),
December 2000, pp 753-761.
2. Alban, S., ?Design and Performance of a Robust GPS/INS Attitude System for
Automobile Applications,? Thesis (PhD), Stanford University, Department of
Aeronautics and Astronautics, 2004.
3. Anderson, R., ?Using GPS for Model Based Estimation of Critical Vehicle States and
Parameters,? Thesis (MS), Auburn University, Department of Mechanical
Engineering, 2004.
4. Arnold, J., ?New Applications Make NDGPS More Pervasive? Public Roads, 64(4),
2001.
5. Behringer, R., Sundareswaran, S., Gregory, B., Elsley, R., Addison, B., Guthmiller,
W., Daily, R., Bevly, D.M., ?The DARPA Grand Challenge - Development of an
Autonomous Vehicle,? The 2004 IEEE Intelligent Vehicles Symposium, June 2004,
pp 226-231.
6. Behringer, R., Travis, W., Daily, R., Bevly, D.M., Kubinger, W., Herzner, W.,
Fehlberg, V., ?RASCAL ? An Autonomous Ground Vehicle for Desert Driving in the
109
DARPA Grand Challenge 2005,? Proceedings of the 8
th
International IEEE ITSC
Conference, September 2005, pp 644-649.
7. Bevly, D.M., ?Global Positioning System (GPS): A Low Cost Velocity Sensor for
Correcting Inertial Sensor Errors on Ground Vehicles,? Journal of Dynamic Systems,
Measurement, and Control, 126, June 2004, pp 1-10.
8. Bevly, D.M., Gerdes, J., Wilson, C., Zhang, G., 2000 ?The Use of GPS Based
Velocity Measurements for Improved Vehicle State Information,? Proceedings of the
American Controls Conference, 2000.
9. Campbell, J., de Haag, M.U., van Graas, F., ?Light Detection and Ranging-Based
Terrain Navigation ? A Concept Exploration,? Proceedings of the ION GNSS 2003,
September 2003 pp 461-469.
10. Cook, B., ?The United States Nationwide Differential Global Positioning System,?
Proceedings of the ION National Technical Meeting 2000, January 2000, pp 297-305.
11. Daily, R., Bevly, D.M., ?The Use of GPS for Vehicle Stability Control Systems,?
IEEE Transactions on Industrial Electronics, 51 (2), 2004, pp 270-277.
12. Dissanayake, G., Sukkarieh, S., Nebot, E., Durrant-Whyte, H., ?The Aiding of a Low-
Cost Strapdown Inertial Measurement Unit Using Vehicle Model Constraints for
Land Vehicle Applications, ?IEEE Transactions on Robotics and Automation,? 17(5),
October 2001, pp 731-747.
13. Dixon, J., ?Tires, Suspension, and Handling,? Society of Automotive Engineers,
Warrendale, PA, 1996, ISBN: 1-56091-831-4.
110
14. Dugoff, H., Fancher, P.S., Segel, L., ?An Analysis of Tire Traction Properties and
Their Influence on Vehicle Dynamic Performance,? SAE Paper 700377, 1970, pp
1219-1243.
15. Federal Highway Administration, ?High Accuracy ? Nationwide Global Positioning
System Test and Analysys: Phase II Report,? McLean, VA: US Department of
Transportation (FHWA-HRT-05-034), 2005.
16. Gebre-Egziabher, D., ?Design and Performance Analysis of a Low Cost Aided Dead
Reckoning Navigator,? Thesis (PhD), Stanford University, Department of
Aeronautics and Astronautics, 2004.
17. Gelb, A., ?Applied Optimal Estimation,? The MIT Press, Cambridge, MA, 1974,
ISBN: 0-262-57048-3.
18. Gillespie, T., ?Fundamentals of Vehicle Dynamics,? Society of Automotive
Engineers, Warrendale, PA, 1992, ISBN: 1-56091-199-9.
19. Godha, S., Cannon, M. ?Integration of DGPS with a Low Cost MEMS ? Based IMU
for Land Vehicle Navigation Application,? Proceedings of the ION GNSS 2005,
September 2005.
20. Guivant, J., Nebot, E., Baiker, S., ?Autonomous Navigation and Map Building Using
Laser Range Sensors in Outdoor Applications,? Journal of Robotic Systems, 17(10),
October 2000, pp 565-583.
21. Hay, C., ?Turn, Turn, Turn: Wheel Speed Dead Reckoning for Vehicle Navigation,?
GPS World, October 2005, pp 37-42.
111
22. He, B., Wang, D., Pham, M., Yu, T., ?GPS/Encoder Based Precise Navigation for a
4WS Mobile Robot,? Seventh International Conference on Control, Automation,
Robotics, and Vision (ICARCV), 2002.
23. Hirokawa, R., Nakakuki, K., Sato, K., Ishihara, R., ?Autonomous Vehicle Navigation
with Carrier Phase DGPS and Laser-Scanner Augmentation,? Proceedings of the ION
GNSS 2004, September 2004, pp 1115-1123.
24. Hirokawa, R., Nakakuki, K., Sato, K., Ishihara, R., ?Threading the Maze: GPS/INS,
Landmark Sensing, and Obstacle Avoidance,? GPS World, 15(11), November 2004,
pp 20-26.
25. Julier, S., Durrant-Whyte, H., ?On the Role of Process Models in Autonomous Land
Vehicle Navigation Systems,? IEEE Transactions on Robotics and Automation.
19(1), 2003, pp 1-14.
26. Kogler, J., Hemetsberger, H., Alefs, B., Kubinger, W., Travis, W., ?Embedded Stereo
Vision System for Intelligent Autonomous Vehicles,? Proceedings of the IEEE
Intelligent Vehicles Conference, June 2006.
27. Lei, Y., Lu, G., ?Estimation of a Vehicle?s Position and Orientation Using
Probabilistic Data Association,? Proceedings of the IMECE 2005, November 2005.
28. Milliken D., Milliken, W., ?Race Car Vehicle Dynamics,? Society of Automotive
Engineers, Warrendale, PA, 1995, ISBN: 1-56091-526-9.
29. Olson, B., Shaw, S., St?p?n, G., ?Nonlinear Dynamics of Vehicle Traction,? Vehicle
System Dynamics, 40(6), 2003, pp 377-399.
112
30. Ozguner, U., Redmill, K.A., Broggi, A., ?Team TerraMax and the DARPA Grand
Challenge: A General Overview,? The 2004 IEEE Intelligent Vehicles Symposium,
June 2004, pp 232-237.
31. Pacejka, H., Bakker, E., Nyborg, L., ?Tyre Modelling for Use in Vehicle Dynamics
Studies,? SAE Paper 870421, 1987.
32. Scheding, S., Dissanayake, G., Nebot, E.M., Durrant-Whyte, H., ?An Experiment in
Autonomous Navigation of an Underground Mining Vehcile,? IEEE Transactions on
Robotics and Autonomation, 40(6), 1999, pp 85-95.
33. St. Pierre, M., Gingras, D., ?Comparison Between the Unscented Kalman Filter and
the Extended Kalman Filter for the Position Estimation Module of an Integrated
Navigation Information System,? IEEE Symposium on Intelligent Vehicles 2004, June
2004.
34. Stengal, R., ?Optimal Control and Estimation,? Dover Publications, Mineola, NY,
ISBN: 0-486-68200-5, 1994.
35. Talaya, J., Alamus, R., Bosch, E., Serra, A., Kornus, W., Baron, A., ?Integration of a
Terrestrial Laser Scanner with GPS/IMU Orientation Sensors,? International Society
for Photogrammetry and Remote Sensing, XXXV, July 2004, pp 19-24.
36. Travis, W., Whitehead, R., Bevly, D.M., Flowers, G.T., ?Using Scaled Vehicles to
Investigate the Influence of Various Properties on Rollover Propensity,? Proceedings
of the 2004 American Controls Conference, June 2004, pp 3381-3386.
113
37. Travis, W., Bevly, D.M., ?Corridor Navigation with a Lidar/INS Kalman Filter
Solution,? Proceedings of the IEEE Intelligent Vehicles Conference, June 2005, pp
343-348.
38. Travis, W., Bevly, D.M., ?Navigation Errors Introduced By Ground Vehicle
Dynamics,? Proceedings of the ION GNSS 2005, September 2005.
39. Venhovens, P., Naab, K., ?Vehicle Dynamics Estimation Using Kalman Filters,?
Vehicle System Dynamics, 32, 1999, pp 171-184.
40. Whitehead, R., Travis, W., Bevly, D.M., Flowers, G., ?A Study of the Effect of
Various Vehicle Properties on Rollover Propensity,? SAE Paper 2004-01-2094, 2004.
41. Ye, C., Borenstein, J., ?Characterization of a 2-D Laser Scanner for Mobile Robot
Obstacle Negotiation,? Proceedings of the 2002 IEEE International Conference on
Robots and Automation, May 2002, pp 2512-2518.
114
APPENDICES
APPENDIX A: DARPA GRAND CHALLENGE VEHICLE
A small All Terrain Vehicle (ATV) platform, shown in Figure A.1, was chosen for
the entry vehicle due to its size, agility, and ruggedness (Behringer, et al, 2004; 2005).
The Prowler by ATV Corp. is a Yamaha Grizzly 660 modified for military use that is
equipped with a 660cc Yamaha engine, enhanced suspension, full roll cage, run-flat tires,
and cargo rack. This combination of power, ruggedness, and space proved to be an
excellent foundation for an off road autonomous vehicle. The large payload capacity and
heavy duty suspension can handle the multitude of motors, sensors, and computers that
were mounted on the cargo rack and in the roll cage. The suspension and high ground
clearance allow the vehicle to traverse difficult terrain with relative ease.
Figure A.1: RASCAL at the starting gate during the 2005 DGC.
115
Modifications were made to the ATV, dubbed RASCAL (Robust Autonomous Sensor
Controlled All terrain Land vehicle), for automation. A servo motor was installed in the
engine bay and attached to the steering system to actuate the front wheels. The motor
output, 6.5 ft-lbs of torque, was fed into a 14:1 gearbox, placing a total of 90 ft-lbs of
torque on the steering rack. Throttle, brake, and gear were controlled with smaller
servos.
Figure A.2: Steering motor (left); throttle and brake servos (right).
All of the servos were directed by microcontrollers, which communicated with a
computer via serial ports. An emergency stop mechanism with ultimate authority over
the microcontrollers was wired in series to both the vehicle?s power and the brake and
throttle servos to provide the ability to stop and kill the vehicle in the event of a software
or hardware failure. Two 15 gallon gas tanks were added to the side of the ATV and
gave RASCAL more than enough fuel to finish the course. Two AC generators provided
additional power needed to operate the electronics on board. An enclosure mounted in
the rear contained the delicate hardware.
116
The autonomous system was designed to have a modular architecture, depicted in
Figure A.3, so a failure from one module would not impede the operation of other
modules. At the core are the necessary modules required for autonomous operation: the
vehicle control and the GPS/INS navigation module. The other modules are ?optional
add-ons? required for entry into the Grand Challenge. They provide information about
the environment and about objects in it, which may need to be avoided in case they are on
the path trajectory. These modules are considered ?optional? because a disconnection or
failure for whatever reason will not impede the vehicle?s operation completely. The core
vehicle control will still continue to operate, using solely GPS and inertial input for
computing the control output.
Figure A.3 Flow of information on RASCAL.
One of the key tasks for RASCAL was to remain in a pre-defined corridor while
choosing the fastest and/or easiest path through the corridor and successfully avoiding
any obstacles within the corridor. Obstacle detection was realized with four Lidars and a
stereo vision system custom built by ARC seibersdorf (Kolger, et al, 2006). Two of the
117
LIDARs scanned vertically to detected negative obstacles such as holes or ditches. One
Lidar was mounted so that the scan plane was parallel to the ground to detect long range
obstacles, and one was mounted so that the scan plane intersected the ground
approximately five meters in front of the vehicle to detect obstacles near the vehicle.
Each sensor data set was processed by an algorithm specialized to that sensor/orientation
and then passed off for planning.
Figure A.4: Obstacle detection sensors on RASCAL.
Initially, most of the core technical members on this team were engineers at Rockwell
Scientific (RSC), and the team received a large portion of its funding from RSC.
However, in order to complement the existing expertise, SciAutonics sought other
participants to join the team. A very significant partner was Auburn University, who
118
119
joined the SciAutonics team in November 2003, contributing to the vehicle control and
navigation aspects of the system. ATV Corporation contributed by donating the vehicle
platform and a second test vehicle, as well as providing continual technical support
throughout the project.
APPENDIX B: TEST VEHICLE
B.1. TEST BED
Infiniti G35 sedan was used as a test vehicle to gather experimental data for the
research in this thesis. The car was loaned to the GPS and Vehicle Dynamics Lab at
Auburn University. It is equipped with a data logging system that reads a multitude of
sensors, including the vehicle?s onboard sensors over the Control Area Network (CAN).
Figure B.1: Infiniti G35 with data logging console.
A Navcom Starfire? DGPS receiver was utilized for its sub 10 centimeter position
accuracy measurements at 5 Hz, and was used as a truth measurement when analyzing
the algorithms with experimental data. A dual antenna Novatel Beeline? GPS receiver
was used to measure vehicle heading and roll at 5 Hz, as well as provide standard, non-
differential GPS measurements of velocity, course, and position. The GPS antennas are
120
shown mounted on the roof of the G35 in Figure B.2 on the right. A Crossbow
IMU400CC automotive grade MEMS IMU measured accelerations and angular rates at
32 Hz. The IMU is shown mounted in the center console in the left figure below. The
hand wheel angle was taken from the CAN and translated to a steering angle at the tire
using a 16.1:1 steering ratio, also at 32 Hz.
Figure B.2: The Crossbow IMU (left); Starfire and Beeline (right).
Figure B.3 shows the data logging computer with the power distribution hardware, all
mounted in the trunk. All power was provided by the vehicle and cleaned using DC-DC
converters with 12VDC and 5VDC outputs. A 12VDC-ATX power supply was installed
in the computer, which ran Windows XP. Communication to and from sensors was
carried out with RS232 serial ports and a DAQ (data acquisition) card.
121
Figure B.3: The G35 data logging computer and power distribution center.
The sedan was driven on a 1.7 mile oval test track with 8 degree banked turns, which
was operated by Auburn University?s National Center for Asphalt Technology (NCAT).
The straight sections are crowned 2 degrees in each lane. Experiments consisted of
various maneuvers, including emergency lane changes, slaloms, and high speed
cornering. The onboard vehicle dynamic control (VDC) was turned off, and the
maneuvers were performed at speeds ranging from 5 to 45 m/s.
B.2. SIDESLIP CALCULATION
The heading and course measurements were used to calculate vehicle sideslip. Most
receivers use a carrier phase differencing technique to calculate velocity and course,
which injects an inherent half sample lag in the calculation. It is critical to synchronize
all GPS measurements with respect to time, as any offset will calculate a false slip. The
algorithm to compute slip was written so it could be implemented in real time.
The heading and course measurements are output on two separate GPS messages.
The heading measurement is time stamped with time into the week in GPS time. Course
122
and velocity measurements are time stamped with UTC time. GPS time was converted
into time of day, and then converted to UTC time by accounting for the number of leap
seconds since January 6, 1980. At the date of this thesis, the difference between GPS and
UTC time was 14 seconds. The velocity and course measurements were then shifted by a
half sample to remove the delay induced by computation. A linear interpolation between
two consecutive measurements computes a value at time t that corresponds with the
position based heading measurement recorded at time t. Sideslip can then be calculated
by taking the difference between the two measurements.
Equation B.1 shows how sideslip was used to convert the velocity vector in to lateral
and longitudinal velocity at the antenna.
)sin(
)cos(
_
_
BLBLBLy
BLBLBLx
VV
VV
?
?
=
=
B.1
The vehicle yaw rate was used in Equation B.2 to translate the velocities at the antenna to
the CG.
BLCGBLCG
RrVV
/
?+=
B.2
123
124
APPENDIX C: VEHICLE SIMULATION PARAMETERS
m = 1528.15 kg
W = 149991.18 N
I
x
= 535 kg?m
2
I
y
= 2222 kg?m
2
I
z
= 2400 kg?m
2
L = 2.8499 m
a = 1.3679 m
b = 1.4819 m
h
cg
= 0.6 m
t
f
= 1.5011 m
t
r
= 1.5062 m
s
f
= 1.2009 m
s
r
= 1.2049 m
K
p
= 28165.70 N?m/rad
b
p
= 7850 N?m?s/rad
K
r
= 17985.76 N?m/rad
b
r
= 3850 N?m?s/rad
C
?FL
= 45837 N/rad
C
?FR
= 45837 N/rad
C
?BL
= 76394 N/rad
C
?BL
= 76394 N/rad
?
f
= 0.84
?
r
= 0.81