A Tracking Algorithm for Autonomous Navigation of

AGVs in an Automated Container Terminal

Faizan Farooq
Department of Mechatronics Engineering
National University of Science and Technology
Islamabad, Pakistan
Fazi_80@yahoo.com
Abstract: In this paper an algorithm is investigated of automated
guided vehicles (AGVs) for autonomous navigation operated in
container terminal. Sensor based navigation is introduced for
detecting the bearing and the range. Multiple model algorithms
(navigation algorithm) are used for obstacle avoidance and to avoid
other AGVs in the path. Two Kinematics models are used to derive
and detect obstacles (AGVs).For linear motion we used constant
velocity model and for curvilinear we use constant speed turn
model. Instead of Kalman filter we use unscented Kalman filter in
constant speed turn model because of the fact that it is non linear
as a system. Root mean square error is being reduced in case of
linear motions.

I.

INTRODUCTION

Automated plants are now become feasible because of the fact

in advancement of electronics, Sensors and information
technology. Fully automated container terminal is now
operated by the port of Rotterdam (European Combined
Terminal) using automated yard crabs and Automated Guided
vehicles (AGVs) on the other hand other ports like the
hamburg port and thames port are using the same idea.
Automatic control System having AGVs and conveyors belts
are a source of a driver. Motion of the vehicle are assessed by
sensor which is controlled by automated control system by
generating appropriate commands, For material handling tools
AGVs are count to be the most flexible tools. Looking at the
size that ranges from few kilograms to 130 ton kilogram.

Fig.1 An AGV in the automated container terminal of ECT at

Rotterdam

In Modern Container terminals automated plants manual

system are now replaced by AGVs .In Rotterdam port An
AGVs with part load in ECT terminal is shown. Conventional
diesel engines batteries and electric motors are the main
source through which AGVs automated industrial trucks are
powered. So restricted areas together with AGVs having low
speed characteristics are operated in making the problem more
simpler to solve. Therefore container used for AGVs are more
feasible as handling tools in modern industries being a strong
potential to improve the efficiency of the plant
An AGV system consist of a management system, a
navigation system, an on board controller communication
system and the vehicle. We use navigation system for path
guidance in an operating environment which is based on free
path approach and fixed path approach .If we look at the fixed
path approach AGVs movement is restricted to obey a fixed
path approach where there is no flexibility in guide path
change examples are embedded wires, rail track (fixed path)
or other type of guided ways. Dynamically changed path are
there in free path method whereas autonomous system is
capable of path detecting using information online.
Navigation effectiveness depends on interpretation of sensors
information by knowing the exact location, surrounding and
the environment. Such information are provided by lidar,
radar, vision sensor and scanner. Autonomous navigation of
AGVs is done by enabling the controller and surrounding
objects motion is interpreted properly by sensors available on
the ground.
Many studies on the autonomous navigation and localization
of AGVs have appeared in the literature. Adam et al. (1999)
presented a method of determining the position and orientation
of an AGV by fusing odometry with the information provided
by a vision system. Zhang et al. (2000) introduced a novel
navigation sensor using a position-sensitive detector to direct
the vehicle along a predefined trajectory. The navigation
sensor mounted on the vehicle picks up signals from floor
marks and gives commands to make the vehicle perform linear
or turning motions. As well, Park et al. (2002) proposed a
path- generation algorithm that uses the sensor scanning
method.
This paper Characterize the architecture of tracking algorithm
for AGVs Navigation for cargo transportation containers in a
port environment. In particular, for an AGV to navigate for
other obstacles and AGVs we derive an autonomous
maneuvering algorithm. By deriving this algorithm we can

accurately anticipate the movement of other AGVs by

