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

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

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

The organization of the paper is as follows: section 2 presents a brief description of navigation system of elaborated of fusing together mobile robot developed as part of this research. The proposed approach to solve the robot motion problem is in section 3. In section 4 and 5, we describe the methodology Extended Kalman Filter, Fuzzy Logic Control, and Obstacle Avoidance techniques to implement the proposed system including hardware and software realization. The experimental results are shown in section 6. Conclusion and future research direction are given in section 7.

2. Mobile robot navigation techniques

The dead-reckoning principle has been used as standard technique navigation for mobile robot positioning. The fundamental idea of dead reckoning is based on a simple mathematical procedure for determining the present location of the robot by advancing previous position through known course and velocity information over a given length of time (H.R. Everett, 1995).

This allows for used of measured wheel revolutions to calculate displacement relative to floor. The advantage of odometry is that it provides good short-term accuracy and allows very high sampling rates.

2.1 Kalman Filter

Kalman filter (KF) is widely used in studies of dynamic systems, analysis, estimation, prediction, processing and control. Kalman filter is an optimal solution for the discrete data linear filtering problem. KF is a set of mathematical equations which provide an efficient computational solution to sequential systems. The filter is very powerful in several aspects: It supports estimation 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 the estimator for a linear system, subject to additive white Gaussian noise (Ashraf Aboshosha, 2003). However, the real system is non-linear, Linearization using the approximation technique has been used to handle the non-linear system. This extension of the nonlinear system is called the Extended Kalman Filter (EKF). The general non-linear system and measurement form is as given by equation (1) and (2) as follows:

{\vec{x}}_{k} = f ({\vec{x}}_{k - 1}, {\vec{u}}_{k}; {\vec{w}}_{k - 1}, {\vec{γ}}_{k - 1})

(1)

{\vec{Z}}_{k} = h ({\vec{x}}_{k}; {\vec{v}}_{k})

(2)

where x_k is a state vector and z_k is a measurement vector at the time k, f(·) and z(·)are non-linear system and measurement function, u_k is input to the system, w_k, γ_k and v_k are the system, input and measurement noises respectively‥ w_k ∼ N(0, Q), γ_k ∼ N(0, Γ) and v_k ∼ N(0, R) indicate a Gaussian noise with zero mean and covariance matrix Q, Γ and R (Evgeni Kiriy & Martin Buehler, 2002).

The main operation of Extended Kalman Filter is divided into two parts, the prediction part and the correction part.

Prediction part: This part of the EKF predicts the future state of the system ${\hat{x}}_{k}^{-}$ and projects ahead the state error covariance matrix $P_{k}^{-}$ using equation (3) and (4) as:

{\hat{x}}_{k}^{-} = f (x_{k - 1}, u_{k}; 0, 0)

(3)

P_{k}^{-} = A_{k} P_{k - 1} A_{k}^{T} + B_{k} Γ B_{k}^{T} + Q_{k - 1}

(4)

Correction part: This part performs the measurement update using equation (5), (6) and (7) as shown below:

K_{k} = P_{k}^{-} H_{k}^{T} (H_{k} P_{k}^{-} H_{k}^{T} + R_{k})^{- 1}

(5)

{\hat{x}}_{k} = {\hat{x}}_{k}^{-} + K_{k} (z_{k} - h ({\hat{x}}_{k}^{-}, 0))

(6)

P_{k} = (I - K_{k} H_{k}) P_{k}^{-}

(7)

where I is an identity matrix and system (A), input (B), and measurement (H) metrics are calculated by Jacobians of the system f(·) and measurement h(·) (Evgeni Kiriy & Martin Buehler, 2002) using equations (8), (9) and (10).

A_{[i, j]} = \frac{\partial f_{[i]}}{\partial x_{[j]}} ({\hat{x}}_{k}^{-}, u_{k}; 0, 0)

(8)

B_{[i, j]} = \frac{\partial f_{[i]}}{\partial y_{[j]}} ({\hat{x}}_{k}^{-}, u_{k}; 0, 0)

(9)

H_{[i, j]} = \frac{\partial h_{[i]}}{\partial x_{[j]}} ({\hat{x}}_{k}^{-}; 0)

(10)

