Choomuang, R. & Afzulpurkar, N.

/ Hybrid Kalman Filter/Fuzzy Logic based Position Control of Autonomous Mobile

Robot, pp. 197 - 208, International Journal of Advanced Robotic Systems, Volume 2, Number 3 (2005), ISSN 1729-
8806

Hybrid Kalman Filter/Fuzzy Logic based Position Control of

Autonomous Mobile Robot

Rerngwut Choomuang & Nitin Afzulpurkar

Asian Institute of Technology, School of Advanced Technologies, Pathumthani, Thailand
[email protected] [email protected]

Abstract: This paper describes position control of autonomous mobile robot using combination of Kalman filter and
Fuzzy logic techniques. Both techniques have been used to fuse information from internal and external sensors to
navigate a typical mobile robot in an unknown environment. An obstacle avoidance algorithm utilizing stereo vision
technique has been implemented for obstacle detection. The odometry errors due to systematic-errors (such as unequal
wheel diameter, the effect of the encoder resolution etc.) and/or non-systematic errors (ground plane, wheel-slip etc.)
contribute to various motion control problems of the robot. During the robot moves, whether straight-line and/or arc,
create the position and orientation errors which depend on systematic and/or non-systematic odometry errors. The
main concern in most of the navigating systems is to achieve the real-time and robustness performances to precisely
control the robot movements. The objective of this research is to improve the position and the orientation of robot
motion. From the simulation and experiments, we prove that the proposed mobile robot moves from start position to
goal position with greater accuracy avoiding obstacles.
Keywords: mobile filter, robot, extended Kalman fuzzy logic, obstacle avoidance, stereo vision system.

1. Introduction behavioral approach for local navigation of an

autonomous guided vehicle. Distributed sensor-based
Autonomous robot navigation is a popular research area control strategy for mobile robot navigation has been
which has still many open problems. There are many proposed by (T.M. Sobh; M. Dekhil, A.A. Efros & R.
attempts to solve the problems related to mobile robot Mihali, (2001)). Authors (Toshio Fukuda; Fellow &
navigation in both known and unknown environments. Naoyuki Kubota, (1999)) have focused on a mobile
For the mobile robot navigation system, the motion robotic system with a fuzzy controller and proposed a
control is importance for robot navigation. Utilizing the sensory network that allows the robot to perceive its
odometry data for motion control, (Johann Borenstein, environment. For obstacle avoidance in mobile robot
(1998)) has tested a method called the Internal Position navigation, authors (Alexander Suppes; Frank Suhling &
Error Correction (IPEC) for detection and correction of Michael Hotter, (2001)), have proposed an approach
odometry error without inertial or external-reference using video based obstacle detection for a mobile robot
sensors. Other researchers (Hakyoung Chung & Lauro based on probabilistic evaluation of image data. Obstacle
Ojeda & Johann Borenstein, (2001)) have improved the detection is realized by computing obstacle probability
dead-reckoning accuracy with fiber optic gyroscopes and subsequent application of a threshold operator. They
(FOGS) using Kalman filter technique that fuses the also identified other types of systematic error and non-
sensor data from the FOGS with the odometry system of systematic error related to the mobile robot’s motion
the mobile robot. An Extended Kalman Filter technique such as odometry error due to inadequate encoder
is used for building a localization system (Atanas resolution, unequal wheel diameter, orientation error due
Georgiev & Peter K. Allen, (2004)). This technique to presence of slippage, angular velocity error of the
integrates the sensor data and keeps track of the gyroscope, floor, etc.
uncertainty associated with it. Authors (W.S. Wijesoma; The organization of the paper is as follows: section 2
P.P. Khaw & E.K. Teoh, (2001)) have presented a fuzzy presents a brief description of navigation system of