adapting themselves for smooth and non curvilinear path. This
competence relies on how the sensors read other obstacles and
AGVs. Bayesian approach (Bar-Shalom et al., 2001) is used
for detecting other obstacles, Techniques that are used in
maneuvering targets are worn in many surveillance systems
and also in many applications dependability and reliability is a
main concern. Multiple models can administer better results
than using a single model in case of tracking maneuvering
target. By using multiple model algorithm significant gain in
performance possible. Many literature has been on the
exploring side and many of them are proposed.
Generally Targeted models are divided into two categories the
maneuvering model and uniform motion model .Constant turn
rate and speed refers to maneuvering target models and also
called coordinated turn model (Bar-shalom model) .A single
constant velocity model with an IMM algorithm are used in
air traffic control from which two models are scrutinize.
Augmented algorithm using extended kalman filters has
following drawbacks such as non Gaussian density in EKF
and Approximate Gaussian mixture with a single Gaussian
density in AIMM. If these premise break down, the AIMMEKF may diverge. So for curvilinear model we use unscented
kalman filter (UKF) instead of EKF.
II. Problem Formulation
Basically we have determine the AGV pattern of navigation so
after analyzing we found that we need an hybrid system which
includes IMM algorithm for the detection of other AGVs
using different sensors .Although two Kinematics model are
introduced to analyze the navigation pattern
A. Navigation patterns
Different models of assorted navigation patterns are shown in
figure 2 which includes curve and straight line, u turn and cut
in/out represented by constant velocity rectilinear motion,
constant angular acceleration curvilinear motion and constant
angular velocity curvilinear motion. Two stochastic models
are assorted one curvilinear motion and other is rectilinear
motion
B. Straight line and curve

Sequel in the figure shows that constant acceleration and

velocity rectilinear motion is captured by constant velocity
model with and without an additional noise term respectively.
While angular curvilinear and constant angular velocity
motions will be covered by constant speed turn model.
III. Stochastic Hybrid System
Admiring the work of Bar-shalom and Li (1993), A stochastic
hybrid system is

And with noisy Measurement

This technique refers to when AGV detects preceding AGV

which follows curves and Straight line
C. U-turn
As the name suggests that an AGV take a u turn while
changing its direction to 180 degree. As it consist of three
multiple routes. Rectilinear movement of targeted AGV
undergoes circular turning up to 180 degree following a
constant yaw rate that converts its motion in opposite direction

Where x(k) Rnx is state vector counting the velocity, yaw

rate and including the position of the AGV considering
discrete time k. m(k) is the scalar valued model state at instant
k which is in transition with Markov chain

Where M is the set of modal states that is constant

acceleration, velocity and constant radius of curvature turning

with constant angular rate, etc. Since the discrete Event occur
in the system so the system is Hybrid. Whereas m(k) denotes
the preceding AGVs navigation mode in effect to sampling
periods which ends at k that is time period (kk-1, tk) Whereas
Mode mj is in effect k ,the time that is mj(k) { m(k)=mj }
Where z(k) Rnz is the noisy measurement from the sensor
having vector value and is mode dependent. [k-1, m(k)] Rnv
depends on process noise sequence having mean
v[k-1 , m(k)] and covariance Q[k-1, m(k)]. w[k, m(k)] Rnz
is the noisy measurement with mean w[k, m(k)] and
covariance R[k, m(k)]

for tangential acceleration denotes constant yaw rate with w>0

means anti clockwise turn. The rate of the change in speed is
equal to the acceleration tangential component. i.e.

Where
.
System Transient matrix

IV. Two Kinematics Models:

Noise with contrasting levels of variance can present images
of different motions. Maneuvering motions are captured by a
noise with high variance. While low variance noise can
capture uniform motion. Immediate capturing of complex
system can be done by multiple models better than others.
So Curvilinear and Rectilinear are the two kinematics model
that we have derived. First assuming acceleration is quite
small in the steady state while in Constant velocity model
process noise is reasonably covered by linear acceleration or
deceleration. That is the uniform motion on the road is carried
out by constant velocity model plus zero- mean noise with
appropriate covariance which is represented by acceleration
magnitude. So Noisy constant velocity model is given by