2.2 Fuzzy Logic Control

The idea of fuzzy sets was proposed by Lofti A. Zadeh in July 1964. The Fuzzy Logic can be used for controlling a process (i.e., a plant in control engineering terminology) which is non-linear, the advantage of Fuzzy logic control is that it enables control engineers to easily implement control strategies which can be used by human operator. Its core technique is based on four basic concepts: fuzzy sets, linguistic variables, possibility distributions and fuzzy if-then rule (John Yen & Reza Langari, 1999). Fuzzy logic has been applied to various control system. The block diagram of fuzzy logic controller shown in Fig. 1 is composed of the following four elements (Kevin M. Passino & Stephen Yurkovich, 1998):

Fig. 1.

The block diagram of fuzzy logic control system

A rule-base (a set of If-Then rules), which contains a fuzzy logic quantification of the expert's linguistic description of how to achieve good control.

An inference mechanism (also called “inference engine” or “fuzzy inference” module), which emulates the expert's decision making in interpreting and applying knowledge about how best to control the plant.

A fuzzification interface, which converts controller inputs into information that the inference mechanism can easily use to activate and apply rules.

A defuzzification interface, which converts the conclusions of the inference mechanism into actual inputs for the process.

2.3 Stereo Vision System

The basic idea of a typical stereo vision system is to find corresponding points in stereo images and infer 3D knowledge about the environment. Corresponding points are the projections of a single 3D point in the different image spaces. The difference in the position of the corresponding points in their respective images is called disparity (Marti Gaëtan, 1997). The ideal situation is illustrated in Fig. 2. An arbitrary point in a 3-D scene is projected into different locations in stereo images.

Fig. 2.

A typical 3-D scene projected in stereo images

Assume that a point P in a surface is projected onto two cameras image planes, P_L and P_R, respectively. When the imaging geometry is known, the disparity between these two locations provides an estimate of the corresponding 3-D position. Specifically, the location of P can be calculated from the known information, P_L and P_R, and the internal and external parameters of these two cameras, such as the focal lengths and positions of two cameras. Shown in Fig. 2 is a parallel configuration, where one point, p(x,y,z), is projected onto the left and right of the imaging planes at P_L(x_l,y_l) and P_R(x_r,y_r), respectively. The coordinates of P can be calculated (Jung-Hua Wang & Chih-Ping Hslao, 1999) using equation (11) as follow:

P (x, y, z) = (\frac{b (x_{l} + x_{r})}{2 (x_{l} + x_{r})}, \frac{b (y_{l} + y_{r})}{2 (x_{l} + x_{r})}, \frac{b f}{(x_{l} + x_{r})})

(11)

where (x_l -x_r) is the disparity, base line b is the distance between the left and right cameras, and f is the focal length of the camera

3. Proposed mobile robot design and motion control system

3.1 The motion control problem

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 move.

Assume R to be a robot, this robot starts from an initial position P₀ to the goal position P_g in an unknown workspace. Let O be an obstacle in the required workspace W. During a robot move from current position P_k to the next position P_k+1, there is a change in the position and/or orientation. This change causes accuracy problem the robot's location from in calculating current position to next position. The new location is not a verified true position. The position error increases as the robot keeps moving as shown on Fig. 3.

Fig. 3.

Typical robot move

3.2 New mobile robot platform design

A mobile robot has been developed in this research, as shown in Fig. 4 and Fig. 14a. It was built on a circular platform with three-wheel configuration. The robot has a diameter of 340 mm and a height of 170 mm. It consists of two driven wheels in the front and single passive wheel in the rear. An optical encoder is mounted on the axis of each wheel used for the odometry measurement.

Fig. 4.

Schematic diagram of the mobile robot

Three separate modules are used external sensors. A vector compass module which is attached to the middle of the robot's chassis to measure the robot heading. A position sensor module which is placed underneath robot chassis 3 mm above the floor to provide the robot position. The last module, the vision sensor is placed on top of the upper circular platform of the mobile robot to provide the stereo vision capability to the robot.

3.3 Motion control architecture

The proposed mobile robot controller is actually a small computer network consisting of an embedded PC (a PC104) and two microcontrollers (DS89C420). All of them are connected together as show in Fig. 5.