197
elaborated of fusing together mobile robot developed as 1. Prediction part: This part of the EKF predicts the
part of this research. The proposed approach to solve the future state of the system x̂k− and projects ahead the
robot motion problem is in section 3. In section 4 and 5,
we describe the methodology Extended Kalman Filter, state error covariance matrix Pk− using equation (3)
Fuzzy Logic Control, and Obstacle Avoidance and (4) as:
techniques to implement the proposed system including
hardware and software realization. The experimental xˆk− = f ( xk −1, uk ;0,0) (3)
results are shown in section 6. Conclusion and future Pk− = Ak Pk −1 AkT + Bk ΓBkT + Qk −1 (4)
research direction are given in section 7.
2. Correction part: This part performs the measurement
2. Mobile robot navigation techniques update using equation (5), (6) and (7) as shown
below:
The dead-reckoning principle has been used as standard
technique navigation for mobile robot positioning. The K k = Pk− H kT ( H k Pk− H kT + Rk ) −1 (5)
fundamental idea of dead reckoning is based on a simple
mathematical procedure for determining the present xˆk = xˆk− + K k ( zk − h( xˆk− ,0)) (6)
location of the robot by advancing previous position Pk = ( I − K k H k ) Pk− (7)
through known course and velocity information over a
given length of time (H.R. Everett, 1995). where I is an identity matrix and system (A), input (B),
This allows for used of measured wheel revolutions to and measurement (H) metrics are calculated by Jacobians
calculate displacement relative to floor. The advantage of of the system f (⋅) and measurement h(⋅) (Evgeni Kiriy
odometry is that it provides good short-term accuracy
and allows very high sampling rates. & Martin Buehler, 2002) using equations (8), (9) and
(10).
2.1 Kalman Filter
∂f[i ]
Kalman filter (KF) is widely used in studies of dynamic A[i , j ] = ( xˆk− , uk ;0,0) (8)
systems, analysis, estimation, prediction, processing and ∂x[ j ]
control. Kalman filter is an optimal solution for the ∂f[i ]
discrete data linear filtering problem. KF is a set of B[i , j ] = ( xˆk− , uk ;0,0) (9)
mathematical equations which provide an efficient ∂y[ j ]
computational solution to sequential systems. The filter ∂h[i ]
is very powerful in several aspects: It supports estimation H[ i , j ] = ( xˆk− ;0) (10)
∂x[ j ]
of past, present, and future states (prediction), and it can
do so even when the precise nature of the modeled
system is unknown. The filter is derived by finding 2.2 Fuzzy Logic Control
the estimator for a linear system, subject to additive The idea of fuzzy sets was proposed by Lofti A. Zadeh
white Gaussian noise (Ashraf Aboshosha, 2003). in July 1964. The Fuzzy Logic can be used for
However, the real system is non-linear, Linearization controlling a process (i.e., a plant in control engineering
using the approximation technique has been used to terminology) which is non-linear, the advantage of
handle the non-linear system. This extension of the non- Fuzzy logic control is that it enables control engineers to
linear system is called the Extended Kalman Filter easily implement control strategies which can be used by
(EKF). The general non-linear system and measurement human operator. Its core technique is based on four basic
form is as given by equation (1) and (2) as follows: concepts: fuzzy sets, linguistic variables, possibility
distributions and fuzzy if-then rule (John Yen & Reza
& & & & & Langari, 1999). Fuzzy logic has been applied to various
x k = f ( x k −1 , u k ; wk −1 , γ k −1 ) (1)
& & & control system. The block diagram of fuzzy logic
z k = h( x k ; v k ) (2) controller shown in Fig. 1 is composed of the following
four elements (Kevin M. Passino & Stephen Yurkovich,
where x k is a state vector and z k is a measurement vector 1998):
at the time k, f (⋅) and z (⋅) are non-linear system and
1. A rule-base (a set of If-Then rules), which contains a
measurement function, uk is input to the system,
fuzzy logic quantification of the expert’s linguistic
wk , γ k and v k are the system, input and measurement description of how to achieve good control.
noises respectively.. w k ~ N (0, Q) , γ k ~ N (0, Γ) and 2. An inference mechanism (also called “inference
v k ~ N (0, R) indicate a Gaussian noise with zero mean engine” or “fuzzy inference” module), which
emulates the expert’s decision making in
and covariance matrix Q , Γ and R (Evgeni Kiriy & interpreting and applying knowledge about how best
Martin Buehler, 2002). to control the plant.
3. A fuzzification interface, which converts controller
The main operation of Extended Kalman Filter is divided inputs into information that the inference mechanism
into two parts, the prediction part and the correction part. can easily use to activate and apply rules.

198
4. A defuzzification interface, which converts the where (xl -xr) is the disparity, base line b is the distance
conclusions of the inference mechanism into actual between the left and right cameras, and f is the focal
inputs for the process. length of the camera

Fuzzy Logic Controller 3. Proposed mobile robot design and motion control
system

Defuzzification
Inference
Fuzzification

mechanism
Reference input Inputs Outputs
r(t) u(t) Process y(t) 3.1 The motion control problem
Rule-base
In most robot’s motion, it is very likely that there are
positioning problems when the robot moves from a
starting position to a goal position. This error may
include the orientation error due to slippage during the
Fig. 1. The block diagram of fuzzy logic control system move.
Assume R to be a robot, this robot starts from an initial
2.3 Stereo Vision System position P0 to the goal position Pg in an unknown
The basic idea of a typical stereo vision system is to find workspace. Let O be an obstacle in the required
corresponding points in stereo images and infer 3D workspace W. During a robot move from current position
knowledge about the environment. Corresponding points Pk to the next position Pk+1, there is a change in the
are the projections of a single 3D point in the different position and/or orientation. This change causes accuracy
image spaces. The difference in the position of the problem the robot’s location from in calculating current
corresponding points in their respective images is called position to next position. The new location is not a
disparity (Marti Gaëtan, 1997). The ideal situation is verified true position. The position error increases as the
illustrated in Fig. 2. An arbitrary point in a 3-D scene is robot keeps moving as shown on Fig. 3.
projected into different locations in stereo images.
Obstacle

P0 , θ 0 P1 , θ1 P2 , θ 2 Pg , θ g
Pk ,θ k

P ( x, y , z )

zL zR P2 , θ 2

yL yR Fig. 3. Typical robot move

PL PR
3.2 New mobile robot platform design
A mobile robot has been developed in this research, as
xL xR shown in Fig. 4 and Fig. 14a . It was built on a circular
oL oR
platform with three-wheel configuration. The robot has a
b diameter of 340 mm and a height of 170 mm. It consists
of two driven wheels in the front and single passive
Fig. 2. A typical 3-D scene projected in stereo images wheel in the rear. An optical encoder is mounted on the
axis of each wheel used for the odometry measurement.
Assume that a point P in a surface is projected onto two
10mm
cameras image planes, PL and PR, respectively. When the
imaging geometry is known, the disparity between these position sensor
two locations provides an estimate of the corresponding
Motion Controller board
3-D position. Specifically, the location of P can be
calculated from the known information, PL and PR, and
the internal and external parameters of these two 92.5mm Motor Motor
cameras, such as the focal lengths and positions of two
cameras. Shown in Fig. 2 is a parallel configuration, Encoder
120$
Encoder