It is noticed that if the angular rate is time varying in the

equation (7) then equation (9) is no further a valid equation.
Basically in this paper (Bar Shalom et al, 2001) a discrete
time domain with the speed turn model is introduced. In this
approach Model itself acquire motivation from equation (9)
but with varying angular rate.
A new state vector by augmenting the state vector in Equation
(7) with angular rate w(k) is defined as follows

Where superscript a is the augmented value

Then, nearly constant speed model is as follows:

T refers to sampling time, x(k) the state vector which include

longitudinal and lateral direction of the preceding vehicle at
discrete time k

For turning another model is derived from continuous time

model (Bar-Shalom et al, 2001).A constant speed turn is a turn
having yaw rate constant along with constant curvature radius.
Whereas actual road curvature is not constant. For capturing
the variations of the road curvature small noise is added for
constant speed turn model. Modeling error defines the noise in
the model such as the presence of non constant radius and
angular acceleration of curvature. So for a vehicle which is
moving with constant speed having constant angular rate, the
kinematics equation in this plane is

Stands for longitudinal acceleration and

stands

Moreover both equation (5) and equation (11) are special form
of equation (1).Equation (3) has the Markovian probability
assuming the transition mode of an AGV. So Stochastic

hybrid system can easily define the kinematics behavior of an

AGV.
V. Proposed Unscented Kalman Filter for Turning
Motions
The Structure of IMM algorithm referred to (Bar Shalom et,
1993) .Study suggests two IMM algorithms one for curvilinear
motion and the other is rectilinear motion. Whereas Equation
(5) refers to tracking procedure of an AGV in rectilinear
motion which is carried out by standard Kalman filter.
However for curvilinear motions we need w for estimation of
new augmented model equation (8) in section 2.
A. The EKF for the constant-speed turn model
Since Equation (11) model is nonlinear, So an EKF model is
used for the estimation of the state in equation (10).The nearly
constant speed turn model of equation (11) is as follows:

In Estimation procedure, fa is known and remains unchanged.

Noise transition matrix G (k-1) is the same form that is given
in the equation (11).
Is the predicated state
so Taylor series is used for the expansion of the non linear
function in the equation (12) with term up to first order, to find
the first order EKF which is given by

Where HOT= Higher order terms

EKF state predication covariance of the above equation is

Where Qa is the covariance of the process noise in equation

(12)
B. The UKF for the constant speed turn model
So we use UKF model because of the drawbacks that are in
EKF.UKF is basically a square error who estimates the
mean.UKF takes the true measurement and distribution values
to find the state vector which is represented by Gaussian
vector But we have to specify some sigma points to find the
mean and covariance. Unscented transform is the main part of
UKF.
Let x Rnx is a random vector, p: RnxRny a non- linear
transformation and y=p(x). Mean=x and covariance =Px,
respectively.
Find (2nx+1), Sigma Point xi and Weight Wi

Where k a scalar vector

(1) Sigma points for Non linear Function

is Jacobian matrix represented by f. The Partial derivatives are

as follow

(2) Find the Covariance and Mean:

Now using equation (18) we have to find the sigma point xi(k1|k-1) ,the weight Wi(i=0,., 2n)
(3) So by using state equation we predict sigma points that is

with a speed of 28 m/s; a turn between 160 s and 179 s with a

yaw rate of = 3.74 /s and a straight line between 180 s and
200 s.

C. U-turn scenario
The target initial positions and velocities were (x0 = 10 m y0=
10 m 0= 28 m/s, 0= 0 m/s, = 0). This scenario included a
non-maneuvering driving mode during scans from 1 s to 60 s
with a speed of 28 m/s, a 180 -turn, lasting from scan 61 s to
145 s with a yaw rate of = 3.74 /s , and a non-maneuvering
driving mode from scan 146 s to 200 s.
D. Interchange scenario

Where Pazz and Paxz are the covariances

Now finding the filter gain:

VI. Simulation Results