Fig. 5.

Network architecture of the robot

The PC104 is the master controller for the mobile robot, it controls all the activities of two slave controllers (DS89C420s). On a typical robot command, the master controller collects the following information from the Microcontrollers,: optical encoder feedback data from both wheels, robot heading data from vector compass module and absolute position from position sensor module. The slave controllers also act as protocol converters. This eliminates all the protocol conversion burden for the Master controller and allows the PC104 to concentrate only on the necessary control calculations. Table 1. provides typical protocol conversion burden times for each microcontroller.

Table 1.

Typical protocol burden's time between PC104 and Microcontrollers

	PC104 protocol's burden time	No. of protocol bytes	Baud rate
Microcontroller I	5 ms	5	9600
Microcontroller II	1 ms	1	9600

With this approach, the maximum time for updating the position control command is approximately 8 milliseconds. The motor response time is set at 20 milliseconds interval.

As mentioned before there are 4 sensors mounted on the mobile robot: vector compass, wheel encoder, position sensor and vision sensor.

The detail characteristics of sensors are follows:

Vector compass: this is an electronic compass module with a resolution of 0.1 degree (more resolution is also possible with appropriate hardware and software configurations). Appropriate calibrations techniques provided by the manufacturer cancle out the errors due to variation of the local geomagnetic field and geographic north effects.

Wheel encoder: each wheel of the mobile robot is equipped with an optical encoder (with 200 counts per revolution). This allows each axis to provide separate wheel's information independently and 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.

Position sensor: this is an electronics sensor based on encoder circuit, to provide direction and counts on 2 linear axes (x-axis and y-axis). The normal resolution is 400 counts/inch (with a maximum resolution of 800 counts/inch). This is an attempt to minimize the computation due to translation during the robot movement.

Stereo vision sensor: this is a vision sensor which consists of a two-camera module (The Digiclops™ Stereo Vision camera) to perform range measurements for sensing the environment. It is possible to determine the distance to points in the scene reliably in a range from 0.5 to 6.0 meters.

3.4 Proposed motion control algorithm

Fig. 6 shows a block diagram for our proposed motion control system. In the proposed algorithm stereo vision is used to calculate the 3D position of obstacles in the robot path. Extended Kalman Filter, Fuzzy Logic technique and Obstacle avoidance have been implemented in the proposed new motion control system (including information from various sensors). The proposed algorithm has improved the robot's movement as well as the robot's position and orientation.

Fig. 6.

Block diagram of the proposed motion control system

The operation of the proposed motion control algorithm can be summarized as follows:

Set the start and goal position.

Check obstacles in front of the robot using stereo vision sensor. If the robot finds an obstacle, then it computes the avoidance angle or/and the remaining distance to the goal.

After the robot has computed the angle error and the distance error in step 2, robot then calculates a set of velocity of the left and the right wheels using the Fuzzy Logic technique.

While the robot moves, it verifies a motion's information from the existing internal and external sensors. The Extended Kalman Filter is activated for the robot's motion.

If robot does not reach the required goal position, the algorithm step 2 is repeated.

4. The proposed system realization

For implementing the proposed system, complete control operation is divided into separate sections. Each section has it's own function. The sections are are: Kinematics Model (a two wheels mobile robot with optical encoders), Fuzzy Logic Control (FLC), the Extended Kalman Filter (EKF) and Obstacle Avoidance. Fuzzy Logic technique has been used for the position control. To improve the position and the orientation of the mobile 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 maneuver around the obstacle(s). The above modules receive input from multiple sensors such as: encoders, vector compass, position and stereo vision sensor. The detail working of each section is given below.

4.1 Kinematics Model

First, a kinematic model of a two-wheel robot which input data measurement from optical encoder has been considered as shown in Fig 7. The model calculates the 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 Buehler, 2002). The wheel encoders provide the required feedback to the system. The measurement verification process has been carried out with the information from the vector compass and the position sensor.

Fig. 7.

Schematic robot kinematic model

The kinematics equations for the mobile robot are in the following form:

f_{x} = x_{k + 1} = x_{k} + Δ D_{k} \cos (θ_{k} + \frac{Δ θ_{k}}{2})

(12)