where one point, p(x,y,z), is projected onto the left and 208mm
right of the imaging planes at PL(xl,yl) and PR(xr,yr), Bettery
respectively. The coordinates of P can be calculated Driving wheel

(Jung-Hua Wang & Chih-Ping Hslao, 1999) using

equation (11) as follow:
Passive wheel

240mm
§ b( x l + x r ) b( y l + y r ) bf ·
P ( x, y , z ) = ¨¨ , , ¸ (11)
¸
© 2( x l − x r ) 2( x l − x r ) ( x l − x r ) ¹ Fig. 4. Schematic diagram of the mobile robot

199
Three separate modules are used external sensors. A The detail characteristics of sensors are follows:
vector compass module which is attached to the middle
of the robot’s chassis to measure the robot heading. A • Vector compass: this is an electronic compass
position sensor module which is placed underneath robot module with a resolution of 0.1 degree (more
chassis 3 mm above the floor to provide the robot resolution is also possible with appropriate hardware
position. The last module, the vision sensor is placed on and software configurations). Appropriate
top of the upper circular platform of the mobile robot to calibrations techniques provided by the manufacturer
provide the stereo vision capability to the robot. cancle out the errors due to variation of the local
geomagnetic field and geographic north effects.
3.3 Motion control architecture • Wheel encoder: each wheel of the mobile robot is
The proposed mobile robot controller is actually a small equipped with an optical encoder (with 200 counts
computer network consisting of an embedded PC (a per revolution). This allows each axis to provide
PC104) and two microcontrollers (DS89C420). All of separate wheel’s information independently and
them are connected together as show in Fig. 5. accurately. The speed of the motor is set at 50
revolutions per minute, with this speed, the time
interval between each encoder pulse is 100
microseconds.
Master Controller
• Position sensor: this is an electronics sensor based on
encoder circuit, to provide direction and counts on 2
COM 1 IEEE 1394 COM 2 linear axes (x-axis and y-axis). The normal resolution
is 400 counts/inch (with a maximum resolution of
Microcontroller I
Stereo Vision
Sensor Microcontroller II
800 counts/inch). This is an attempt to minimize the
computation due to translation during the robot
movement.
•
X-Axis Y-Axis
Velocity
(PWM signal)
LEAD/LAG
PULSE
(LEAD/LAG)
PULSE
(LEAD/LAG)
PULSE
8-BIT data
Stereo vision sensor: this is a vision sensor which
Vector
consists of a two-camera module (The Digiclops™
Position sensor
M1 M2 En1 En2
Compass Stereo Vision camera) to perform range
measurements for sensing the environment. It is
possible to determine the distance to points in the
Fig. 5. Network architecture of the robot scene reliably in a range from 0.5 to 6.0 meters.
The PC104 is the master controller for the mobile robot, 3.4 Proposed motion control algorithm
it controls all the activities of two slave controllers Fig. 6 shows a block diagram for our proposed motion
(DS89C420s). On a typical robot command, the master control system. In the proposed algorithm stereo vision
controller collects the following information from the is used to calculate the 3D position of obstacles in the
Microcontrollers,: optical encoder feedback data from robot path. Extended Kalman Filter, Fuzzy Logic
both wheels, robot heading data from vector compass technique and Obstacle avoidance have been
module and absolute position from position sensor implemented in the proposed new motion control system
module. The slave controllers also act as protocol (including information from various sensors). The
converters. This eliminates all the protocol conversion proposed algorithm has improved the robot’s movement
burden for the Master controller and allows the PC104 to as well as the robot’s position and orientation.
concentrate only on the necessary control calculations. The operation of the proposed motion control algorithm
Table 1. provides typical protocol conversion burden can be summarized as follows:
times for each microcontroller.
With this approach, the maximum time for updating the 1. Set the start and goal position.
position control command is approximately 8 2. Check obstacles in front of the robot using stereo
milliseconds. The motor response time is set at 20 vision sensor. If the robot finds an obstacle, then it
milliseconds interval. computes the avoidance angle or/and the remaining
As mentioned before there are 4 sensors mounted on the distance to the goal.
mobile robot: vector compass, wheel encoder, position 3. After the robot has computed the angle error and the
sensor and vision sensor. distance error in step 2, robot then calculates a set of
velocity of the left and the right wheels using the
Fuzzy Logic technique.
PC104 No. of 4. While the robot moves, it verifies a motion’s
Baud
protocol’s protocol information from the existing internal and external
rate
burden time bytes sensors. The Extended Kalman Filter is activated for
Microcontroller I 5 ms 5 9600 the robot’s motion.
5. If robot does not reach the required goal position,
Microcontroller II 1 ms 1 9600
the algorithm step 2 is repeated.
Table 1. Typical protocol burden’s time between PC104
and Microcontrollers