A. Scenario for straight line and curve:
Initial velocity of the targeted models are (x0 = 0 m, y0 = 0
m, 0=28 m/s, 0 =28 m/s , w=0).Its trajectory was a constant
velocity between 0 s and 19 s with a speed of 28 m/s; a turn
with a constant yaw rate of = 3.74 /s between 20 s and 59
s; a constant velocity between 60 s and 89 s; a turn with a
constant yaw rate of = 3.74 /s between 90 s and 129 s; a
constant velocity between 130 s and 149 s; a turn with a
constant yaw rate of = 3.74 /s between 150 s and 200 s
B. Cut-in/out scenario
The target initial positions and velocities were (x 0 = 0 m, y0=
20 m, 0= 28 m/s, 0 m/s, = 0). Its trajectory was a straight
line between 0 s and 19 s with a speed of 28 m/s; a turn with a
constant yaw rate of = 3.74 /s between 20 s and 39 s; a
constant velocity between 40 s and 41 s with a speed of 28
m/s; a turn between 42 s and 63 s with a yaw rate of = 3.74
/s ; a straight line between 64 s and 134 s with a speed of 28
m/s; a turn with a constant yaw rate of = 3.74 /s between
135 s and 154 s; a constant velocity between 155 s and 159 s

The target initial positions and velocities were (x0 = 0 m y0= 0

m, 0= 28 m/s, 0= 0 m/s, = 0). This scenario included a
non-maneuvering driving mode during scans from 1 s to 40 s
with a speed of 28 m/s, a 270-turn, lasting from scan 41 s to
168 s with a yaw rate of = 3.74 /s , and a non-maneuvering
driving mode from scan 169 s to 200s. The maneuvering
vehicle speed was 28 m/s

VII. Criterion used in design

CV=Constant Velocity
CST=Constant speed turn
W (0) =30/s
CV mode: PKF (0) = diag {100 100 100 100},QKF= (0.001)2I
CST mode: PEKF (0)=PUKF(0)=diag {100 100 100 100 2w} ,
QEKF=QUKF= diag {(0.25)2 {(0.25)2{(0.25)2{(0.25)2 2w }
Where w= (0.01)0/s =10 and n=10
Where

And

Position errors versus Time

Conclusion
velocity errors versus Time
In this paper an algorithm for tracking the AGVs is designed
.As the model is able to detect obstacles, other AGVs in the
path follower by using different kinematics model. For linear
motion we use constant velocity model whereas for curvilinear
motion we use constant speed turn model. Although an
unscented kalman filter is also used for nonlinear systems
which reduces the errors for possible movement
References
position errors in cut-in/out situation

velocity errors in cut-in/out situation

[1] Bar-Shalom, Y., Li, X. and Kirubarajan, T., 2001,

Estimation with Applications to Tracking and
Navigation, John Wiley & Sons, INC, New York, pp.
421 --490.
[2] Lee, C. K. and Yi, K. S., 2002, "An Investiga- tion of
Vehicle-to-Vehicle Distance Control Laws Using
Hardware-in-the Loop Simulation," (in Korean),
Transactions of the KSME, A, Vol. 26, No. 7, pp.
1401~1407.
[3] Jilkov, V.P., Angelova, D.S. and Semerd- jiev, TZ.
A., 1999, "Design and Comparison of Mode-Set
Adaptive IMM Algorithms for Maneu- vering Target
Tracking," IEEE Transactions on Aerospace and
Electronic Systems, Vol. 35, No. 1, pp.
[4] Munir and D. P. Atherton, Adaptive interacting
multiple model algorithm for tracking a manoeuvring
target, IEE Proceedings on Radar, Sonar, and
Navigation, vol. 142, no. 1, pp. 11-17, February 1995
[5] S. Caveney, Multiple Target Tracking in the
Adaptive Cruise Control Environment Using
Multiple Models and Probabilistic Data Association,
M. S. Thesis, University of California, Berkeley, U.
S. A., 1999