f_{y} = y_{k + 1} = y_{k} + Δ D_{k} \sin (θ_{k} + \frac{Δ θ_{k}}{2})

(13)

f_{θ} = θ_{k + 1} = θ_{k} + Δ θ_{k}

(14)

where the ΔD_k is an incremental distance traveled for the left and right wheel $(Δ D_{k} = \frac{Δ d_{k}^{R} + Δ d_{k}^{L}}{2})$ and the Δθ_k is an incremental change in the orientation $(Δ θ_{k} = \frac{Δ d_{k}^{R} + Δ d_{k}^{L}}{b})$

4.2 Extended Kalman Filter

From the kinematics equation, the system state vector is specified as x_k = [x_k y_k θ_k] ^T and the input vector as u_k = [Δd_k Δd_k^L] ^T and the system function f(·) as[f_x f_y f_θ] ^T . EKF is implemented as follow: The time update equations (15) and (16) are given by:

{\hat{x}}_{k}^{-} = f (x_{k - 1}, u_{k}; 0, 0)

(15)

P_{k}^{-} = A_{k} P_{k - 1} A_{k}^{T} + B_{k} Γ B_{k}^{T} + Q_{k - 1}

(16)

where A_k and B_k are the Jacobian matrix of the system and input matrix respectively are obtained by linearization as shown in equation (17) and (18) (Evgeni Kiriy & Martin Buehler, 2002).

\begin{aligned} A_{k} & = [\begin{matrix} \frac{\partial f_{x}}{\partial x_{k}} \frac{\partial f_{x}}{\partial y_{k}} \frac{\partial f_{x}}{\partial θ_{k}} \\ \frac{\partial f_{y}}{\partial x_{k}} \frac{\partial f_{y}}{\partial y_{k}} \frac{\partial f_{y}}{\partial θ_{k}} \\ \frac{\partial f_{θ}}{\partial x_{k}} \frac{\partial f_{θ}}{\partial y_{k}} \frac{\partial f_{θ}}{\partial θ_{k}} \end{matrix}] \\ = [\begin{matrix} 1 & 0 & - Δ D_{k} \sin (θ_{k} + \frac{Δ θ_{k}}{2}) \\ 0 & 1 & Δ D_{k} \cos (θ_{k} + \frac{Δ θ_{k}}{2}) \\ 0 & 0 & 1 \end{matrix}] \end{aligned}

(17)

\begin{aligned} B_{k} & = [\begin{matrix} \frac{\partial f_{x}}{\partial Δ D_{k}^{L}} & \frac{\partial f_{x}}{\partial Δ D_{k}^{R}} \\ \frac{\partial f_{y}}{\partial Δ D_{k}^{L}} & \frac{\partial f_{y}}{\partial Δ D_{k}^{R}} \\ \frac{\partial f_{θ}}{\partial Δ D_{k}^{L}} & \frac{\partial f_{θ}}{\partial Δ D_{k}^{R}} \end{matrix}] \\ = [\begin{matrix} \frac{1}{2} \cos (θ_{k} + \frac{Δ θ_{k}}{2}) + \frac{Δ D_{k}}{2 b} \sin (θ_{k} + \frac{Δ θ_{k}}{2}) & \dots \\ \frac{1}{2} \sin (θ_{k} + \frac{Δ θ_{k}}{2}) - \frac{Δ D_{k}}{2 b} \cos (θ_{k} + \frac{Δ θ_{k}}{2}) & \dots \\ - \frac{1}{b} & \dots \end{matrix} \\ \begin{matrix} \dots & \frac{1}{2} \cos (θ_{k} + \frac{Δ θ_{k}}{2}) - \frac{Δ D_{k}}{2 b} \sin (θ_{k} + \frac{Δ θ_{k}}{2}) \\ \dots & \frac{1}{2} \sin (θ_{k} + \frac{Δ θ_{k}}{2}) + \frac{Δ D_{k}}{2 b} \cos (θ_{k} + \frac{Δ θ_{k}}{2}) \\ \dots & \frac{1}{b} \end{matrix}] \end{aligned}

(18)

For the measurement update, the Jacobian of the function h(·) = [x_k y_k θ_k] ^T , measurement matrix H_k is represented in equation (19) as:

H_{k} = [\begin{matrix} \frac{\partial h_{x}}{\partial x_{k}} & \frac{\partial h_{x}}{\partial y_{k}} & \frac{\partial h_{x}}{\partial θ_{k}} \\ \frac{\partial h_{y}}{\partial x_{k}} & \frac{\partial h_{y}}{\partial y_{k}} & \frac{\partial h_{y}}{\partial θ_{k}} \\ \frac{\partial h_{θ}}{\partial x_{k}} & \frac{\partial h_{θ}}{\partial y_{k}} & \frac{\partial h_{θ}}{\partial θ_{k}} \end{matrix}] = [\begin{matrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{matrix}]

(19)

The measurement update equations are given by (Evgeni Kiriy & Martin Buehler, 2002):

K_{k} = P_{k}^{-} H_{k}^{T} (H_{k} P_{k}^{-} H_{k}^{T} + R_{k})^{- 1}

(20)

{\hat{x}}_{k} = {\hat{x}}_{k}^{-} + K_{k} (z_{k} - h ({\hat{x}}_{k}^{-}, 0))

(21)

P_{k} = (I - K_{k} H_{k}) P_{k}^{-}

(22)

With these set of equations all the necessary components of EKF have been defined.

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 start position to goal position. The specification of the control input and output variables have been set. These inputs are: angle θ_error which is the difference between the goal angle θ_g and the current heading θ_r of robot, the distance d_error which is the difference between the current position and the goal position. The calculation is a simple computation of the basic equation between two points. The outputs of Fuzzy Logic Controller are velocities of the left and right motors of the mobile robot. Fig. 8 shows the proposed Fuzzy Logic Controller block diagram.

Fig. 8.

Schematic diagram for the proposed fuzzy logic controller

Fuzzy logic technique has been used to generate the velocities for each motor of the two wheel mobile robot. The fuzzy logic algorithm is as follows: Step 1:

Define the linguistic variable(s) for input and output system

Compute two input variables: angle error (the different angle between goal angle and robot's heading) and distance error (the difference between the current position and goal position). Compute two output variables: the velocities of the left and right motors.

Step 2:

Define fuzzy set

The fuzzy set values of the fuzzy variables are divided as shown in the Table 2.

Table 2.

Notations for the Fuzzy Logic

Angle: θ_error	Distance: d_error
NL: Negative Large	Z: Zero
NM: Negative Middle	N: Near
NS: Negative Small	M: Middle
ZE: Zero	F: Far
PS: Positive Small
PM: Positive Middle
PL: Positive Large

The fuzzy set values are set of overlapping values represented by triangular shape that is called the fuzzy membership function. Typical representations of the fuzzy membership function are shown in Fig. 9, Fig. 10 and Fig. 11 below.

Fig. 9.

Representation of the fuzzy membership functions of angle error

Fig. 10.

Representation of the fuzzy membership functions of velocity output

Fig. 11.

Representation of the fuzzy membership functions of distance error

Step 3:

Define fuzzy rules

The operation of the system utilizes the fuzzy rules. The notation for making fuzzy logic rules is: Fast Forward (FSFW), Forward (FW), Slow Forward (SLFW), Stop (S), Slow Backward (SLBW), Backward (BW) and Fast Backward (FSBW). The fuzzy rules are shown in the Table 3. and 4. below

Table 3.

Fuzzy rule of the right motor

	Z	N	M	F
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
PM	S	S	SLFW	SLFW
PL	S	S	FW	FW

Table 4.

A typical fuzzy rule of the left motor

	Z	N	M	F
NM	S	S	BW	BW
NM	S	S	SLBW	SLBW
NS	S	S	SLBW	SLBW
ZE	S	SLBW	BW	FSBW
PS	S	SLBW	BW	BW
PM	S	BW	BW	BW
PL	S	FSBW	FSBW	FSBW

Examples of fuzzy rule employed by the path following behavior are shown below:

ℜ₁ : IF θ_error is zero angle (ZE) and d_error is distance near (N) THEN the left motor is Slow Backward (SLBW)

ℜ₂ : IF θerror is zero angle (ZE) and d_error is distance near (N) THEN the right motor is Slow Forward (SLFW)

Step 4:

Defuzzification

The output action given by the input conditions, the defuzzification computes the velocity output of the fuzzy controller.

Centroid defuzzification or Center of Area method calculated by equation (23) as:

v = \frac{\sum_{i = 1}^{n} v_{i} \times min (μ_{i}^{θ_{e r r o r}}, μ_{i}^{d_{e r r o r}})}{\sum_{i = 1}^{n} min (μ_{i}^{θ_{e r r o r}}, μ_{i}^{d_{e r r o r}})}

(23)

where v is defined the fuzzy output of i^th rule, μ^θ_i ^error and μ_i^derror are the membership value of the input fuzzy set

4.4 Obstacle Avoidance

4.4.1 Detecting an obstacle

In this research, stereo vision technique has been used for obstacle detection. The flowchart for stereo vision algorithm is shown in Fig. 12. We capture the left and right images at the same time using stereo vision camera. For noise reduction, we used low-pass filter for reducing the noise in the images. Edge detection technique is then used to find edges of each images. In the next step, we find the matched corresponding points between the left and right images. Triangulation method is then applied to calculate the depth for the matched corresponding point. Various tests have been done using the above stereo vision system, We found that an accurate range for obstacle detection is between 0.5 to 6.0 meters.

Fig. 12.

Typical flowchart of a stereo vision system

The algorithm for obstacle detection is as follows:

If d_obs ≥ 0.5 and d_obs ≤ 6.0 then

found obstacle(s)

return (x_obs, y_obs, z_obs)

else

not found obstacle(s)

return (0,0,0)

end

where d_obs is the distance between the robot and the obstacle.

4.4.2 Avoiding an obstacle

To avoid the existing obstacles, a series of movement commands must be generated to allow the mobile robot to move pass the obstacles. In order to do so, the program within the mobile robot must analyze the incoming coordinates from 4.4.1. Additional data such as the obstacle angle θ_obs is calculated which eventually lead to the calculation avoidance angle θ_avoid for the mobile robot to move through the detected obstacles.

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} - θ_{o b s}

(24)

Avoiding angle, θ_avoid is then calculated as:

if θ_d < 0 then

θ_avoid = θ_g – θ_obs

else

θ_avoid = θ_g + θ_obs

end

The equations for inputs to the Fuzzy Logic control are as follow:

θ_{e r r o r} = θ_{r} - θ_{a v o i d}

(25)

d_{e r r o r} = \sqrt{(x_{g} - x_{r})^{2} + (y_{g} - y_{r})^{2}}

(26)

where θ_error is error angle for input fuzzy logic, d_error is error distance for inputs to fuzzy logic, θ_obs is an obstacle angle, θ_avoid is avoiding angle, θ_g is goal angle, θ_r is the heading of the mobile robot, (x_g, y_g) is a destination position, and (x_r, y_r) is a current position of the robot.

To improve the precision in the motion control of the robot motion, Extended Kalman Filter, the Fuzzy Logic control and the obstacle avoidance techniques have been implemented. We present the results from the experiments in section 6 to validate the above algorithms.

5. Hardware and software implementation

To test the proposed algorithms, a mobile robot which corresponds to the schematic design has been constructed and is shown in Fig. 4 and Fig 14a. The maximum speed 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. 14a. The interfacing circuit in Fig. 14b also receives signals from the vector compass and the position sensor. In both cases, the DALLAS 89C420 microcontroller has been used. The software interface for the DALLAS 89C420 microcontrollers and sensor module has written in assembly language (MCS-51 family instruction). Software interface for the stereo camera module is written in Microsoft Visual C++ (using vision library from the Point Grey Company). The simulation program is written using MATLAB.

Fig. 14a.

An experimental mobile robot

Fig. 14b.

Interface circuit for the mobile robot controller

6. Experimental results

The mobile robot with the proposed algorithm has been tested in various unknown environments with obstacles. In all experiments, the robot moved from the start positions to the goal positions correctly and the rest of this section provides results form the simulation experirments performed using the above control algrothm

6.1 Simulation Results

The simulations were performed using different maps consisting goal and obstacles.

Fig. 15a to show the trajectory followed by the robot avoiding an obstacle on the left hand side of the robot. The simulation results are shown in Fig. 15b, Fig. 15c and Fig. 15d respectively. The “–•–” represents the true position/orientation of the robot motion, the “–Δ–” represents trajectory after the position/orientation error are compensated using the fuzzy logic technique and the “–o–” represents the trajectory after position/orientation errors are compensated by the Kalman Filter and Fuzzy Logic techniques. Fig. 16 and Fig. 17 show similar experiments using other maps. Second simulation experiment (Fig. 16a) has three obstacles in the workspace. The robot moves from start position (30,200) to destination position (350,220). The results changes of xy-position, y-position and heading of robot are show in Fig. 16b, Fig. 16c and Fig. 16d respectively.

Fig. 15a.

The trajectory of the robot movement

Fig. 15b.

The comparison of robot motion in xy-axis using only FLC and FLC & EKF

Fig. 15c.

The comparison of robot motion in y-axis using FLC and FLC & EKF

Fig. 15d.

The comparison of robot heading using FLC and FLC & EKF

Fig. 16a.

The trajectory of the robot movement

Fig. 16b.

The comparison of robot motion in xy-axis using only FLC and FLC & EKF

Fig. 16c.

The comparison of robot motion in y-axis using FLC and FLC & EKF

Fig. 16d.

The comparison of robot heading using FLC and FLC & EKF

Fig. 17a.

The trajectory of the robot movement

Third simulation experiment, the robot moves from the start position (30,200) to destination position (350,260) by the workspace has seven obstacles (Fig. 17a). Fig. 17b, Fig. 17c and Fig. 17d show the results of xy-position, y-position and heading changes of robot respectively.

Fig. 17b.

The comparison of robot motion in xy-axis using only FLC and FLC & EKF

Fig. 17c.

The comparison of robot motion in y-axis using FLC and FLC & EKF

Fig. 17d.

The comparison of robot heading using FLC and FLC & EKF

The quantitative results of the simulation experiments are shown n the above Table 5. From the table we can conclude that, the robot can move from start position to goal position with higher accuracy when FLC and EKF is used together improving the performance of robot motion.

Table 5.

The sample results from the simulation experiments

Simulation	Sampling time (sec)	Action	FLC (true position)		FLC (error position)		FLC & EKF (error position)		Percentage Error
			FLC (true position)		FLC (error position)		FLC & EKF (error position)		FLC		FLC & EKF
			X (mm)	Y (mm)	X (mm)	Y (mm)	X (mm)	Y (mm)	X (%)	Y (%)	X (%)	Y (%)
1	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
	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
2	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
	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
3	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
	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

6.2 Experiments

In this section, the results from two experiments performed in unknown environments such as a corridor (Fig. 18) and a non-linear type passage way (Fig. 19) are presented. Fig. 18 shows a series of snap-shots of the mobile robot passing through an unknown corridor, starts from (a), then proceeds through (b), (c), (d), (e) and finishes at (f).

Fig. 18.

A typical robot motion passing through an unknown corridor

Fig. 19.

A typical of robot motion passing through an unknown non-linear type passage way

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), (c), (d), (e), (f), (g), (h) and finishes. The ‘Rerngwut I’ robot which has a maximum speed of 0.45 meters per second can move from the starting position, avoid the obstacle, pass through the corridor and reach the goal position successfully in both cases.