200
 Stereo vision [ x' , y ' , z ' ] Stereo vision Image data
Algorithms camera
[ xv , y v , zv ]

Trajectory [ xr , y r , θ r ] Avoidance θ error Fuzzy Logic VL , VR Robot R(θ ), t [ x , y ,θ ]

Robot Real world
Planner Control d error Control Dynamics

[ x f , y f ,θ f ]

update predicted Kinematics Δd , ω Wheel

Correction Prediction
x̂ k xˆ k−+1 , Pk−+1 xk model encoders

zk Absolute position data

Measurements
Optical
Vector
position
sensor compass

Fig. 6. Block diagram of the proposed motion control system

4. The proposed system realization

where the ΔD k is an incremental distance traveled for
For implementing the proposed system, complete control
the left and right wheel §¨ ΔDk = Δd k + Δd k ·¸ and the Δθ k is
R L
operation is divided into separate sections. Each section ¨ ¸
© 2 ¹
has it’s own function. The sections are are: Kinematics
Model (a two wheels mobile robot with optical an incremental change in the orientation
¨ Δθ k = Δd k − Δd k
§ R L ·
encoders), Fuzzy Logic Control (FLC), the Extended ¸
Kalman Filter (EKF) and Obstacle Avoidance. Fuzzy ¨ b ¸
© ¹
Logic technique has been used for the position control.
To improve the position and the orientation of the mobile y − axis
robot Extended Kalman Filter technique has been
chosen. A stereo vision technique has been utilized to
detect obstacle(s) and allow the mobile robot to Δd L
maneuver around the obstacle(s). The above modules ΔD
receive input from multiple sensors such as: encoders,
vector compass, position and stereo vision sensor. The
detail working of each section is given below. θ Δd R
ym
( xr , y r )
4.1 Kinematics Model
First, a kinematic model of a two-wheel robot which
input data measurement from optical encoder has been r
considered as shown in Fig 7. The model calculates the b
robot’movement with a set of Cartesian coordinates (x, y)
and heading ( θ ). These sets are used as the state
variables in Kalman Filter (Evgeni Kiriy & Martin xm x − axis
Buehler, 2002). The wheel encoders provide the required
feedback to the system. The measurement verification
Fig. 7. Schematic robot kinematic model
process has been carried out with the information from
the vector compass and the position sensor.
4.2 Extended Kalman Filter
The kinematics equations for the mobile robot are in the
From the kinematics equation, the system state vector is
following form:
specified as xk = [ xk yk θ k ]T and the input vector as
Δθ k uk = [Δd kR Δd kL ]T and the system function
f x = x k +1 = x k + ΔD k cos(θ k + ) (12)
2 T
f (⋅) as [ f x f y f θ ] . EKF is implemented as follow:
Δθ k The time update equations (15) and (16) are given by:
f y = yk +1 = yk + ΔDk sin(θ k + ) (13)
2
xˆk− = f ( xk −1, uk ;0,0) (15)
fθ = θ k +1 = θ k + Δθ k (14)
Pk− = Ak Pk −1 AkT + B k ΓB kT + Q k −1 (16)

201
where Ak and Bk are the Jacobian matrix of the system the goal angle θ g and the current heading θ r of robot,
and input matrix respectively are obtained by the distance d error which is the difference between the
linearization as shown in equation (17) and (18) (Evgeni current position and the goal position. The calculation is
Kiriy & Martin Buehler, 2002). a simple computation of the basic equation between two
ª ∂f x ∂f x ∂f x º
points. The outputs of Fuzzy Logic Controller are
« ∂x ∂y k ∂θ k » velocities of the left and right motors of the mobile robot.
« k » Fig. 8 shows the proposed Fuzzy Logic Controller block
« ∂f y ∂f y ∂f y »
Ak = « diagram.
∂x ∂y k ∂θ k »
« k » (17)
« ∂fθ ∂fθ ∂fθ »
«¬ ∂xk ∂y k ∂θ k »¼
Fuzzy Logic Controller encoder

ª Δθ k º θ error
vf

«1 0 − ΔDk sin(θ k + 2 )» Fuzzy of vL drive motor

« d error
Δθ k »
motor left v ref control left v
= «0 1 ΔDk cos(θ k + )»
« 2 »
«0 0 1 » Fuzzy of vR v ref drive motor v
« » motor right control right
¬ ¼ vf
ª ∂f x ∂f x º encoder
« R»
« ∂ΔDk ∂ΔDk »
L

« ∂f y ∂f y » Fig. 8. Schematic diagram for the proposed fuzzy logic

Bk = « R»
« ∂ΔDk ∂ΔDk »
L
controller
« ∂fθ ∂fθ »
« R»
¬ ∂ΔDk ∂ΔDk ¼
L
Fuzzy logic technique has been used to generate the
ª1 Δθ k ΔDk Δθ k (18) velocities for each motor of the two wheel mobile robot.
« 2 cos(θ k + 2 ) + 2b sin(θ k + 2 )  The fuzzy logic algorithm is as follows:
«1 Δθ k ΔDk Δθ k
= « sin(θ k + )− cos(θ k + )  Step 1: Define the linguistic variable(s) for input and
«2 2 2b 2
« 1 output system
« − 
¬ b • Compute two input variables: angle error (the
1 Δθ k ΔDk Δθ k º different angle between goal angle and robot’s
 cos(θ k + )− sin(θ k + )
2 2 2b 2 » heading) and distance error (the difference
1 Δθ k ΔDk Δθ k »
 sin(θ k + )+ cos(θ k + )» between the current position and goal position).
2 2 2b 2 »
1 » Compute two output variables: the velocities of
 »
b ¼ the left and right motors.
For the measurement update, the Jacobian of the Step 2: Define fuzzy set
• The fuzzy set values of the fuzzy variables are
function h(⋅) = [ x k y k θ k ]T , measurement matrix H k is
divided as shown in the Table 2.
represented in equation (19) as: • The fuzzy set values are set of overlapping
ª ∂hx ∂hx ∂hx º values represented by triangular shape that is
« ∂x ∂yk ∂θ k » called the fuzzy membership function. Typical
« k » ª1 0 0º
« ∂hy ∂hy ∂hy » «0 1 0» (19) representations of the fuzzy membership
Hk = « =
∂x ∂yk ∂θ k » « » function are shown in Fig. 9, Fig. 10 and Fig. 11
« k » «¬0 0 1»¼
∂
« hθ ∂hθ ∂hθ » below.
«¬ ∂xk ∂yk ∂θ k »¼ Angle: θerror Distance: derror
NL: Negative Large Z: Zero
The measurement update equations are given by (Evgeni
NM: Negative Middle N: Near
Kiriy & Martin Buehler, 2002): NS: Negative Small M: Middle
K k = Pk− H kT ( H k Pk− H kT + Rk ) −1 (20) ZE: Zero F: Far
PS: Positive Small
xˆk = xˆk− + K k ( zk − h( xˆk− ,0)) (21) PM: Positive Middle
PL: Positive Large
Pk = ( I − K k H k ) Pk− (22) Table 2. Notations for the Fuzzy Logic
With these set of equations all the necessary components
of EKF have been defined. ZE

NL NM NS PS PM PL
4.3 Fuzzy Logic Control
The objective to use the Fuzzy Logic Control is to
generate the velocities for both the left and the right
motors of the robot, which allows the robot to move from − 90 $ − 60 $ − 40$ − 20$ 0 $ 20 $ 40 $ 60$ 90 $
start position to goal position. The specification of the
control input and output variables have been set. These Fig. 9. Representation of the fuzzy membership functions
inputs are: angle θ error which is the difference between of angle error

202
 • Centroid defuzzification or Center of Area
S
method calculated by equation (23) as:
FSBW BW SLBW SLFW FW FSFW
n

¦ v × min(μ θ
i =1
i i
error
, μ i error )
d

v= n
(23)
−100 −80 −60 −40 −20 0 20 40 60 80 100 θ error
¦ min(μ
i =1
i , μ i error
d
)
Fig. 10. Representation of the fuzzy membership
functions of velocity output θ
where v is defined the fuzzy output of i th rule, μ i error
and μ i error are the membership value of the input fuzzy
Z d

N M F set

4.4 Obstacle Avoidance

m
0 1 2 3 4 5
4.4.1 Detecting an obstacle
Fig. 11. Representation of the fuzzy membership In this research, stereo vision technique has been used
functions of distance error for obstacle detection. The flowchart for stereo vision
algorithm is shown in Fig. 12. We capture the left and
Step 3: Define fuzzy rules right images at the same time using stereo vision camera.
• The operation of the system utilizes the fuzzy For noise reduction, we used low-pass filter for reducing
rules. The notation for making fuzzy logic rules the noise in the images. Edge detection technique is then
is: Fast Forward (FSFW), Forward (FW), Slow used to find edges of each images. In the next step, we
Forward (SLFW), Stop (S), Slow Backward find the matched corresponding points between the left
(SLBW), Backward (BW) and Fast Backward and right images. Triangulation method is then applied to
(FSBW). The fuzzy rules are shown in the calculate the depth for the matched corresponding point.
Table 3. and 4. below Various tests have been done using the above stereo
vision system, We found that an accurate range for
Z N M F obstacle detection is between 0.5 to 6.0 meters.
NM S FSFW FSFW FSFW
NM S FW FW FW
NS S SLFW FW FW
ZE S SLFW FW FSFW
PS S S SLFW SLFW Raw input images
PM S S SLFW SLFW
PL S S FW FW
Table 3. Fuzzy rule of the right motor

Z N M F Pre-processing
NM S S BW BW
NM S S SLBW SLBW Low-pass filtering
NS S S SLBW SLBW
ZE S SLBW BW FSBW
PS S SLBW BW BW Edge detecting
PM S BW BW BW
PL S FSBW FSBW FSBW Pre-processed images
Table 4. A typical fuzzy rule of the left motor

Examples of fuzzy rule employed by the path following Stereo processing

behavior are shown below:
Stereo matching
ℜ1 : IF θerror is zero angle (ZE) and derror is distance
near (N) THEN the left motor is Slow Depth calculation
Backward (SLBW)
ℜ2 : IF θerror is zero angle (ZE) and derror is distance
near (N) THEN the right motor is Slow
Forward (SLFW)
Depth Images
Step 4: Defuzzification
• The output action given by the input conditions,
the defuzzification computes the velocity output
of the fuzzy controller. Fig. 12. Typical flowchart of a stereo vision system

203
The algorithm for obstacle detection is as follows:
derror = ( xg − xr ) 2 + ( y g − yr ) 2 (26)
If d obs ≥ 0.5 and dobs ≤ 6.0 then
where θ error is error angle for input fuzzy logic, d error is
found obstacle( s )
error distance for inputs to fuzzy logic, θ obs is an
return ( xobs , yobs , zobs )
obstacle angle, θ avoid is avoiding angle, θ g is goal angle,
else
θ r is the heading of the mobile robot, ( x g , y g ) is a
not found obstacle( s )
destination position, and ( x r , y r ) is a current position of
return (0,0,0)
the robot.
end
To improve the precision in the motion control of the
where d obs is the distance between the robot and the robot motion, Extended Kalman Filter, the Fuzzy Logic
obstacle. control and the obstacle avoidance techniques have been
implemented. We present the results from the
4.4.2 Avoiding an obstacle experiments in section 6 to validate the above
To avoid the existing obstacles, a series of movement algorithms.
commands must be generated to allow the mobile robot
to move pass the obstacles. In order to do so, the 5. Hardware and software implementation
program within the mobile robot must analyze the
incoming coordinates from 4.4.1. Additional data such as To test the proposed algorithms, a mobile robot which
the obstacle angle θobs is calculated which eventually lead corresponds to the schematic design has been constructed
to the calculation avoidance angle θ avoid for the mobile and is shown in Fig. 4 and Fig 14a. The maximum speed
robot to move through the detected obstacles. of the robot is approximately 0.45 meters per second.
The interfacing circuit of the velocity controller which
receives signals from the wheel encoder is shown in Fig.
y − axis y '− axis 14a. The interfacing circuit in Fig. 14b also receives
Obstacle
signals from the vector compass and the position sensor.
In both cases, the DALLAS 89C420 microcontroller has
( xobs , yobs )
Goal
been used. The software interface for the DALLAS
d obs
( xg , y g )
89C420 microcontrollers and sensor module has written
θd dg in assembly language (MCS-51 family instruction).
θr
θg Software interface for the stereo camera module is
θ obs
x'−axis written in Microsoft Visual C++ (using vision library
( xr , y r )
from the Point Grey Company). The simulation program
is written using MATLAB.
( x0 , y0 ) x − axis

