Process Mode s For The High-speed Navigation of Road

Vehicles
Simon Julier and Hugh Durramt-Whyte
University of Oxford, Department of Engineering Science,
19 Parks Road, Oxford OX1 3PJ

February 15, 1995

Abstract 2 The Vehicle Model

A nonlinear process m o d e l for t h e navigation of a high- Problem Specification. We wish t o determine the position
and orientation of a vehicle in Cartesian co-ordinates. To be
speed conventional r o a d vehicle is described. I n simula-
more precise, we shall estimate the position of the centre of
tions it is s h o w n to significantly reduce t h e errors in es-
mass of the vehicle, [ X G , y c I T , and its heading angle 4. Esti-
t i m a t i n g of vehicle position a n d orientation. T h e model mation will occur at discrete points in time which are referred
also performs limited online estimation of certain critical t o using the time index k . T h e control inputs are the steer
tyre p a r a m e t e r s such as m e a n radius a n d stiffness. angle and angular speed of the front wheel, S ( t ) and w ( t ) re-
spective1:y. We shall specify a discrete-time process model of
the form
1 Introduction +
x ( k 1) = f[x(k),u(k),k] w(k),+ (1)
where x(k) is the system state vector which evolves according
In the last decade Automatic Guided Vehicles (AGVs) have t o the discrete-time (and possibly time-varying) state transi-
found widespread acceptance in many industrial environments. tion equation f[.,., .], u(k) is the control vector, and w(k) is
However, most applications have been restricted t o low speeds uncorrelated, zero-mean dynamic process noise.
in highly structured environments and the full potential of The K i n e m a t i c s of the Vehicle M o t i o n . In developing
AGVs have yet t o be realised. In particular, AGVs have not this model we assume that a four-wheeled vehicle can be rep-
been employed in outdoor environments at speeds approaching resented as the simple two-wheeled vehicle as shown in Fig-
those of human-controlled vehicles. In all of these applications ure 1[3][8]. T h e vehicle body is the line AGB with the front
there is the demand for high-speed, high-performance naviga- axle affixed at A and the rear axle fixed at B. T h e centre of
tion systems. This paper addresses one aspect of developing mass of the vehicle is located at G, a distance a behind the
such a system: the vehicle process model. T h e work in this front axle and b in front of the rear axle. T h e pairs of tyres
paper is motivated by a current project at Oxford t o develop a on each axle are replaced by a “mean” tyre which is located
system which estimates the position and orientation of a con- at the centre of the axle. T h e radius of the mean front tyre
ventional car with sub-metre accuracy[5] at vehicle speeds of is R(t) and in this model the radius of the rear wheel has no
up t o 25ms-I (60mph). In many respects this is the worst effect.
case for au AGV: a highly unstructured and varying environ-
ment, large and fluctuating vehicle speeds, and complex vehicle
design. This work can be contrasted with other vehicle navi-
gation systems which are under development. Many of these
systems are concerned with road and route following using a
variety of sensor systems including vision [4]. However, in our
application we wish t o estimate absolute vehicle position as a
set of co-ordinates on a global map.
T h e navigation system employs an extended Kalman Filter
(EKF) whose structure requires the specification of a process
model of system dynamics and an observation model which
relates the observed quantities t o the system’s state space[l].
In the following section we present the vehicle model used in
predicting position. Initially this is presented in continuous
time, and we then derive the discrete-time form. Since the
equations are not directly integrable we employ a series of ap-
proximations which introduce a number of additional proper-
ties t o the model. In the third section we present test results
for the system using simulated data. The fourth section dis- Figure 1: The kinematics of a turning manoeuvre.
cusses the implications of this model and the test results on
process models of high-speed vehicles. Consider the situation shown in Figure 1, which illustrates

IEEE International Conference

on Robotics and Automation 101 -
0-7803-1965-6/95 $4.00 01995 IEEE

Authorized licensed use limited to: BEIHANG UNIVERSITY. Downloaded on May 16,2020 at 09:23:58 UTC from IEEE Xplore. Restrictions apply.
the kinematics of a turning manoeuvre. In general the vehicle
axles do not translate in the direction in which the wheels are
pointing. Rather there is some s i d e slzp at the front and rear
tyres with magnitudes a f ( t ) and m,(t) respectively. Because
T h e kinematics of vehicle motion have now been expressed
of this slip it is necessary t o note an important distinction solely in terms of the slip angles a f ( . )and a,(.). These angles
in velocities. Vf(t) is the velocity of the tread of the front
are now determined by considering the dynamics of the motion.
tyre. It is oriented in the direction of the front tyre and has
The Dynamics of the Vehicle Motion. Figure 2 shows
the magnitude Vf(t) = R(t)w(t),. However the tyre actually
the free-body diagram for the turning manoeuvre. T h e front
translates along the velocity vector VA(~) which is at an angle
and rear lateral forces Fyf(.)and F,,(.) act towards the in-
a f ( t )t o V f ( t ) .Similarly, the tread of the rear tyre has velocity
stantaneous centre of rotation 0. Assuming that the vehicle is
Vr(t) and the tyre translates with velocity VB(~).
front wheel drive and that rolling resistances on the rear wheel
We employ two assumptions. First, the vehicle body is rigid.
are negligible, the only longitudinal tyre force is F 3 ; f ( . ) .T h e
This leads t o the rolling constraint that the velocity of points
resultant force from the equilibrium condition is also shown
A, G and B projected along the vehicle body AGB must be
and is discussed in more detail below.
the same. Therefore

VG(t)COSP(t) = Vf(t)COSS(t). (2)

Second, i t is assumed that at a n y time t the vehicle has reached

dynamic equilibrium. If the steer angle is zero then the vehicle
will continue t o drive in a straight line a t constant speed. If the
steer angle is non-zero the vehicle will continue t o travel along
an arc of constant curvature p ( t ) about 0 at constant speed.
Note that these conditions, are assumed t o apply even if the
steer angle is changing and 6 ( t ) is non-zero. This is a somewhat
unrealistic assumption which is made for convenience. T h e
impact of this is discussed later.
At the instant shown the vehicle is rotating about its in-
stantaneous centre of rotation in a direction orthogonal t o OG.
T h e instantaneous speed of translation of G is VG(t) in the di-
rection of the course angle ~ ( t )Note . that this angle is clearly
distinguished from $ ( t ) : whereas the former is the instanta-
neous direction in which G is translating at each point in time,
the latter is the direction in which the car body is orientated at
each instant in time. In general these two angles are not equal
and the difference, known as the attitude angle is denoted by
the symbol p(t). Figure 2: Free body diagram of a turning manoeuvre.
Taking limits with respect t o time, the kinematics update
in continuous time according t o In general the acceleration of a point object has two compo-
nents: one parallel and one perpendicular t o the direction of
motion. Applying the equilibrium assumption simplifies these
equations since i t is assumed that the vehicle travels around
an arc at constant radius and constant speed. T h e resultant
force acts along OG in the direction of 0 and is equal t o the
centripetal acceleration for the circular motion. By resolving
T h e problem of brilding any vehicle process model is, there- inertia forces with tyre forces and taking moments, the tyre
fore, to find values for VG(.), p ( . ) and p(.) at each time t . How- forces are given by the solutions to the nonlinear simultaneous
ever, this is not a very convenient formulation of the problem equations,
since these quantities are abstracted from the physical situa-
tion and i t is more meaningful t o write the vehicle kinematics
in terms of the side slip angles. Employing the earlier assump-
tions, these angles are given by

aJ(t) = ~ ( t ) tan-’
~ [p(t)asecP(t) +t a n ~ ( t ) ] (7)

a.(t) = tan-’ [p(t)bsecp(t) - t a n p ( t ) ] . (8)

By using small angle approximations, linearised versions of
these equations are frequently employed. However, we assume
that these angles may not be small and so retain the full non-
linear form of the equations. Rearranging these equations to
make p(t) and p ( t ) the subject of the formulae,
P(t) = tan-’ (- b tan[s(t) - a f ( t ) ]- a tan a r ( t )
a+b
where m is the mass of the vehicle.
In this situation F Z f ( . )is not the inertia force required t o
accelerate G. Rather i t is the additional tractive force re-
quired t o overcome “tyre drag”[3] - the components of lateral
tyre forces which try t o slow the vehicle down as i t negoti-
ates a turn. In practice wit was found t h a t the value of this
force is very low and has a negligible effect on estimator per-
formance. Therefore, the approximation t h a t F 3 ; f ( t )M 0 was

102

Authorized licensed use limited to: BEIHANG UNIVERSITY. Downloaded on May 16,2020 at 09:23:58 UTC from IEEE Xplore. Restrictions apply.
used for computational simplicity since we no longer need t o
solve Equation 12.
T h e dynamic model is linked with the kinematic model
through an analytical tyre model.
The Analytical Tyre Model. T h e tyre model captures the
relationship between the kinematic and dynamic models. In
normal driving conditions when, for example, the car is not
close t o loosing traction with the road, very simple models
can suffice. When the side slip angle is below 4', an adequate
description is t o assume that the tyres behave like linear tor-
sional springs and that
T h e tyre stiffness parameters C,, (.) and C a p ( . )must com-
pensate for changing tyre stiffness and the different d i p ge-
ometries in the two-wheeled model and the four-wheeled truth
model. This is achieved by assuming that the rate of change
of the tyre stiffness parameters is a zero-mean, uncorrelated
process. Since tyre radius is unknown and can vary with time,
we model this process as a zero-mean, uncorrelated Gaussian
process. Therefore the cxpected rate of change of the tyre
parameters are
R(t) = 0
Ca,(t) = 0
CO&) = 0.
Discrete-time Process Model. T h e discrete time process
model is derived by integrating the continuous time equations
over the limits t k t o t k + l . However, this task is complicated by
the nonlinearity of the state transition equations. In general,
when 6(.) and U ( . ) are time varying the kinematic continu-
ous time equations are not integrable. Further, two non-linear
simultaneous Equations (11 and 13) must be solved at each in-
stant in time. Providing the timestep is small, these problems
can be circumvented by assuming that the tyre forces remain
during the interval AT,,= [ t k , t k + 1 ] . As a consequence, the ve-
hicle moves along a circular arc with constant curvature p ( k )
and v ( . ) increases linearly with time. The discrete-time kine-
matic transition equations of this motion are
ZG(k) =ZG(k - I)+
+ p ( k ) V G ( k ) A T k ]- sinv(k
(
sin[v(k - 1)
P(k)
- 1)
) (19)
I/G(k) = y G ( k - I)+
+ P(k)VG(k)ATk]- cosv(k - 1)
(
cos[v(k - 1)
P(k)
) (20)
v ( k ) = v ( k - 1) + P(k)VG(k)ATk.
T h e tyre forces could be found by using a numerical scheme
to iterate to a solution at the start of each timestep. However,
because the tyre forces are observable so long as the vehicle
is moving, d a t a about their value can be determined from the
observation information. By including the tyre forces in the
state space description of the system it is possible, at each time
step, to have updated estimates of the vehicle tyre forces. In
applications when there is a risk that the vehicle can skid (such
as on a smooth surface), this knowledge is very important for
determining a safe control strategy.
To incorporate the tyre forces into the state space descrip-
tion a discrete-time state transition model must be specified.
Authorized licensed use limited to: BEIHANG UNIVERSITY. Downloaded on May 16,2020 at 09:23:58 UTC from IEEE Xplore. Restrictions apply.
We use the information from the dynamic model t o do this.
In particular, we choose a numerical scheme t o solve the three
nonlinear state transition equations. One iteration of this
scheme corresponds t o the one step dynamics of the force
states. For brevity we write xf(k) = [F,f(k), FY,(k)lT as the
vector of forcr states. T h e dynamics for the transition must
be expressed in discrete time with the form
Xf(k + 1) = g[x(k),4 k ) , kl (22)
where g [ . ,., .] is a discrete-time function t h a t causes xf(k) to
converge to the solution of the simultaneous equations. T h e
form of this function was motivated with reference to literature
on numerical analysis and the ease with which a state transi-
tion Jacobian matrix could be derived. Solutions to probleins
of the form Ay = b are well-documented and well-understood.
One such scheme is the relaxatzon method[2] which iterates a t
the nth step according t o the expression
Y, = W b - h Y , - l > (23)
and converges if a t least one matrix norm of I- R A is less than
one. In this form, the process dynamics of the force states are
xf(k + I ) = xf(k) - n(b(k)- A(k)xf(k)) (24)
with
(25)
(18)
Under this scheme the equilibrium conditions (Equations 11
and 13 are written as
and g [ . , ., .] has the desired form.
For small time steps it is quite easy t o ensure convergence
over quite a wide range of situations. For a given C? conver-
gence is governed by the value of A(.). Although this matrix is
time-varying, all the terms are sines and cosines or constants
and the actual changes t o A(.) are quite small. I t is possible
t o select a value which leads t o convergence over quite a large
range of operating conditions. Even in exceptional circum-
stances when the convergence condition fails the filter updates
prevent the solutions from diverging.
Finally the tyre parameters update according t o
R ( k ) = R ( k - 1)
(21)
C,,(k) = COf(k - 1)
COr(k) = Ca,(k- 1).
In summary, the state vector for this model is
These states are updated using Equations 19, 20, 21, 28, 24,
29 and 30 respectively.
The Process Noise Model. In this model errors and uncer-
tainties arise from disturbances in the tyre-ground interaction
and variations in the tyre parameters. Distnrbances between
the tyres and the ground are modelled as additive noise terms.
103
If py.(.) is the nominal lateral tyre force and Fy.(.)is the ef-
fective lateral tyre force, then
F,f(k) = p9f ( k ) + AFuf(k)
Fy,(k) = +
@?Jr(k) AF,,(k)
where A F y f ( k ) and AF,,(k) are assumed zero-mean; uncor-
,
related and with constant co-variances U $ and u $ respec-
~ ~
tively. Time correlated errors are modellec! as additive distur-
bances t o the tyre parameters of the form
R ( k ) - R(k - I) = AR(k)
C,, ( k ) - 60,( k - 1) = Ace, ( k )
G ( k ) - C a 7 ( k - 1) = ACar(k)
where A E ( k ) , AC,, ( k ) and AC,,(k) are zero-mean, uncorre-
lated and with constant co-variances U:, and cr"C,, re-
spectively. Note t h a t we assume that there is no noise on the
control inputs and all uncertainties in wheel speed and steer
angle are expressed in terms of disturbances of the tyre forces
and tyre parameters. 'Therefore when the vehicle accelerates
or decelerates, these effects will be shown as changes in the
estimates of the wheel radius and the tyre parameters. This is
equivalent t o using a constant velocity model to estimate the
position of a body t h a t may accelerate. Poor performance dur-
ing an acceleration is traded for good performance at constant
velocity.
3 Results
T h e performance of the process model was tested by incor-
porating i t into a simple navigation system. This system was
used t o estimate the position and orientation of a simulated ve-
hicle as it drove along a simple test track. T h e vehicle, shown
in Figure 3 , was based on d a t a from a typical car and was
simulated using the ADAMS software package. This package
comprehensively models the motion of a mechanical system
t o forces and excitations and is widely used by the automotive
industry t o examine ride and handling performance of a car[i'].
Figure 3: Vehicle model.
T h e test track was simple: the vehicle starts from rest, ac-
celerates up t o 6Omph, performs a single sharp turn to the left
arid then slows down t o a halt. For this test a simple observa-
tion model was used: the position of G and the heading angle
of the vehicle were measured at each time step,
z(k) = [ 7:;))] [
X.(k)
=
XG(k)
YG(li)
4 k ) -P(k)
] .
These measurements were corrupted by zero-mean, uncorre-
lated process noise. T h e benefits of the estimator were ex-
pressed in terms of the reduction of the covariance estimate
compared t o the observation noise.
Authorized licensed use limited to: BEIHANG UNIVERSITY. Downloaded on May 16,2020 at 09:23:58 UTC from IEEE Xplore. Restrictions apply.
Figure 4 shows the state errors in the estimates produced by
this model and the two standard deviations bounds. As can
be seen, the errors in ZG(.), y ~ ( . )and $(.) are approximately
zero-mean and lie within the 2a bounds. T h e mean covariances
in these estimates are approximately 25%, 29% and 20% of the
observation noises on these states respectively. Therefore, by
employing an EKF we have significantly reduced the errors
in knowledge about the position and orientation of the vehi-
cle. However, we also estimate additional, physically mean-
ingful states. From the graphs it is clear t h a t the estimates of
(33) these parameters exhibit interesting patterns of behaviour. It
(34) is worthwhile examining this behaviour since i t offers insights
into the behaviour of the process model as compared t o that
(35)
in the simulation.
T h e estimate of the mean wheel radius, E ( . ) , exhibits large
errors near the start and the end of the simulation. As ex-
pected these are related t o the time during which the vehi-
cle changes its angular wheel speed. Although wheel speed
changes continuously, the discrete-time model assumes that
the angular speed is constant across the interval between dis-
crete time steps. At the start of the motion the vehicle accel-
erates from rest and the filter uses a vehicle speed which is too
high, underestimating estimates wheel radius. In the latter
part of the motion the opposite effect occurs. However, the
behaviour is dominated by the fact that the vehicle skids as it
brakes to a halt. This is manifest as a very rapid increase in
the tyre radius estimate. Although the mean tyre radius esti-
mate is not necessarily an accurate representation of true tyre
radius, it provides important information as t o behaviour of
the vehicle. In particular if the vehicle begins t o skid the esti-
mated mean tyre radius rapidly increases t o an unrealistically
large value.
Second, the force estimates both show initial transients (due
to nnmodelled vehicle dynamics and the dynamics of the nu-
merical scheme) and have approximately constant offsets in
the steady part of the corners. However, the sum of the error
is mean zero. These errors are a direct result of the assumption
that the pairs of tyres on each axle can be combined t o produce
a mean tyre a t the centre of the axle. In general as a vehicle
goes around a corner the longitudinal forces on each tyre are
different. These apply additional, unmodelled moments that
are manifest as a couple which is superimposed on the tyre
forces of a simple, two-wheeled vehicle. Therefore any model
which seeks t o present an accurate portrayal of the tyre forces
must use a four-wheeled model of vehicle motion.
Finally, Figure 5 shows the estimated front and rear tyre
stiffness parameters. As can be seen, there is quite a substan-
tial change in these quantities during the turn. Starting from
arbitrarily values, the value of each falls sharply during the
turn and rises to different levels after the turn. There are a
number of reasons for this behaviour. First, these state cap-
ture the effects of different slip geometries on the two-wheeled
model and four-wheeled vehicle. Second, they compensate for
the effects of unmodelled vehicle transients which appear as
initial rises in the values of the state estimates. Third, the ini-
tial values were chosen at random with little knowledge about
(36)
vehicle mass, actual tyre stiffnesses and the coefficients of fric-
tion between the tyres and the road surface. Therefore, some
of the behaviour is explained by initial transients as the pa-
rameters adjust t o more realistic values. This was illustrated
in simulations when the vehicle conducted more than one turn.
During each turn the tyre stiffness parameter values initially
decreased and then increased again. Apart from the first turn,
104
which is illustrated in the figure, the stiffness parameters re- that a number of vehicle parameters, especially those relat-
turned to approximately the same value as before the turn. ing t o tyres, are critical in describing and understanding the
Therefore these states provide additional information as t o the behaviour of a vehicle. Tests have shown that this process
road conditions and whether the vehicle is slipping. If the front model is very effective, and reduces errors in position and ori-
or rear tyre stiffness estimates become very small during the entation estimates by over 20%. Further, we have shown that
turn, then there is the risk that the vehicle's wheels are close these critical parameters can be estimated in real-time only
t o locking and loosing traction with the road. employing knowledge of the vehicle's position and orientation.
No accurate knowledge of vehicle mass or coefficients of fric-
X state error Y state error
tion between the tyres and ground are required. There are a
-1 number of significant applications including the development
.i
".-
n i 0 21
of more refined anti-lock braking systems, and as a research
E o.l tool to aid engineers in understanding of the behaviour of tyres
$
m
0 in real situations.
I

-0.1 Analysis presented in the third section shows that the be-
haviour of these states are not quite as expected. Although
t __
1 - - /

-0 2
- 0 q , , 1
0 500 1000 0 500 1000 some of these behaviours are artifacts of unmodelled process
time step
dynamics (for example the estimated wheel radius increases
3 psi state errol
during a period in which the vehicle is accelerating), many
affects relate t o significant facets of vehicle behaviour. For ex-
ample, a large increase in the tyre radius estimate is indicative
of the fact that the vehicle's wheels have locked and that it is
m .
vI-05- skidding.
Further work will be carried out t o refine the vehicle model.
1-
0 500 1000
Using a new nonlinear filtering technique which has recently
time step been developed at Oxford[G], we shall incorporate additional,
undescribed dynamics into the vehicle model. This will have
Fyf state error Fyr state erroi
two effects. First, we expect the estimate of position t o be-
05 come more accurate. Second, the estimates of vehicle parame-
0
ters will become freed of bias and other effects and will reflect
g o
I
the true state of the system. Armed with these two sets of in-
formation, we believe that the high speed navigation of AGVs
is a feasible option.
-0 5}
L - L
i
0 500 1000
lime step
1500 0 500 1000
time step
1500 Y. Bar-Shalom and T. E. Fortmann. Tracking and Data
Association. T h e Academic Press, 1988.
R. L. Burden. Numerical Analysis. Prindle, Weber and
Figure 4: State errors in the estimates of x ~ ( k ) yc(IC),, Schmidt,, 3 edition, 1985.
$ ( k ) , R ( k ) , F,f(IC) and Fy,.(IC). Units are m, m, radians,
J. C. Dixon. Tyres, Suspension and Handling. Cambridge
m, k N and kN respectively.
University Press, 1991.
F. Jurie et al. High speed vehicle guidance based on vision.
In '9i ICAR. Fifth International Conference o n Advanced
Robots. Robots i n Unstructured Environments, pages 205-

/1
210, 1991.
74

S. J. Julier. Process models for the high-speed navigation

Figure 5: Estimates of C,, ( k ) (dotted) and Car( I C ) across
time. Units of tyre stiffness are kN/radian.
of conventional road vehicles. Transfer thesis, University
of Oxford, 1994.
S. J. Julier, J. K. Uhlmann and H. F. Durrant-Whyte. A
new approach for the nonlinear transformation of means
and covariances in linear filters. IEEE Transactions on
Autonzatic Control, In preparation.
R. R. Ryan. Adams multibody system analysis software.
In Werner Schielen, editor, Multibody Systems Handbook,
pages 361--402. Springer-Verlag, 1990.
J. Y. Wong. Theory of Ground Vehicles. John Wiley and
Sons, 1978.

4 Discussion
This paper has described a dynamic model which is used to
estimate vehicle position and orientation as part of an AGV's
navigation system. In developing the model we have shown

Authorized licensed use limited to: BEIHANG UNIVERSITY. Downloaded on May 16,2020 at 09:23:58 UTC from IEEE Xplore. Restrictions apply.