7. Conclusion

This research presents a combination of the Extended Kalman Filter, the Fuzzy Logic Control and the Obstacle avoidance technique which fuses information from different sensors. The proposed system consists of three parts and each of them has it's own function, the Fuzzy Logic technique for the position control, the Extended Kalman Filter technique is for the position predition and correction and the stereo vision technique for the obstacle avoidance. The stereo vision sensor supplies the obstacle detection. The simulations and experiments of robot motion show the results for robot's in which the robot can move from start position to goal position avoiding obstacles in an unknown environment. The proposed algorithm improves the accuracies of the position and the orientation of the robot motion problem. In future work, we propose to apply this algorithm to other platform such as “omni-direction mobile robot” (non-steering wheel type) because it may improve the position and orientation control for achieving a smooth robot motion.

References

Suppes

Alexander

Suhling

Frank

& Hotter

Michael

(2001). Robust Obstacle Detection from Stereoscopic Image Sequences Using Kalman Filter, pp. 385–391, Pattern Recognition 23^rd DAGM Symposium, September 2001, Munich, Germany

Aboshosha

Ashraf

(2003). Adaptation of Rescue Robot Behaviour in Unknown Terrains Based on Stochastic and Fuzzy Logic Approaches, http://www-ra.informatik.uni-tuebingen.de, Accessed: 2003-09-14