Fig. 13. Schematic for obstacle avoidance

The angle θd is the difference between the heading angle

θr and the obstacle angle θobs . It’s calculated by equation
(24) as follow:

θ d = θ r − θobs (24)

Avoiding angle, θavoid is then calculated as:

Fig. 14a. An experimental mobile robot
if θ d < 0 then
θ avoid = θ g − θobs PWM1
TX P0.0 PWM 2 EN1 OUTA TX P 0.0
RX P 0.1
DIR1
EN 2 RX P0.1
else GND P0.2
P 0.3
DIR 2
74LS 00 INA
INB M1
GND P 0.2
P0.3
Vector Compass
INC P 0.4

θ avoid = θ g + θobs
IND OUTB P 0.5
OUTC P 0.6
P 0.7
L 298 M2
DS89C420 DS89C420
end OUTD
CHA CHA
INT 1 INT 0 INT 0
INT1
Position Sensor
INT 2
The equations for inputs to the Fuzzy Logic control are CHB
P1.1 P1.0
CHB
INT 3

as follow: (a) (b)

θ error = θ r − θ avoid (25) Fig. 14b. Interface circuit for the mobile robot controller

204
6. Experimental results Heading of robot
0.6
True position
FLC
The mobile robot with the proposed algorithm has been 0.4 FLC & EKF
tested in various unknown environments with obstacles.
In all experiments, the robot moved from the start 0.2