Georgiev

Atanas

& Allen

Peter K.

(2004). Localization Methods for a Mobile Robot in Urban Environments IEEE, Transactions on robotics, Vol. 20. No. 5, October 2004, pp 851–864

Longo

Domenico

Muscato

Giovanni

& Sacco

Vincenzo

(2002). Localization using fuzzy and Kalman filtering data fusion, http://www.robovolc.dees.unict.it/download/files/Clawar2002.pdf, Accessed: 2003-09-12

Fabrizi

Oriolo

Panzieri

& Ulivi

(2000). Mobile robot localization via fusion of ultrasonic and inertial sensor data, http://panzieri.dia.uniroma3.it/Articoli/ISORA2000.pdf, Accessed: 2003-09-12

Kiriy

Evgeni

& Buehler

Martin

(2002). Three-state Extended Kalman Filter for Mobile Robot Localization, http://www.cim.mcgill.ca/∼kiriy/publications/eKf-3state.pdf, Accessed: 2003-05-03

Everett

H.R.

(1995). Sensor for mobile robot theory and application, A K Peter Publisher, ISBN 1-56881-048-2, Wellesley, Massachusetts

Borenstein

& Koren

(1998). Real-time Obstacle Avoidance for Fast Mobile Robots, IEEE Transactions on Systems, Man, and Cybernetics, Vol. 19, No. 5, Sept./Oct. 1989, pp. 1179–1187