positions to the goal positions correctly and the rest of

0
this section provides results form the simulation d
]
ar[
experirments performed using the above control at
-0.2
e
ht
algrothm
-0.4
6.1 Simulation Results
The simulations were performed using different maps -0.6

consisting goal and obstacles.

-0.8
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
time[sec]
Obstacle 1
50

Obstacle 2
Fig. 15d. The comparison of robot heading using FLC
100 and FLC & EKF
150

Goal(350,200)
Start(30,200)
]
m 200 50 Obstacle 3
m
[
y
Obstacle 1
250 100

300 150

Start(30,200)
Obstacle 3 Goal(350,220)
]
350 m 200
m
[
y

400 250
50 100 150 200 250 300 350 400
x[mm]

300

Fig. 15a. The trajectory of the robot movement 350

Obstacle 2
Position of x
214 400
50 100 150 200 250 300 350 400
True position x[mm]

212 FLC
FLC & EKF

210
Fig. 16a. The trajectory of the robot movement
Position of x
225
208
True position
FLC
206 220 FLC & EKF
]
m
m
[ 204
y
215
202
210
200 ]
m
m
[
198 y 205

196
0 50 100 150 200 250 300 350 400 200
x[mm]

Fig. 15b. The comparison of robot motion in xy-axis 195

using only FLC and FLC & EKF

190
0 50 100 150 200 250 300 350 400
Position of y
214 x[mm]
True position
212 FLC Fig. 16b. The comparison of robot motion in xy-axis
FLC & EKF
using only FLC and FLC & EKF
210

208 Fig. 15a to show the trajectory followed by the robot

206
avoiding an obstacle on the left hand side of the robot.
]
m The simulation results are shown in Fig. 15b, Fig. 15c
and Fig. 15d respectively. The “ − • − ” represents the
m
[ 204
y

202 true position/orientation of the robot motion, the “ −Δ − ”

200
represents trajectory after the position/orientation error
are compensated using the fuzzy logic technique and the
198
“ −o − ” represents the trajectory after
196
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5
position/orientation errors are compensated by the
time[sec] Kalman Filter and Fuzzy Logic techniques. Fig. 16 and
Fig. 15c. The comparison of robot motion in y-axis using Fig. 17 show similar experiments using other maps.
FLC and FLC & EKF Second simulation experiment (Fig. 16a) has three

205
obstacles in the workspace. The robot moves from start Position of x
260
position (30,200) to destination position (350,220). The True position
FLC
results changes of xy-position, y-position and heading of 250 FLC & EKF
robot are show in Fig. 16b, Fig. 16c and Fig. 16d
respectively. 240

230
Position of y
]
220 m
True position m
[
y 220
FLC
FLC & EKF
215 210

200
210

]
m 190
m 0 50 100 150 200 250 300 350
[
y x[mm]
205

Fig. 17b. The comparison of robot motion in xy-axis

200
using only FLC and FLC & EKF
Position of y
260
True position
195 FLC
0 1 2 3 4 5 6 250 FLC & EKF
time[sec]

Fig. 16c. The comparison of robot motion in y-axis using 240

FLC and FLC & EKF

230
Heading of robot ]
0.6 m
True position m
[
y 220
FLC
0.4 FLC & EKF
210

0.2
200

0
]
d 190
ar[
at 0 1 2 3 4 5 6
e -0.2 time[sec]
ht

-0.4
Fig. 17c. The comparison of robot motion in y-axis using
FLC and FLC & EKF
-0.6 Heading of robot
0.8
True position
FLC
-0.8
0 1 2 3 4 5 6 0.6 FLC & EKF
time[sec]

Fig. 16d. The comparison of robot heading using FLC 0.4

and FLC & EKF

0.2
]
d
ar[
Third simulation experiment, the robot moves from the at
e 0
ht
start position (30,200) to destination position (350,260)
by the workspace has seven obstacles (Fig. 17a). Fig. -0.2

17b, Fig. 17c and Fig. 17d show the results of xy-
position, y-position and heading changes of robot -0.4

respectively.
-0.6
0 1 2 3 4 5 6
time[sec]
Obstacle 3
50
Fig. 17d. The comparison of robot heading using FLC
100
Obstacle 1
and FLC & EKF
Obstacle 4
150

]
m 200 Goal(340,260)
m
[
y

250
Start(30,200)
Obstacle 2 Obstacle 5
300
Obstacle 7
The quantitative results of the simulation experiments
are shown n the above Table 5. From the table we can
350 Obstacle 6
conclude that, the robot can move from start position to
400
50 100 150 200 250 300 350 400
goal position with higher accuracy when FLC and EKF
x[mm]
is used together improving the performance of robot
Fig. 17a. The trajectory of the robot movement motion.

206
 FLC FLC FLC & EKF Percentage Error
Simulation Sampling time (sec) Action (true position) (error position) (error position)
FLC FLC & EKF
X (mm) Y (mm) X (mm) Y (mm) X (mm) Y (mm) X (%) Y (%) X (%) Y (%)
At 0.5 straight 67.06 200.00 68.93 200.17 68.05 200.13 2.79 0.08 1.48 0.06
At 0.7 ccw 81.08 200.00 81.28 199.90 81.19 199.91 0.24 0.04 0.13 0.04
1 At 0.8 cw 87.78 202.07 87.34 201.79 87.56 201.87 0.50 0.13 0.25 0.09
At 1.2 cw 114.73 209.81 114.27 209.84 114.47 209.87 0.40 0.01 0.22 0.03
At 2.0 straight 170.52 209.16 171.54 209.21 171.07 209.23 0.59 0.02 0.31 0.03
At 0.5 straight 62.94 202.34 63.68 201.97 63.34 201.99 1.17 0.18 0.63 0.17
At 1.0 straight 75.59 203.12 76.60 204.72 76.09 204.47 1.33 0.78 0.67 0.66
2 At 1.8 ccw 81.91 203.51 83.30 204.63 82.63 204.44 1.70 0.55 0.87 0.45
At 1.9 cw 107.20 205.07 106.73 204.02 106.97 204.19 0.44 0.51 0.21 0.42
At 2.0 cw 157.04 206.33 157.04 207.81 157.11 207.59 0.00 0.71 0.04 0.61
At 0.5 straight 58.42 205.49 58.06 207.66 58.12 207.32 0.61 1.05 0.51 0.88
At 0.6 cw 69.58 205.63 70.06 206.05 69.85 206.01 0.68 0.20 0.38 0.18
3 At 0.7 ccw 75.18 204.90 73.84 206.30 74.51 206.00 1.78 0.68 0.88 0.53
At 1.6 ccw 97.61 202.32 97.19 201.54 97.36 201.67 0.42 0.38 0.25 0.32
At 1.7 cw 142.20 203.99 142.00 204.21 142.08 204.19 0.13 0.10 0.08 0.09

Table 5. The sample results from the simulation experiments

6.2 Experiments (c), (d), (e), (f), (g), (h) and finishes. The ‘Rerngwut I’
In this section, the results from two experiments robot which has a maximum speed of 0.45 meters per
performed in unknown environments such as a corridor second can move from the starting position, avoid the
(Fig. 18) and a non-linear type passage way (Fig. 19) are obstacle, pass through the corridor and reach the goal
presented. Fig. 18 shows a series of snap-shots of the position successfully in both cases.
mobile robot passing through an unknown corridor, starts
from (a), then proceeds through (b), (c), (d), (e) and
finishes at (f).

(a) (b)

(a) (b)

(c) (d)

(c) (d)

(e) (f)

(e) (f)
Fig. 18. A typical robot motion passing through an
unknown corridor

Similarly in Fig. 19, another series of snap-shot motions

passing through an unknown non-linear passage way is
shown the robot starts from (a) then passes through (b), (g) (h)

207
 Domenico Longo; Giovanni Muscato & Vincenzo Sacco
(2002). Localization using fuzzy and Kalman
filtering data fusion, http://www.robovolc.dees.unict
.it/download/files/Clawar2002.pdf, Accessed: 2003-
09-12
E. Fabrizi G. Oriolo; S. Panzieri & G. Ulivi (2000).
Mobile robot localization via fusion of ultrasonic
and inertial sensor data, http://panzieri.dia.uniroma3.
(i) (j) it/Articoli/ISORA2000.pdf, Accessed: 2003-09-12
Evgeni Kiriy & Martin Buehler (2002). Three-state
Extended Kalman Filter for Mobile Robot
Localization, http://www.cim.mcgill.ca/~kiriy/
publications/eKf-3state.pdf, Accessed: 2003-05-03
H.R. Everett (1995). Sensor for mobile robot theory and
application, A K Peter Publisher, ISBN 1-56881-
048-2, Wellesley, Massachusetts
(k) (l) J. Borenstein & Y. Koren (1998). Real-time Obstacle
Avoidance for Fast Mobile Robots, IEEE
Fig. 19. A typical of robot motion passing through an Transactions on Systems, Man, and Cybernetics,
unknown non-linear type passage way Vol. 19, No. 5, Sept./Oct. 1989, pp. 1179-1187
Johann Borenstein & Yoram Korem (1995). Motion
7. Conclusion Control analysis of a mobile robot, Transaction of
ASME, Journal of Dynamics, Measurement and
This research presents a combination of the Extended Control, Vol. 109, No. 2, pp. 73-79
Kalman Filter, the Fuzzy Logic Control and the Obstacle Johann Borenstein (1998). Experimental Results from
avoidance technique which fuses information from Internal Odometry Error Correction with the
different sensors. The proposed system consists of three OmniMate Mobile Robot, IEEE Transactions on
parts and each of them has it’s own function, the Fuzzy Robotics an Automation, Vol. 14,No. 6, December
Logic technique for the position control, the Extended 1998, pp 963-969
Kalman Filter technique is for the position predition and John Yen & Reza Langari (1999), Fuzzy Logic, Prentice-
correction and the stereo vision technique for the Hall Inc., ISBN 0-13-525817-0, Simon & Schuster /
obstacle avoidance. The stereo vision sensor supplies the A Viacom Company, Upper Saddle River, New
obstacle detection. The simulations and experiments of Jersey 0748
robot motion show the results for robot’s in which the Jung-Hua Wang & Chih-Ping Hslao (1999). On
robot can move from start position to goal position Disparity Matching in Stereo Vision via a Neural
avoiding obstacles in an unknown environment. The Network Framework, Proc. Natl. Sci. Counc.
proposed algorithm improves the accuracies of the ROC(A), Vol. 23, No. 5, 1999, pp. 665-678
position and the orientation of the robot motion problem. Kenneth R. Castleman (1996). Digital Image Processing,
In future work, we propose to apply this algorithm to Prentice Hall, ISBN 0-13-211467-4, Upper Saddle
other platform such as “omni-direction mobile robot” River, New Jersey 0748
(non-steering wheel type) because it may improve the Kevin M. Passino & Stephen Yurkovich (1998). Fuzzy
position and orientation control for achieving a smooth Control, Addison Wesley Longman Inc., ISBN 0-
robot motion. 201-18074-X
Marti Gaëtan (1997). Stereo Vision mobile robotics,
8. References Micro Engineering Laboratory, Swiss Federal
Institute of Technology lansaunne, http://imtsg7.epfl
Alexander Suppes; Frank Suhling & Michael Hotter .ch /papers/LUCERNE97-GM.doc, Accessed: 2002-
(2001). Robust Obstacle Detection from 20-11
Stereoscopic Image Sequences Using Kalman Filter, T.M. Sobh; M. Dekhil, A.A. Efros & R. Mihali (2001).
pp. 385-391, Pattern Recognition 23rd DAGM Logic Control For Mobile Robot, International
Symposium, September 2001, Munich, Germany Journal of Robotics and Automation, Vol. 16, No. 2,
Ashraf Aboshosha (2003). Adaptation of Rescue Robot pp. 74-82
Behaviour in Unknown Terrains Based on Toshio Fukuda; Fellow & Naoyuki Kubota (1999). An
Stochastic and Fuzzy Logic Approaches, Intelligent Robotic System Based on a Fuzzy
http://www-ra.informatik.uni-tuebingen.de, Approach, Proceedings of the IEEE, Vol. 87 No. 9,
Accessed: 2003-09-14 September 1999, pp. 1448-1470
Atanas Georgiev & Peter K. Allen (2004). Localization W.S. Wijesoma; P.P. Khaw & E.K. Teoh (2001). Sensor
Methods for a Mobile Robot in Urban Environments Modelling and Fusion For Fuzzy Navigation of an
IEEE, Transactions on robotics, Vol. 20. No. 5, AGV, International Journal of Robotics and
October 2004, pp 851-864 Automation, Vol. 16, No. 2, pp. 14-24

208