Borenstein

Johann

& Korem

Yoram

(1995). Motion Control analysis of a mobile robot, Transaction of ASME, Journal of Dynamics, Measurement and Control, Vol. 109, No. 2, pp. 73–79

10.

Borenstein

Johann

(1998). Experimental Results from Internal Odometry Error Correction with the OmniMate Mobile Robot, IEEE Transactions on Robotics an Automation, Vol. 14, No. 6, December 1998, pp 963–969

11.

Yen

John

& Langari

Reza

(1999), Fuzzy Logic, Prentice-Hall Inc., ISBN 0-13-525817-0, Simon & Schuster / A Viacom Company, Upper Saddle River, New Jersey 0748

12.

Wang

Jung-Hua

& Hslao

Chih-Ping

(1999). On Disparity Matching in Stereo Vision via a Neural Network Framework, Proc. Natl. Sci. Counc. ROC(A), Vol. 23, No. 5, 1999, pp. 665–678

13.

Castleman

Kenneth R.

(1996). Digital Image Processing, Prentice Hall, ISBN 0-13-211467-4, Upper Saddle River, New Jersey 0748

14.

Passino

Kevin M.

& Yurkovich

Stephen

(1998). Fuzzy Control, Addison Wesley Longman Inc., ISBN 0-201-18074-X

15.

Gaëtan

Marti

(1997). Stereo Vision mobile robotics, Micro Engineering Laboratory, Swiss Federal Institute of Technology lansaunne, http://imtsg7.epfl.ch/papers/LUCERNE97-GM.doc, Accessed: 2002-20-11

16.

Sobh

T. M.

Dekhil

Efros

A.A.

& Mihali

(2001). Logic Control For Mobile Robot, International Journal of Robotics and Automation, Vol. 16, No. 2, pp. 74–82

17.

Fukuda

Toshio

Fellow & Kubota

Naoyuki

(1999). An Intelligent Robotic System Based on a Fuzzy Approach, Proceedings of the IEEE, Vol. 87 No. 9, September 1999, pp. 1448–1470

18.

Wijesoma

W. S.

Khaw

P. P.

& Teoh

E.K.

(2001). Sensor Modelling and Fusion For Fuzzy Navigation of an AGV, International Journal of Robotics and Automation, Vol. 16, No. 2, pp. 14–24